1 package org.lcsim.recon.tracking.vsegment.transform;
2
3 import hep.physics.matrix.MatrixOp;
4 import hep.physics.matrix.SymmetricMatrix;
5 import hep.physics.vec.BasicHep3Matrix;
6 import hep.physics.vec.Hep3Matrix;
7 import hep.physics.vec.Hep3Vector;
8 import hep.physics.vec.VecOp;
9
10 import static org.lcsim.recon.tracking.vsegment.transform.Axis.*;
11
12
13
14
15
16
17
18 public class Rotation3D implements Transformation3D {
19
20
21
22
23 public Rotation3D() {
24 _from = new BasicHep3Matrix(1., 0., 0.,
25 0., 1., 0.,
26 0., 0., 1);
27 }
28
29
30
31
32
33
34 public Rotation3D(double e11, double e12, double e13,
35 double e21, double e22, double e23,
36 double e31, double e32, double e33) {
37 _from = new BasicHep3Matrix(e11, e12, e13,
38 e21, e22, e23,
39 e31, e32, e33);
40 }
41
42
43
44
45
46
47 public Rotation3D(Hep3Matrix matrix) {
48 _from = matrix;
49 }
50
51
52
53
54 public Rotation3D(Axis axis, double angle) {
55 double sin = Math.sin(angle);
56 double cos = Math.cos(angle);
57 BasicHep3Matrix active = new BasicHep3Matrix();
58 active.setElement(axis.index(), axis.index(), 1.);
59 active.setElement((axis.index()+1)%3, (axis.index()+1)%3, cos);
60 active.setElement((axis.index()+1)%3, (axis.index()+2)%3, -sin);
61 active.setElement((axis.index()+2)%3, (axis.index()+1)%3, sin);
62 active.setElement((axis.index()+2)%3, (axis.index()+2)%3, cos);
63 _from = active;
64 }
65
66
67
68
69
70
71
72 public Hep3Vector transformTo(Hep3Vector vector) {
73 if (_to == null) _to = VecOp.inverse(_from);
74 return VecOp.mult(_to, vector);
75 }
76
77
78
79
80
81 public Hep3Vector transformFrom(Hep3Vector vector) {
82 if (_from == null) _from = VecOp.inverse(_to);
83 return VecOp.mult(_from, vector);
84 }
85
86
87
88
89
90 public SymmetricMatrix transformTo(SymmetricMatrix covMatrix) {
91 if (_to == null) _to = VecOp.inverse(_from);
92 double[] loc = new double[6];
93 int index = -1;
94 for (int l = 0; l <3; l++) {
95 for (int k = 0; k <= l; k++) {
96 loc[++index] = 0.;
97 for (int i = 0; i<3; i++) {
98 for (int j = 0; j<= i; j++) {
99 double term = _to.e(l,i)*_to.e(k,j)*covMatrix.e(i,j);
100 loc[index] += (i==j) ? term : 2.*term;
101 }
102 }
103 }
104 }
105 return new SymmetricMatrix(3, loc, true);
106 }
107
108
109
110
111
112 public SymmetricMatrix transformFrom(SymmetricMatrix covMatrix) {
113 if (_from == null) _from = VecOp.inverse(_to);
114 double[] glob = new double[6];
115 int index = -1;
116 for (int l = 0; l <3; l++) {
117 for (int k = 0; k <= l; k++) {
118 glob[++index] = 0.;
119 for (int i = 0; i<3; i++) {
120 for (int j = 0; j<= i; j++) {
121 double term = _from.e(l,i)*_from.e(k,j)*covMatrix.e(i,j);
122 glob[index] += (i==j) ? term : 2.*term;
123 }
124 }
125 }
126 }
127 return new SymmetricMatrix(3, glob, true);
128 }
129
130
131
132
133
134 public SymmetricMatrix transformTo(SymmetricMatrix covMatrix, Hep3Vector vector) {
135 return transformTo(covMatrix);
136 }
137
138
139
140
141
142 public SymmetricMatrix transformFrom(SymmetricMatrix covMatrix, Hep3Vector vector) {
143 return transformFrom(covMatrix);
144 }
145
146
147
148 Hep3Matrix _to;
149 Hep3Matrix _from;
150 }