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