Add quaternion and vector code

diff --git a/src/edu/ucla/iBeaconNav/Quat.java b/src/edu/ucla/iBeaconNav/Quat.java
new file mode 100644 (file)
index 0000000..7483f8d
--- /dev/null
+++ b/src/edu/ucla/iBeaconNav/Quat.java
@@ -0,0 +1,130 @@
+class Quat
+{
+       private double x = 0;
+       private double y = 0;
+       private double z = 0;
+       private double w = 1;
+
+       public Quat()
+       {
+       }
+
+       public Quat(double roll, double pitch, double yaw)
+       {
+               double euler[] = { roll, pitch, yaw };
+               this.set(euler);
+       }
+
+       public void set(double euler[])
+       {
+               // Calculate sin/cos of roll/pitch/yaw
+               double sr = Math.sin(euler[0] * 0.5f);
+               double cr = Math.cos(euler[0] * 0.5f);
+
+               double sp = Math.sin(euler[1] * 0.5f);
+               double cp = Math.cos(euler[1] * 0.5f);
+
+               double sy = Math.sin(euler[2] * 0.5f);
+               double cy = Math.cos(euler[2] * 0.5f);
+
+               // Calculate quaternion
+               double tx = (cr * cp * sy) + (sr * sp * cy);
+               double ty = (sr * cp * cy) + (cr * sp * sy);
+               double tz = (cr * sp * cy) - (sr * cp * sy);
+               double tw = (cr * cp * cy) - (sr * sp * sy);
+
+               // Normalize output
+               double dist = Math.sqrt(tx*tx + ty*ty + tz*tz + tw*tw);
+
+               this.x = tx / dist;
+               this.y = ty / dist;
+               this.z = tz / dist;
+               this.w = tw / dist;
+       }
+
+       public void get(double[] euler)
+       {
+               double test = x*y + z*w;
+
+               if (test > 0.499) {
+                       // north pole
+                       euler[0] = 2 * Math.atan2(x, w);
+                       euler[1] = Math.PI / 2;
+                       euler[2] = 0;
+               } else if (test < -0.499) {
+                       // south pole
+                       euler[0] = -2 * Math.atan2(x, w);
+                       euler[1] = Math.PI / 2;
+                       euler[2] = 0;
+               } else {
+                       // normal
+                       euler[0] = Math.atan2(2*y*w - 2*x*z, 1 - 2*(y*y) - 2*(z*z));
+                       euler[1] = Math.asin(2*x*y + 2*z*w);
+                       euler[2] = Math.atan2(2*x*w - 2*y*z, 1 - 2*(x*x) - 2*(z*z));
+               }
+       }
+
+       public void mul(Quat q)
+       {
+               double tw = (w * q.w) - (x * q.x) - (y * q.y) - (z * q.z);
+               double tx = (w * q.x) + (x * q.w) + (y * q.z) - (z * q.y);
+               double ty = (w * q.y) - (x * q.z) + (y * q.w) + (z * q.x);
+               double tz = (w * q.z) + (x * q.y) - (y * q.x) + (z * q.w);
+
+               this.w = tw;
+               this.x = tx;
+               this.y = ty;
+               this.z = tz;
+       }
+
+       public void print(String label)
+       {
+               double euler[] = {0, 0, 0};
+               this.get(euler);
+               System.out.format("%8s rpy - %7.2f %7.2f %7.2f\n",
+                               label + ":",
+                               euler[0]*180/Math.PI,
+                               euler[1]*180/Math.PI,
+                               euler[2]*180/Math.PI);
+       }
+
+       public static void test() {
+               Quat id  = new Quat(0, 0, 0);
+               Quat r90 = new Quat(Math.PI/2, 0, 0);
+               Quat p90 = new Quat(0, Math.PI/2, 0);
+               Quat y90 = new Quat(0, 0, Math.PI/2);
+
+               Quat sum = new Quat();
+
+               System.out.println("Init");
+               r90.print("r90");
+               p90.print("p90");
+               y90.print("y90");
+
+               System.out.println("Roll:");
+               sum.mul(r90); sum.print("sum");
+               sum.mul(r90); sum.print("sum");
+               sum.mul(r90); sum.print("sum");
+               sum.mul(r90); sum.print("sum");
+
+               System.out.println("Pitch:");
+               sum.mul(p90); sum.print("sum");
+               sum.mul(p90); sum.print("sum");
+               sum.mul(p90); sum.print("sum");
+               sum.mul(p90); sum.print("sum");
+
+               System.out.println("Yaw:");
+               sum.mul(y90); sum.print("sum");
+               sum.mul(y90); sum.print("sum");
+               sum.mul(y90); sum.print("sum");
+               sum.mul(y90); sum.print("sum");
+
+               System.out.println("Test:");
+               sum.mul(r90); sum.print("sum");
+               sum.mul(p90); sum.print("sum");
+               sum.mul(y90); sum.print("sum");
+               sum.mul(p90); sum.print("sum");
+               sum.mul(y90); sum.print("sum");
+               sum.mul(y90); sum.print("sum");
+       }
+}

diff --git a/src/edu/ucla/iBeaconNav/Vect.java b/src/edu/ucla/iBeaconNav/Vect.java
new file mode 100644 (file)
index 0000000..1949da5
--- /dev/null
+++ b/src/edu/ucla/iBeaconNav/Vect.java
@@ -0,0 +1,56 @@
+package edu.ucla.iBeaconNav;
+
+class Vect
+{
+       public double x = 0;
+       public double y = 0;
+       public double z = 0;
+
+       /* Constructors */
+       public Vect()
+       {
+       }
+
+       public Vect(double x, double y, double z)
+       {
+               this.x = x;
+               this.y = y;
+               this.z = z;
+       }
+
+       /* Set functions */
+       public void set(double x, double y, double z)
+       {
+               this.x = x;
+               this.y = y;
+               this.z = z;
+       }
+
+       /* Add functions */
+       public void add(Vect v)
+       {
+               this.x += v.x;
+               this.y += v.y;
+               this.z += v.z;
+       }
+       public void add(double off)
+       {
+               this.x += off;
+               this.y += off;
+               this.z += off;
+       }
+
+       /* Multiply functions (scalar) */
+       public void mul(Vect v)
+       {
+               this.x *= v.x;
+               this.y *= v.y;
+               this.z *= v.z;
+       }
+       public void mul(double sf)
+       {
+               this.x *= sf;
+               this.y *= sf;
+               this.z *= sf;
+       }
+}