CLHEP VERSION Reference Documentation
   
CLHEP Home Page     CLHEP Documentation     CLHEP Bug Reports

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