summaryrefslogtreecommitdiffstats
path: root/org/madore/ephem/Frames.java
blob: 3671b3c2a99e43734aaebd683257daadc2348d8a (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
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));
	}
    }

}