altoslib: Clean up quaternion and rotation interfaces

Export euler to quaternion (instead of half_euler).
Provide angles to rotate rather than rates and time.
Add comments to quaternion and rotation code.

Signed-off-by: Keith Packard <keithp@keithp.com>

1 files changed, 27 insertions, 16 deletions

diff --git a/altoslib/AltosQuaternion.java b/altoslib/AltosQuaternion.java
index 66b5f4f5..9217fc41 100644
--- a/altoslib/AltosQuaternion.java
+++ b/altoslib/AltosQuaternion.java
@@ -22,6 +22,7 @@ public class AltosQuaternion {
 	double	r;		/* real bit */
 	double	x, y, z;	/* imaginary bits */
 
+	/* Multiply by b */
 	public AltosQuaternion multiply(AltosQuaternion b) {
 		return new AltosQuaternion(
 			this.r * b.r - this.x * b.x - this.y * b.y - this.z * b.z,
@@ -31,35 +32,36 @@ public class AltosQuaternion {
 	}
 
 	public AltosQuaternion conjugate() {
-		return new AltosQuaternion(
-			this.r,
-			-this.x,
-			-this.y,
-			-this.z);
+		return new AltosQuaternion(this.r,
+					   -this.x,
+					   -this.y,
+					   -this.z);
 	}
 
 	public double normal() {
-		return (this.r * this.r +
-			this.x * this.x +
-			this.y * this.y +
-			this.z * this.z);
+		return Math.sqrt(this.r * this.r +
+				 this.x * this.x +
+				 this.y * this.y +
+				 this.z * this.z);
 	}
 
+	/* Scale by a real value */
 	public AltosQuaternion scale(double b) {
-		return new AltosQuaternion(
-			this.r * b,
-			this.x * b,
-			this.y * b,
-			this.z * b);
+		return new AltosQuaternion(this.r * b,
+					   this.x * b,
+					   this.y * b,
+					   this.z * b);
 	}
 
+	/* Divide by the length to end up with a quaternion of length 1 */
 	public AltosQuaternion normalize() {
 		double	n = normal();
 		if (n <= 0)
 			return this;
-		return scale(1/Math.sqrt(n));
+		return scale(1/n);
 	}
 
+	/* dot product */
 	public double dot(AltosQuaternion b) {
 		return (this.r * b.r +
 			this.x * b.x +
@@ -67,10 +69,14 @@ public class AltosQuaternion {
 			this.z * b.z);
 	}
 
+	/* Rotate 'this' by 'b' */
 	public AltosQuaternion rotate(AltosQuaternion b) {
 		return (b.multiply(this)).multiply(b.conjugate());
 	}
 
+	/* Given two vectors (this and b), compute a quaternion
+	 * representing the rotation between them
+	 */
 	public AltosQuaternion vectors_to_rotation(AltosQuaternion b) {
 		/*
 		 * The cross product will point orthogonally to the two
@@ -145,7 +151,12 @@ public class AltosQuaternion {
 		return new AltosQuaternion(1, 0, 0, 0);
 	}
 
-	static public AltosQuaternion half_euler(double x, double y, double z) {
+	static public AltosQuaternion euler(double x, double y, double z) {
+
+		x = x / 2.0;
+		y = y / 2.0;
+		z = z / 2.0;
+
 		double	s_x = Math.sin(x), c_x = Math.cos(x);
 		double	s_y = Math.sin(y), c_y = Math.cos(y);
 		double	s_z = Math.sin(z), c_z = Math.cos(z);;