Pileus Git - ~andy/iBeaconNav/blob - src/edu/ucla/iBeaconNav/Quat.java

   1 package edu.ucla.iBeaconNav;
   2
   3 class Quat
   4 {
   5         public double x = 0;
   6         public double y = 0;
   7         public double z = 0;
   8         public double w = 1;
   9
  10         public Quat()
  11         {
  12         }
  13
  14         public Quat(double roll, double pitch, double yaw)
  15         {
  16                 this.set(new Vect(roll, pitch, yaw));
  17         }
  18
  19         public void set(Vect rpy)
  20         {
  21                 // Calculate sin/cos of roll/pitch/yaw
  22                 double sr = Math.sin(rpy.x * 0.5f);
  23                 double cr = Math.cos(rpy.x * 0.5f);
  24
  25                 double sp = Math.sin(rpy.y * 0.5f);
  26                 double cp = Math.cos(rpy.y * 0.5f);
  27
  28                 double sy = Math.sin(rpy.z * 0.5f);
  29                 double cy = Math.cos(rpy.z * 0.5f);
  30
  31                 // Calculate quaternion
  32                 double tx = (cr * cp * sy) + (sr * sp * cy);
  33                 double ty = (sr * cp * cy) + (cr * sp * sy);
  34                 double tz = (cr * sp * cy) - (sr * cp * sy);
  35                 double tw = (cr * cp * cy) - (sr * sp * sy);
  36
  37                 // Normalize output
  38                 double dist = Math.sqrt(tx*tx + ty*ty + tz*tz + tw*tw);
  39
  40                 this.x = tx / dist;
  41                 this.y = ty / dist;
  42                 this.z = tz / dist;
  43                 this.w = tw / dist;
  44         }
  45
  46         public void get(Vect rpy)
  47         {
  48                 double test = x*y + z*w;
  49
  50                 if (test > 0.499) {
  51                         // north pole
  52                         rpy.x = 2 * Math.atan2(x, w);
  53                         rpy.y = Math.PI / 2;
  54                         rpy.z = 0;
  55                 } else if (test < -0.499) {
  56                         // south pole
  57                         rpy.x = -2 * Math.atan2(x, w);
  58                         rpy.y = Math.PI / 2;
  59                         rpy.z = 0;
  60                 } else {
  61                         // normal
  62                         rpy.x = Math.atan2(2*y*w - 2*x*z, 1 - 2*(y*y) - 2*(z*z));
  63                         rpy.y = Math.asin(2*x*y + 2*z*w);
  64                         rpy.z = Math.atan2(2*x*w - 2*y*z, 1 - 2*(x*x) - 2*(z*z));
  65                 }
  66         }
  67
  68         public void mul(Quat q)
  69         {
  70                 double tw = (w * q.w) - (x * q.x) - (y * q.y) - (z * q.z);
  71                 double tx = (w * q.x) + (x * q.w) + (y * q.z) - (z * q.y);
  72                 double ty = (w * q.y) - (x * q.z) + (y * q.w) + (z * q.x);
  73                 double tz = (w * q.z) + (x * q.y) - (y * q.x) + (z * q.w);
  74
  75                 this.w = tw;
  76                 this.x = tx;
  77                 this.y = ty;
  78                 this.z = tz;
  79         }
  80
  81         public String toString()
  82         {
  83                 Vect rpy = new Vect();
  84                 this.get(rpy);
  85                 return String.format("%7.2f %7.2f %7.2f\n",
  86                                 rpy.x*180/Math.PI,
  87                                 rpy.y*180/Math.PI,
  88                                 rpy.z*180/Math.PI);
  89         }
  90
  91         public void print(String label)
  92         {
  93                 System.out.format("%-8s rpy - %s", label+":", this);
  94         }
  95
  96         public void debug(String label)
  97         {
  98                 Util.debug(String.format("%-8s rpy - %s", label+":", this));
  99         }
 100
 101         public static void test() {
 102                 Quat id  = new Quat(0, 0, 0);
 103                 Quat r90 = new Quat(Math.PI/2, 0, 0);
 104                 Quat p90 = new Quat(0, Math.PI/2, 0);
 105                 Quat y90 = new Quat(0, 0, Math.PI/2);
 106
 107                 Quat sum = new Quat();
 108
 109                 System.out.println("Init");
 110                 r90.print("r90");
 111                 p90.print("p90");
 112                 y90.print("y90");
 113
 114                 System.out.println("Roll:");
 115                 sum.mul(r90); sum.print("sum");
 116                 sum.mul(r90); sum.print("sum");
 117                 sum.mul(r90); sum.print("sum");
 118                 sum.mul(r90); sum.print("sum");
 119
 120                 System.out.println("Pitch:");
 121                 sum.mul(p90); sum.print("sum");
 122                 sum.mul(p90); sum.print("sum");
 123                 sum.mul(p90); sum.print("sum");
 124                 sum.mul(p90); sum.print("sum");
 125
 126                 System.out.println("Yaw:");
 127                 sum.mul(y90); sum.print("sum");
 128                 sum.mul(y90); sum.print("sum");
 129                 sum.mul(y90); sum.print("sum");
 130                 sum.mul(y90); sum.print("sum");
 131
 132                 System.out.println("Test:");
 133                 sum.mul(r90); sum.print("sum");
 134                 sum.mul(p90); sum.print("sum");
 135                 sum.mul(y90); sum.print("sum");
 136                 sum.mul(p90); sum.print("sum");
 137                 sum.mul(y90); sum.print("sum");
 138                 sum.mul(y90); sum.print("sum");
 139         }
 140 }