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