Use quaternions instead of 3*3 matrices to represent rotations.

3 files changed, 46 insertions, 54 deletions

diff --git a/org/madore/ephem/Frames.java b/org/madore/ephem/Frames.java
index e85412c..a8a4667 100644
--- a/org/madore/ephem/Frames.java
+++ b/org/madore/ephem/Frames.java
@@ -10,75 +10,67 @@ public final class Frames {
     }
 
     public static final class Vector {
-	double[] crd;
-	public Vector(double... crd) {
-	    if ( crd.length != 3 )
+	final double[] v;  // v.length == 3
+	public Vector(double... v) {
+	    if ( v.length != 3 )
 		throw new IllegalArgumentException("Vector constructor expects 3 coordinates");
-	    this.crd = crd;
+	    this.v = v;
 	}
-	public Vector(Double... crd) {
-	    this(unboxDoubleArray(crd));
+	public Vector(Double... v) {
+	    this(unboxDoubleArray(v));
 	}
-	public double dotprod(Vector v) {
+	public double dotprod(Vector w) {
 	    double res = 0;
 	    for ( int i=0 ; i<3 ; i++ )
-		res += crd[i]*v.crd[i];
+		res += v[i]*w.v[i];
 	    return res;
 	}
 	public double sqnorm() {
 	    double res = 0;
 	    for ( int i=0 ; i<3 ; i++ )
-		res += crd[i]*crd[i];
+		res += v[i]*v[i];
 	    return res;
 	}
     }
 
-    public static final class RotationMatrix {
-	double[][] mat;
-	public RotationMatrix(double[][] mat) {
-	    if ( mat.length != 3 )
-		throw new IllegalArgumentException("RotationMatrix constructor expects 3 lines");
-	    for ( int i=0 ; i<3 ; i++ )
-		if ( mat[i].length != 3 )
-		    throw new IllegalArgumentException("RotationMatrix constructor expects 3 columns");
-	    this.mat = mat;
-	}
-	private static double[][] matOfVectors(Vector[] vecs) {
-	    double[][] mat = new double[vecs.length][];
-	    for ( int i=0 ; i<mat.length ; i++ )
-		mat[i] = vecs[i].crd;
-	    return mat;
+    public static final class Rotation {
+	final double[] q;  // q.length == 4
+	public Rotation(double... q) {
+	    if ( q.length != 4 )
+		throw new IllegalArgumentException("Rotation constructor expects 4 coordinates");
+	    this.q = q;
 	}
-	public RotationMatrix(Vector... vecs) {
-	    this(matOfVectors(vecs));
+	public Rotation(Double... q) {
+	    this(unboxDoubleArray(q));
 	}
 	public Vector apply(double[] v) {
-	    double out[] = new double[3];
-	    for ( int i=0 ; i<3 ; i++ )
-		for ( int j=0 ; j<3 ; j++ )
-		    out[i] += v[j]*mat[j][i];
+	    if ( v.length != 3 )
+		throw new IllegalArgumentException("Rotation.apply() expects 3 coordinates");
+	    double[] out = new double[3];
+	    out[0] = (q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3])*v[0] + (2*q[1]*q[2] + 2*q[0]*q[3])*v[1] + (-2*q[0]*q[2] + 2*q[1]*q[3])*v[2];
+	    out[1] = (2*q[1]*q[2] - 2*q[0]*q[3])*v[0] + (q[0]*q[0] - q[1]*q[1] + q[2]*q[2] - q[3]*q[3])*v[1] + (2*q[0]*q[1] + 2*q[2]*q[3])*v[2];
+	    out[2] = (2*q[0]*q[2] + 2*q[1]*q[3])*v[0] + (-2*q[0]*q[1] + 2*q[2]*q[3])*v[1] + (q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3])*v[2];
 	    return new Vector(out);
 	}
 	public Vector apply(Vector v) {
-	    return apply(v.crd);
+	    return apply(v.v);
 	}
-	public RotationMatrix apply(RotationMatrix mv) {
-	    return new RotationMatrix(this.apply(mv.mat[0]), this.apply(mv.mat[1]), this.apply(mv.mat[2]));
+	public Rotation apply(Rotation h) {
+	    double[] outq = new double[4];
+	    outq[0] = q[0]*h.q[0] - q[1]*h.q[1] - q[2]*h.q[2] - q[3]*h.q[3];
+	    outq[1] = q[1]*h.q[0] + q[0]*h.q[1] + q[3]*h.q[2] - q[2]*h.q[3];
+	    outq[2] = q[2]*h.q[0] - q[3]*h.q[1] + q[0]*h.q[2] + q[1]*h.q[3];
+	    outq[3] = q[3]*h.q[0] + q[2]*h.q[1] - q[1]*h.q[2] + q[0]*h.q[3];
+	    return new Rotation(outq);
 	}
-	public static RotationMatrix rotx(double theta) {
-	    return new RotationMatrix(new Vector(1, 0, 0),
-				      new Vector(0, Math.cos(theta), -Math.sin(theta)),
-				      new Vector(0, Math.sin(theta), Math.cos(theta)));
+	public static Rotation rotx(double theta) {
+	    return new Rotation(Math.cos(theta/2), Math.sin(theta/2), 0, 0);
 	}
-	public static RotationMatrix roty(double theta) {
-	    return new RotationMatrix(new Vector(Math.cos(theta), 0, Math.sin(theta)),
-				      new Vector(0, 1, 0),
-				      new Vector(-Math.sin(theta), 0, Math.cos(theta)));
+	public static Rotation roty(double theta) {
+	    return new Rotation(Math.cos(theta/2), 0, Math.sin(theta/2), 0);
 	}
-	public static RotationMatrix rotz(double theta) {
-	    return new RotationMatrix(new Vector(Math.cos(theta), -Math.sin(theta), 0),
-				      new Vector(Math.sin(theta), Math.cos(theta), 0),
-				      new Vector(0, 0, 1));
+	public static Rotation rotz(double theta) {
+	    return new Rotation(Math.cos(theta/2), 0, 0, Math.sin(theta/2));
 	}
     }
 
diff --git a/org/madore/ephem/Nutation.java b/org/madore/ephem/Nutation.java
index dcfdc30..bc6e721 100644
--- a/org/madore/ephem/Nutation.java
+++ b/org/madore/ephem/Nutation.java
@@ -63,10 +63,10 @@ public final class Nutation {
 	return data.get(v);
     }
 
-    public static Frames.RotationMatrix matrix(double t) {
-	Frames.RotationMatrix mat1 = Frames.RotationMatrix.rotx(Precession.epsilon.v(t)*Comput.arcsecond);
-	Frames.RotationMatrix mat2 = Frames.RotationMatrix.rotz(-getFunc(Variable.PSI).v(t)*Comput.arcsecond).apply(mat1);
-	Frames.RotationMatrix mat3 = Frames.RotationMatrix.rotx(-(Precession.epsilon.v(t)+getFunc(Variable.EPSILON).v(t))*Comput.arcsecond).apply(mat2);
+    public static Frames.Rotation matrix(double t) {
+	Frames.Rotation mat1 = Frames.Rotation.rotx(Precession.epsilon.v(t)*Comput.arcsecond);
+	Frames.Rotation mat2 = Frames.Rotation.rotz(-getFunc(Variable.PSI).v(t)*Comput.arcsecond).apply(mat1);
+	Frames.Rotation mat3 = Frames.Rotation.rotx(-(Precession.epsilon.v(t)+getFunc(Variable.EPSILON).v(t))*Comput.arcsecond).apply(mat2);
 	return mat3;
     }
 
diff --git a/org/madore/ephem/Precession.java b/org/madore/ephem/Precession.java
index 8b556d5..af8a1dc 100644
--- a/org/madore/ephem/Precession.java
+++ b/org/madore/ephem/Precession.java
@@ -15,11 +15,11 @@ public final class Precession {
     public static Comput.Polynomial eqorg
 	= new Comput.Polynomial(0.014506, 4612.156534, 1.3915817, -0.00000044, -0.000029956, -0.0000000368);
 
-    public static Frames.RotationMatrix matrix(double t) {
-	Frames.RotationMatrix mat1 = Frames.RotationMatrix.rotx(epsilon0*Comput.arcsecond);
-	Frames.RotationMatrix mat2 = Frames.RotationMatrix.rotz(-psi.v(t)*Comput.arcsecond).apply(mat1);
-	Frames.RotationMatrix mat3 = Frames.RotationMatrix.rotx(-omega.v(t)*Comput.arcsecond).apply(mat2);
-	Frames.RotationMatrix mat4 = Frames.RotationMatrix.rotz(chi.v(t)*Comput.arcsecond).apply(mat3);
+    public static Frames.Rotation matrix(double t) {
+	Frames.Rotation mat1 = Frames.Rotation.rotx(epsilon0*Comput.arcsecond);
+	Frames.Rotation mat2 = Frames.Rotation.rotz(-psi.v(t)*Comput.arcsecond).apply(mat1);
+	Frames.Rotation mat3 = Frames.Rotation.rotx(-omega.v(t)*Comput.arcsecond).apply(mat2);
+	Frames.Rotation mat4 = Frames.Rotation.rotz(chi.v(t)*Comput.arcsecond).apply(mat3);
 	return mat4;
     }