Integrate quaternions

[~andy/iBeaconNav] / src / edu / ucla / iBeaconNav / Quat.java

diff --git a/src/edu/ucla/iBeaconNav/Quat.java b/src/edu/ucla/iBeaconNav/Quat.java
index 7483f8dfda48ccea4b0e9eb9368135d46a3de6b8..f919f6672029a506f88aec04d72484ac702ed9a6 100644 (file)
--- a/src/edu/ucla/iBeaconNav/Quat.java
+++ b/src/edu/ucla/iBeaconNav/Quat.java
@@ -1,9 +1,11 @@
+package edu.ucla.iBeaconNav;
+
 class Quat
 {
-       private double x = 0;
-       private double y = 0;
-       private double z = 0;
-       private double w = 1;
+       public double x = 0;
+       public double y = 0;
+       public double z = 0;
+       public double w = 1;
 
        public Quat()
        {
@@ -11,21 +13,20 @@ class Quat
 
        public Quat(double roll, double pitch, double yaw)
        {
-               double euler[] = { roll, pitch, yaw };
-               this.set(euler);
+               this.set(new Vect(roll, pitch, yaw));
        }
 
-       public void set(double euler[])
+       public void set(Vect rpy)
        {
                // Calculate sin/cos of roll/pitch/yaw
-               double sr = Math.sin(euler[0] * 0.5f);
-               double cr = Math.cos(euler[0] * 0.5f);
+               double sr = Math.sin(rpy.x * 0.5f);
+               double cr = Math.cos(rpy.x * 0.5f);
 
-               double sp = Math.sin(euler[1] * 0.5f);
-               double cp = Math.cos(euler[1] * 0.5f);
+               double sp = Math.sin(rpy.y * 0.5f);
+               double cp = Math.cos(rpy.y * 0.5f);
 
-               double sy = Math.sin(euler[2] * 0.5f);
-               double cy = Math.cos(euler[2] * 0.5f);
+               double sy = Math.sin(rpy.z * 0.5f);
+               double cy = Math.cos(rpy.z * 0.5f);
 
                // Calculate quaternion
                double tx = (cr * cp * sy) + (sr * sp * cy);
@@ -42,25 +43,25 @@ class Quat
                this.w = tw / dist;
        }
 
-       public void get(double[] euler)
+       public void get(Vect rpy)
        {
                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;
+                       rpy.x = 2 * Math.atan2(x, w);
+                       rpy.y = Math.PI / 2;
+                       rpy.z = 0;
                } else if (test < -0.499) {
                        // south pole
-                       euler[0] = -2 * Math.atan2(x, w);
-                       euler[1] = Math.PI / 2;
-                       euler[2] = 0;
+                       rpy.x = -2 * Math.atan2(x, w);
+                       rpy.y = Math.PI / 2;
+                       rpy.z = 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));
+                       rpy.x = Math.atan2(2*y*w - 2*x*z, 1 - 2*(y*y) - 2*(z*z));
+                       rpy.y = Math.asin(2*x*y + 2*z*w);
+                       rpy.z = Math.atan2(2*x*w - 2*y*z, 1 - 2*(x*x) - 2*(z*z));
                }
        }
 
@@ -77,15 +78,24 @@ class Quat
                this.z = tz;
        }
 
+       public String toString()
+       {
+               Vect rpy = new Vect();
+               this.get(rpy);
+               return String.format("%7.2f %7.2f %7.2f\n",
+                               rpy.x*180/Math.PI,
+                               rpy.y*180/Math.PI,
+                               rpy.z*180/Math.PI);
+       }
+
        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);
+               System.out.format("%-8s rpy - %s", label+":", this);
+       }
+
+       public void debug(String label)
+       {
+               Util.debug(String.format("%-8s rpy - %s", label+":", this));
        }
 
        public static void test() {