package org.madore.ephem; public final class Frames { private static double[] unboxDoubleArray(Double[] c) { double[] unboxed = new double[c.length]; for ( int i=0 ; i<unboxed.length ; i++ ) unboxed[i] = c[i]; return unboxed; } public static final class Vector { final double[] v; // v.length == 3 public Vector(double... v) { if ( v.length != 3 ) throw new IllegalArgumentException("Vector constructor expects 3 coordinates"); this.v = v; } public Vector(Double[] v) { this(unboxDoubleArray(v)); } public double dotprod(Vector w) { double res = 0; for ( int i=0 ; i<3 ; i++ ) res += v[i]*w.v[i]; return res; } public double sqnorm() { double res = 0; for ( int i=0 ; i<3 ; i++ ) res += v[i]*v[i]; return res; } } 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 Rotation(Double[] q) { this(unboxDoubleArray(q)); } public Vector apply(double[] v) { 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.v); } 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 Rotation rotx(double theta) { return new Rotation(Math.cos(theta/2), Math.sin(theta/2), 0, 0); } public static Rotation roty(double theta) { return new Rotation(Math.cos(theta/2), 0, Math.sin(theta/2), 0); } public static Rotation rotz(double theta) { return new Rotation(Math.cos(theta/2), 0, 0, Math.sin(theta/2)); } } }