altoslib: Clean up quaternion and rotation interfaces

[fw/altos] / altoslib / AltosQuaternion.java

diff --git a/altoslib/AltosQuaternion.java b/altoslib/AltosQuaternion.java
index 66b5f4f55d6a706030d812cdf1273abc927c452c..9217fc4159337c1af57a6a29f483b993fd468b2e 100644 (file)
--- 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);;