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 
26 namespace CLHEP {
27 
28 static 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 
37 HepRotation & 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 
59 HepRotation::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 
73 double 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 
107 double HepRotation::theta() const {
108 
109  return safe_acos( rzz );
110 
111 } // theta()
112 
113 double 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 
151 static
152 void 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 
165 static
166 void 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 
268 void HepRotation::setPhi (double phi1) {
269  set ( phi1, theta(), psi() );
270 }
271 
272 void HepRotation::setTheta (double theta1) {
273  set ( phi(), theta1, psi() );
274 }
275 
276 void HepRotation::setPsi (double psi1) {
277  set ( phi(), theta(), psi1 );
278 }
279 
280 } // namespace CLHEP
281