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

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