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