| Geant4 Cross Reference |
1 // -*- C++ -*-
2 // ---------------------------------------------------------------------------
3 //
4 // This file is a part of the CLHEP - a Class Library for High Energy Physics.
5 //
6 // This is the implementation of methods of the HepRotation class which
7 // were introduced when ZOOM PhysicsVectors was merged in, and which involve
8 // Euler Angles representation.
9 //
10 // Apr 28, 2003 mf Modified way of computing Euler angles to avoid flawed
11 // answers in the case where theta is near 0 of pi, and
12 // the matrix is not a perfect rotation (due to roundoff).
13
14 #include "CLHEP/Vector/Rotation.h"
15 #include "CLHEP/Vector/EulerAngles.h"
16 #include "CLHEP/Units/PhysicalConstants.h"
17
18 #include <cmath>
19 #include <iostream>
20
21 namespace CLHEP {
22
23 static inline double safe_acos (double x) {
24 if (std::abs(x) <= 1.0) return std::acos(x);
25 return ( (x>0) ? 0 : CLHEP::pi );
26 }
27
28 // ---------- Constructors and Assignment:
29
30 // Euler angles
31
32 HepRotation & HepRotation::set(double phi1, double theta1, double psi1) {
33
34 double sinPhi = std::sin( phi1 ), cosPhi = std::cos( phi1 );
35 double sinTheta = std::sin( theta1 ), cosTheta = std::cos( theta1 );
36 double sinPsi = std::sin( psi1 ), cosPsi = std::cos( psi1 );
37
38 rxx = cosPsi * cosPhi - cosTheta * sinPhi * sinPsi;
39 rxy = cosPsi * sinPhi + cosTheta * cosPhi * sinPsi;
40 rxz = sinPsi * sinTheta;
41
42 ryx = - sinPsi * cosPhi - cosTheta * sinPhi * cosPsi;
43 ryy = - sinPsi * sinPhi + cosTheta * cosPhi * cosPsi;
44 ryz = cosPsi * sinTheta;
45
46 rzx = sinTheta * sinPhi;
47 rzy = - sinTheta * cosPhi;
48 rzz = cosTheta;
49
50 return *this;
51
52 } // Rotation::set(phi, theta, psi)
53
54 HepRotation::HepRotation( double phi1, double theta1, double psi1 )
55 {
56 set (phi1, theta1, psi1);
57 }
58 HepRotation & HepRotation::set( const HepEulerAngles & e ) {
59 return set(e.phi(), e.theta(), e.psi());
60 }
61 HepRotation::HepRotation ( const HepEulerAngles & e )
62 {
63 set(e.phi(), e.theta(), e.psi());
64 }
65
66
67 double HepRotation::phi () const {
68
69 double s2 = 1.0 - rzz*rzz;
70 if (s2 < 0) {
71 std::cerr << "HepRotation::phi() - "
72 << "HepRotation::phi() finds | rzz | > 1 " << std::endl;
73 s2 = 0;
74 }
75 const double sinTheta = std::sqrt( s2 );
76
77 if (sinTheta < .01) { // For theta close to 0 or PI, use the more stable
78 // algorithm to get all three Euler angles
79 HepEulerAngles ea = eulerAngles();
80 return ea.phi();
81 }
82
83 const double cscTheta = 1/sinTheta;
84 double cosabsphi = - rzy * cscTheta;
85 if ( std::fabs(cosabsphi) > 1 ) { // NaN-proofing
86 std::cerr << "HepRotation::phi() - "
87 << "HepRotation::phi() finds | cos phi | > 1 " << std::endl;
88 cosabsphi = 1;
89 }
90 const double absPhi = std::acos ( cosabsphi );
91 if (rzx > 0) {
92 return absPhi;
93 } else if (rzx < 0) {
94 return -absPhi;
95 } else {
96 return (rzy < 0) ? 0 : CLHEP::pi;
97 }
98
99 } // phi()
100
101 double HepRotation::theta() const {
102
103 return safe_acos( rzz );
104
105 } // theta()
106
107 double HepRotation::psi () const {
108
109 double sinTheta;
110 if ( std::fabs(rzz) > 1 ) { // NaN-proofing
111 std::cerr << "HepRotation::psi() - "
112 << "HepRotation::psi() finds | rzz | > 1" << std::endl;
113 sinTheta = 0;
114 } else {
115 sinTheta = std::sqrt( 1.0 - rzz*rzz );
116 }
117
118 if (sinTheta < .01) { // For theta close to 0 or PI, use the more stable
119 // algorithm to get all three Euler angles
120 HepEulerAngles ea = eulerAngles();
121 return ea.psi();
122 }
123
124 const double cscTheta = 1/sinTheta;
125 double cosabspsi = ryz * cscTheta;
126 if ( std::fabs(cosabspsi) > 1 ) { // NaN-proofing
127 std::cerr << "HepRotation::psi() - "
128 << "HepRotation::psi() finds | cos psi | > 1" << std::endl;
129 cosabspsi = 1;
130 }
131 const double absPsi = std::acos ( cosabspsi );
132 if (rxz > 0) {
133 return absPsi;
134 } else if (rxz < 0) {
135 return -absPsi;
136 } else {
137 return (ryz > 0) ? 0 : CLHEP::pi;
138 }
139
140 } // psi()
141
142 // Helpers for eulerAngles():
143
144 static
145 void correctByPi ( double& psi1, double& phi1 ) {
146 if (psi1 > 0) {
147 psi1 -= CLHEP::pi;
148 } else {
149 psi1 += CLHEP::pi;
150 }
151 if (phi1 > 0) {
152 phi1 -= CLHEP::pi;
153 } else {
154 phi1 += CLHEP::pi;
155 }
156 }
157
158 static
159 void correctPsiPhi ( double rxz, double rzx, double ryz, double rzy,
160 double& psi1, double& phi1 ) {
161
162 // set up quatities which would be positive if sin and cosine of
163 // psi1 and phi1 were positive:
164 double w[4];
165 w[0] = rxz; w[1] = rzx; w[2] = ryz; w[3] = -rzy;
166
167 // find biggest relevant term, which is the best one to use in correcting.
168 double maxw = std::abs(w[0]);
169 int imax = 0;
170 for (int i = 1; i < 4; ++i) {
171 if (std::abs(w[i]) > maxw) {
172 maxw = std::abs(w[i]);
173 imax = i;
174 }
175 }
176 // Determine if the correction needs to be applied: The criteria are
177 // different depending on whether a sine or cosine was the determinor:
178 switch (imax) {
179 case 0:
180 if (w[0] > 0 && psi1 < 0) correctByPi ( psi1, phi1 );
181 if (w[0] < 0 && psi1 > 0) correctByPi ( psi1, phi1 );
182 break;
183 case 1:
184 if (w[1] > 0 && phi1 < 0) correctByPi ( psi1, phi1 );
185 if (w[1] < 0 && phi1 > 0) correctByPi ( psi1, phi1 );
186 break;
187 case 2:
188 if (w[2] > 0 && std::abs(psi1) > CLHEP::halfpi) correctByPi ( psi1, phi1 );
189 if (w[2] < 0 && std::abs(psi1) < CLHEP::halfpi) correctByPi ( psi1, phi1 );
190 break;
191 case 3:
192 if (w[3] > 0 && std::abs(phi1) > CLHEP::halfpi) correctByPi ( psi1, phi1 );
193 if (w[3] < 0 && std::abs(phi1) < CLHEP::halfpi) correctByPi ( psi1, phi1 );
194 break;
195 }
196 }
197
198 HepEulerAngles HepRotation::eulerAngles() const {
199
200 // Please see the mathematical justification in eulerAngleComputations.ps
201
202 double phi1, theta1, psi1;
203 double psiPlusPhi, psiMinusPhi;
204
205 theta1 = safe_acos( rzz );
206
207 // if (rzz > 1 || rzz < -1) {
208 // std::cerr << "HepRotation::eulerAngles() - "
209 // << "HepRotation::eulerAngles() finds | rzz | > 1 " << std::endl;
210 // }
211
212 double cosTheta = rzz;
213 if (cosTheta > 1) cosTheta = 1;
214 if (cosTheta < -1) cosTheta = -1;
215
216 if (cosTheta == 1) {
217 psiPlusPhi = std::atan2 ( rxy - ryx, rxx + ryy );
218 psiMinusPhi = 0;
219
220 } else if (cosTheta >= 0) {
221
222 // In this realm, the atan2 expression for psi + phi is numerically stable
223 psiPlusPhi = std::atan2 ( rxy - ryx, rxx + ryy );
224
225 // psi - phi is potentially more subtle, but when unstable it is moot
226 double s1 = -rxy - ryx; // sin (psi-phi) * (1 - cos theta)
227 double c1 = rxx - ryy; // cos (psi-phi) * (1 - cos theta)
228 psiMinusPhi = std::atan2 ( s1, c1 );
229
230 } else if (cosTheta > -1) {
231
232 // In this realm, the atan2 expression for psi - phi is numerically stable
233 psiMinusPhi = std::atan2 ( -rxy - ryx, rxx - ryy );
234
235 // psi + phi is potentially more subtle, but when unstable it is moot
236 double s1 = rxy - ryx; // sin (psi+phi) * (1 + cos theta)
237 double c1 = rxx + ryy; // cos (psi+phi) * (1 + cos theta)
238 psiPlusPhi = std::atan2 ( s1, c1 );
239
240 } else { // cosTheta == -1
241
242 psiMinusPhi = std::atan2 ( -rxy - ryx, rxx - ryy );
243 psiPlusPhi = 0;
244
245 }
246
247 psi1 = .5 * (psiPlusPhi + psiMinusPhi);
248 phi1 = .5 * (psiPlusPhi - psiMinusPhi);
249
250 // Now correct by pi if we have managed to get a value of psiPlusPhi
251 // or psiMinusPhi that was off by 2 pi:
252 correctPsiPhi ( rxz, rzx, ryz, rzy, psi1, phi1 );
253
254 return HepEulerAngles( phi1, theta1, psi1 );
255
256 } // eulerAngles()
257
258
259 void HepRotation::setPhi (double phi1) {
260 set ( phi1, theta(), psi() );
261 }
262
263 void HepRotation::setTheta (double theta1) {
264 set ( phi(), theta1, psi() );
265 }
266
267 void HepRotation::setPsi (double psi1) {
268 set ( phi(), theta(), psi1 );
269 }
270
271 } // namespace CLHEP
272
273