blob: a8a4667742f9e6f00eb778d861adf717b6d6ab20 (
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));
}
}
}
|
