X-Git-Url: http://pileus.org/git/?p=~andy%2FiBeaconNav;a=blobdiff_plain;f=src%2Fedu%2Fucla%2FiBeaconNav%2FQuat.java;h=f919f6672029a506f88aec04d72484ac702ed9a6;hp=7483f8dfda48ccea4b0e9eb9368135d46a3de6b8;hb=c7406355a6be2ceddaa8ec5347d4c62a2da778d3;hpb=3709bedfc90a690696eb5fc0e64fd8e17a023ce6

diff --git a/src/edu/ucla/iBeaconNav/Quat.java b/src/edu/ucla/iBeaconNav/Quat.java
index 7483f8d..f919f66 100644
--- 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() {