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
78
79
80
81
82
83
84
85
|
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 {
double[] crd;
public Vector(double... crd) {
if ( crd.length != 3 )
throw new IllegalArgumentException("Vector constructor expects 3 coordinates");
this.crd = crd;
}
public Vector(Double... crd) {
this(unboxDoubleArray(crd));
}
public double dotprod(Vector v) {
double res = 0;
for ( int i=0 ; i<3 ; i++ )
res += crd[i]*v.crd[i];
return res;
}
public double sqnorm() {
double res = 0;
for ( int i=0 ; i<3 ; i++ )
res += crd[i]*crd[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 RotationMatrix(Vector... vecs) {
this(matOfVectors(vecs));
}
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];
return new Vector(out);
}
public Vector apply(Vector v) {
return apply(v.crd);
}
public RotationMatrix apply(RotationMatrix mv) {
return new RotationMatrix(this.apply(mv.mat[0]), this.apply(mv.mat[1]), this.apply(mv.mat[2]));
}
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 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 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));
}
}
}
|