Skip to content

Commit e89efc2

Browse files
baltzellbaltzell
committed
rich cleanup
1 parent 8a38552 commit e89efc2

17 files changed

Lines changed: 2573 additions & 3765 deletions

‎reconstruction/rich/src/main/java/org/jlab/rec/rich/Quaternion.java‎

Lines changed: 143 additions & 156 deletions
Original file line numberDiff line numberDiff line change
@@ -5,107 +5,105 @@
55
import org.jlab.geom.prim.Vector3D;
66

77
//Class created by gangel
8-
//for the JLAB package
8+
//for the JLAB package
99
//
10-
// The quaternion here is defined as:
10+
// The quaternion here is defined as:
1111
//
1212
public class Quaternion {
13-
//Definition of the components for the Quaternion
14-
public double x;
15-
public double y;
16-
public double z;
17-
public double w;
18-
19-
public Quaternion() {
20-
}
21-
22-
public Quaternion(double angle, Vector3D rotationAxis) {
23-
x = rotationAxis.x() * Math.sin(angle / 2);
24-
y = rotationAxis.y() * Math.sin(angle / 2);
25-
z = rotationAxis.z() * Math.sin(angle / 2);
26-
w = Math.cos(angle / 2);
27-
}
28-
29-
public Quaternion(double w, double x, double y, double z) {
30-
this.x = x;
31-
this.y = y;
32-
this.z = z;
33-
this.w = w;
34-
}
35-
/**
36-
* Set the rotation Quaternion
37-
* @param angle the angle of rotation
38-
* @param rotationAxis the rotation axis (normal two the vector plane)
39-
*/
40-
public void set(double angle, Vector3D rotationAxis) {
41-
x = rotationAxis.x() * Math.sin(angle / 2);
42-
y = rotationAxis.y() * Math.sin(angle / 2);
43-
z = rotationAxis.z() * Math.sin(angle / 2);
44-
w = Math.cos(angle / 2);
45-
}
46-
47-
public void set (double w, double x, double y, double z) {
48-
this.x = x;
49-
this.y = y;
50-
this.z = z;
51-
this.w = w;
52-
}
53-
54-
/**
55-
* Set the Quaternion using another one
56-
* @param q
57-
*/
58-
public void set (Quaternion q) {
59-
this.x = q.x;
60-
this.y = q.y;
61-
this.z = q.z;
62-
this.w = q.w;
63-
}
64-
65-
public double getSize() {
66-
return Math.sqrt(w * w + x * x + y * y + z * z);
67-
}
68-
69-
public void normalize() {
70-
double sizeInv = 1 / getSize();
71-
x *= sizeInv;
72-
y *= sizeInv;
73-
z *= sizeInv;
74-
w *= sizeInv;
75-
}
76-
/**
77-
* Multiplication between two Quaternions
78-
* @param qb the one that multiply
79-
* @return result of the multiplication
80-
*/
81-
public Quaternion multiply(Quaternion qb) {
82-
Quaternion qa = this;
83-
Quaternion qr = new Quaternion();
84-
qr.w = (qa.w * qb.w) - (qa.x * qb.x) - (qa.y * qb.y) - (qa.z * qb.z);
85-
qr.x = (qa.x * qb.w) + (qa.w * qb.x) + (qa.y * qb.z) - (qa.z * qb.y);
86-
qr.y = (qa.y * qb.w) + (qa.w * qb.y) + (qa.z * qb.x) - (qa.x * qb.z);
87-
qr.z = (qa.z * qb.w) + (qa.w * qb.z) + (qa.x * qb.y) - (qa.y * qb.x);
88-
return qr;
89-
}
90-
91-
public void conjugate() {
92-
//w = w;
93-
x = -x;
94-
y = -y;
95-
z = -z;
96-
}
97-
98-
99-
//-----------------
100-
public Vector3D rotate(Vector3D vector) {
101-
//-----------------
102-
/**
103-
* public void rotate(Vector3D vector, Vector3D rotatedVector) {
104-
* Implementing the Quaternion Fomula for Rotation: P' = Q.P.Q*
105-
* @param vetor the vector 3d that needs to be rotate
106-
* @param rotatedVector the rotated vector
13+
//Definition of the components for the Quaternion
14+
public double x;
15+
public double y;
16+
public double z;
17+
public double w;
18+
19+
public Quaternion() {
20+
}
21+
22+
public Quaternion(double angle, Vector3D rotationAxis) {
23+
x = rotationAxis.x() * Math.sin(angle / 2);
24+
y = rotationAxis.y() * Math.sin(angle / 2);
25+
z = rotationAxis.z() * Math.sin(angle / 2);
26+
w = Math.cos(angle / 2);
27+
}
28+
29+
public Quaternion(double w, double x, double y, double z) {
30+
this.x = x;
31+
this.y = y;
32+
this.z = z;
33+
this.w = w;
34+
}
35+
/**
36+
* Set the rotation Quaternion
37+
* @param angle the angle of rotation
38+
* @param rotationAxis the rotation axis (normal two the vector plane)
10739
*/
108-
40+
public void set(double angle, Vector3D rotationAxis) {
41+
x = rotationAxis.x() * Math.sin(angle / 2);
42+
y = rotationAxis.y() * Math.sin(angle / 2);
43+
z = rotationAxis.z() * Math.sin(angle / 2);
44+
w = Math.cos(angle / 2);
45+
}
46+
47+
public void set (double w, double x, double y, double z) {
48+
this.x = x;
49+
this.y = y;
50+
this.z = z;
51+
this.w = w;
52+
}
53+
54+
/**
55+
* Set the Quaternion using another one
56+
* @param q
57+
*/
58+
public void set (Quaternion q) {
59+
this.x = q.x;
60+
this.y = q.y;
61+
this.z = q.z;
62+
this.w = q.w;
63+
}
64+
65+
public double getSize() {
66+
return Math.sqrt(w * w + x * x + y * y + z * z);
67+
}
68+
69+
public void normalize() {
70+
double sizeInv = 1 / getSize();
71+
x *= sizeInv;
72+
y *= sizeInv;
73+
z *= sizeInv;
74+
w *= sizeInv;
75+
}
76+
/**
77+
* Multiplication between two Quaternions
78+
* @param qb the one that multiply
79+
* @return result of the multiplication
80+
*/
81+
public Quaternion multiply(Quaternion qb) {
82+
Quaternion qa = this;
83+
Quaternion qr = new Quaternion();
84+
qr.w = (qa.w * qb.w) - (qa.x * qb.x) - (qa.y * qb.y) - (qa.z * qb.z);
85+
qr.x = (qa.x * qb.w) + (qa.w * qb.x) + (qa.y * qb.z) - (qa.z * qb.y);
86+
qr.y = (qa.y * qb.w) + (qa.w * qb.y) + (qa.z * qb.x) - (qa.x * qb.z);
87+
qr.z = (qa.z * qb.w) + (qa.w * qb.z) + (qa.x * qb.y) - (qa.y * qb.x);
88+
return qr;
89+
}
90+
91+
public void conjugate() {
92+
//w = w;
93+
x = -x;
94+
y = -y;
95+
z = -z;
96+
}
97+
98+
99+
public Vector3D rotate(Vector3D vector) {
100+
/**
101+
* public void rotate(Vector3D vector, Vector3D rotatedVector) {
102+
* Implementing the Quaternion Fomula for Rotation: P' = Q.P.Q*
103+
* @param vetor the vector 3d that needs to be rotate
104+
* @param rotatedVector the rotated vector
105+
*/
106+
109107
Vector3D rotatedVector = new Vector3D(0.,0.,0.);
110108
Quaternion quatTp1 = new Quaternion(0, vector.x(), vector.y(), vector.z());
111109
Quaternion quatTp2 = new Quaternion();
@@ -114,81 +112,70 @@ public Vector3D rotate(Vector3D vector) {
114112
quatTp1.set(this);
115113
quatTp1.conjugate();
116114
quatTp3 = quatTp2.multiply(quatTp1);
117-
115+
118116
rotatedVector.setX(quatTp3.x);
119117
rotatedVector.setY(quatTp3.y);
120118
rotatedVector.setZ(quatTp3.z);
121-
119+
122120
return rotatedVector;
123121
}
124-
125-
126-
// To finish this code
127-
public Matrix toRotation(Quaternion q1)
128-
{
129-
Matrix Matrice = new Matrix(3,3);
130-
double heading =0;
131-
double attitude =0 ;
132-
double bank=0;
133-
134-
double test = q1.x*q1.y + q1.z*q1.w;
135-
if (test > 0.499) { // singularity at north pole
136-
heading = 2 * Math.atan2(q1.x,q1.w);
137-
attitude = Math.PI/2;
138-
bank = 0;
139-
return null;
140-
}
141-
if (test < -0.499) { // singularity at south pole
142-
heading = -2 * Math.atan2(q1.x,q1.w);
143-
attitude = - Math.PI/2;
144-
bank = 0;
145-
return null;
146-
}
147-
double sqx = q1.x*q1.x;
148-
double sqy = q1.y*q1.y;
149-
double sqz = q1.z*q1.z;
150-
heading = Math.atan2(2*q1.y*q1.w-2*q1.x*q1.z , 1 - 2*sqy - 2*sqz);
151-
attitude = Math.asin(2*test);
152-
bank = Math.atan2(2*q1.x*q1.w-2*q1.y*q1.z , 1 - 2*sqx - 2*sqz);
153-
154-
155-
// to finis the implementations of this effect
156-
return Matrice;
157-
}
158-
159-
//-----------------
160-
public double GetX() {
161-
//-----------------
162122

123+
124+
// To finish this code
125+
public Matrix toRotation(Quaternion q1)
126+
{
127+
Matrix Matrice = new Matrix(3,3);
128+
double heading =0;
129+
double attitude =0 ;
130+
double bank=0;
131+
132+
double test = q1.x*q1.y + q1.z*q1.w;
133+
if (test > 0.499) { // singularity at north pole
134+
heading = 2 * Math.atan2(q1.x,q1.w);
135+
attitude = Math.PI/2;
136+
bank = 0;
137+
return null;
138+
}
139+
if (test < -0.499) { // singularity at south pole
140+
heading = -2 * Math.atan2(q1.x,q1.w);
141+
attitude = - Math.PI/2;
142+
bank = 0;
143+
return null;
144+
}
145+
double sqx = q1.x*q1.x;
146+
double sqy = q1.y*q1.y;
147+
double sqz = q1.z*q1.z;
148+
heading = Math.atan2(2*q1.y*q1.w-2*q1.x*q1.z , 1 - 2*sqy - 2*sqz);
149+
attitude = Math.asin(2*test);
150+
bank = Math.atan2(2*q1.x*q1.w-2*q1.y*q1.z , 1 - 2*sqx - 2*sqz);
151+
152+
// to finis the implementations of this effect
153+
return Matrice;
154+
}
155+
156+
public double GetX() {
157+
163158
return x;
164159
}
165-
166-
//-----------------
167-
public double GetY() {
168-
//-----------------
169160

161+
public double GetY() {
162+
170163
return y;
171164
}
172-
173-
//-----------------
174-
public double GetZ() {
175-
//-----------------
176165

166+
public double GetZ() {
167+
177168
return z;
178169
}
179-
180-
//-----------------
181-
public double GetW() {
182-
//-----------------
183-
170+
171+
public double GetW() {
172+
184173
return w;
185174
}
186-
187-
//-----------------
175+
188176
public void show() {
189-
//-----------------
190-
191-
System.out.format(" Quaternion axis %8.3f %8.3f %8.3f angle %8.3f (%8.3f) \n",x,y,z,w,w*57.3);
177+
178+
System.out.format(" Quaternion axis %8.3f %8.3f %8.3f angle %8.3f (%8.3f) \n",x,y,z,w,w*57.3);
192179
}
193-
180+
194181
}

0 commit comments

Comments
 (0)