CLHEP 2.0.4.7 Reference Documentation
CLHEP Home Page CLHEP Documentation CLHEP Bug Reports |
00001 // -*- C++ -*- 00002 // --------------------------------------------------------------------------- 00003 // 00004 // This file is a part of the CLHEP - a Class Library for High Energy Physics. 00005 // 00006 // This is the implementation of methods of the HepRotation class which 00007 // were introduced when ZOOM PhysicsVectors was merged in, and which involve 00008 // Euler Angles representation. 00009 // 00010 // Apr 28, 2003 mf Modified way of computing Euler angles to avoid flawed 00011 // answers in the case where theta is near 0 of pi, and 00012 // the matrix is not a perfect rotation (due to roundoff). 00013 00014 #ifdef GNUPRAGMA 00015 #pragma implementation 00016 #endif 00017 00018 #include "CLHEP/Vector/defs.h" 00019 #include "CLHEP/Vector/Rotation.h" 00020 #include "CLHEP/Vector/EulerAngles.h" 00021 #include "CLHEP/Units/PhysicalConstants.h" 00022 00023 #include <cmath> 00024 #include <stdlib.h> 00025 00026 using std::abs; 00027 00028 namespace CLHEP { 00029 00030 static inline double safe_acos (double x) { 00031 if (abs(x) <= 1.0) return acos(x); 00032 return ( (x>0) ? 0 : CLHEP::pi ); 00033 } 00034 00035 // ---------- Constructors and Assignment: 00036 00037 // Euler angles 00038 00039 HepRotation & HepRotation::set(double phi, double theta, double psi) { 00040 00041 register double sinPhi = sin( phi ), cosPhi = cos( phi ); 00042 register double sinTheta = sin( theta ), cosTheta = cos( theta ); 00043 register double sinPsi = sin( psi ), cosPsi = cos( psi ); 00044 00045 rxx = cosPsi * cosPhi - cosTheta * sinPhi * sinPsi; 00046 rxy = cosPsi * sinPhi + cosTheta * cosPhi * sinPsi; 00047 rxz = sinPsi * sinTheta; 00048 00049 ryx = - sinPsi * cosPhi - cosTheta * sinPhi * cosPsi; 00050 ryy = - sinPsi * sinPhi + cosTheta * cosPhi * cosPsi; 00051 ryz = cosPsi * sinTheta; 00052 00053 rzx = sinTheta * sinPhi; 00054 rzy = - sinTheta * cosPhi; 00055 rzz = cosTheta; 00056 00057 return *this; 00058 00059 } // Rotation::set(phi, theta, psi) 00060 00061 HepRotation::HepRotation( double phi, double theta, double psi ) 00062 { 00063 set (phi, theta, psi); 00064 } 00065 HepRotation & HepRotation::set( const HepEulerAngles & e ) { 00066 return set(e.phi(), e.theta(), e.psi()); 00067 } 00068 HepRotation::HepRotation ( const HepEulerAngles & e ) 00069 { 00070 set(e.phi(), e.theta(), e.psi()); 00071 } 00072 00073 00074 00075 double HepRotation::phi () const { 00076 00077 double s2 = 1.0 - rzz*rzz; 00078 if (s2 < 0) { 00079 ZMthrowC ( ZMxpvImproperRotation ( 00080 "HepRotation::phi() finds | rzz | > 1 ")); 00081 s2 = 0; 00082 } 00083 const double sinTheta = sqrt( s2 ); 00084 00085 if (sinTheta < .01) { // For theta close to 0 or PI, use the more stable 00086 // algorithm to get all three Euler angles 00087 HepEulerAngles ea = eulerAngles(); 00088 return ea.phi(); 00089 } 00090 00091 const double cscTheta = 1/sinTheta; 00092 double cosabsphi = - rzy * cscTheta; 00093 if ( fabs(cosabsphi) > 1 ) { // NaN-proofing 00094 ZMthrowC ( ZMxpvImproperRotation ( 00095 "HepRotation::phi() finds | cos phi | > 1 ")); 00096 cosabsphi = 1; 00097 } 00098 const double absPhi = acos ( cosabsphi ); 00099 if (rzx > 0) { 00100 return absPhi; 00101 } else if (rzx < 0) { 00102 return -absPhi; 00103 } else { 00104 return (rzy < 0) ? 0 : CLHEP::pi; 00105 } 00106 00107 } // phi() 00108 00109 double HepRotation::theta() const { 00110 00111 return safe_acos( rzz ); 00112 00113 } // theta() 00114 00115 double HepRotation::psi () const { 00116 00117 double sinTheta; 00118 if ( fabs(rzz) > 1 ) { // NaN-proofing 00119 ZMthrowC ( ZMxpvImproperRotation ( 00120 "HepRotation::psi() finds | rzz | > 1")); 00121 sinTheta = 0; 00122 } else { 00123 sinTheta = sqrt( 1.0 - rzz*rzz ); 00124 } 00125 00126 if (sinTheta < .01) { // For theta close to 0 or PI, use the more stable 00127 // algorithm to get all three Euler angles 00128 HepEulerAngles ea = eulerAngles(); 00129 return ea.psi(); 00130 } 00131 00132 const double cscTheta = 1/sinTheta; 00133 double cosabspsi = ryz * cscTheta; 00134 if ( fabs(cosabspsi) > 1 ) { // NaN-proofing 00135 ZMthrowC ( ZMxpvImproperRotation ( 00136 "HepRotation::psi() finds | cos psi | > 1")); 00137 cosabspsi = 1; 00138 } 00139 const double absPsi = acos ( cosabspsi ); 00140 if (rxz > 0) { 00141 return absPsi; 00142 } else if (rxz < 0) { 00143 return -absPsi; 00144 } else { 00145 return (ryz > 0) ? 0 : CLHEP::pi; 00146 } 00147 00148 } // psi() 00149 00150 00151 // Helpers for eulerAngles(): 00152 00153 static 00154 void correctByPi ( double& psi, double& phi ) { 00155 if (psi > 0) { 00156 psi -= CLHEP::pi; 00157 } else { 00158 psi += CLHEP::pi; 00159 } 00160 if (phi > 0) { 00161 phi -= CLHEP::pi; 00162 } else { 00163 phi += CLHEP::pi; 00164 } 00165 } 00166 00167 static 00168 void correctPsiPhi ( double rxz, double rzx, double ryz, double rzy, 00169 double& psi, double& phi ) { 00170 00171 // set up quatities which would be positive if sin and cosine of 00172 // psi and phi were positive: 00173 double w[4]; 00174 w[0] = rxz; w[1] = rzx; w[2] = ryz; w[3] = -rzy; 00175 00176 // find biggest relevant term, which is the best one to use in correcting. 00177 double maxw = abs(w[0]); 00178 int imax = 0; 00179 for (int i = 1; i < 4; ++i) { 00180 if (abs(w[i]) > maxw) { 00181 maxw = abs(w[i]); 00182 imax = i; 00183 } 00184 } 00185 // Determine if the correction needs to be applied: The criteria are 00186 // different depending on whether a sine or cosine was the determinor: 00187 switch (imax) { 00188 case 0: 00189 if (w[0] > 0 && psi < 0) correctByPi ( psi, phi ); 00190 if (w[0] < 0 && psi > 0) correctByPi ( psi, phi ); 00191 break; 00192 case 1: 00193 if (w[1] > 0 && phi < 0) correctByPi ( psi, phi ); 00194 if (w[1] < 0 && phi > 0) correctByPi ( psi, phi ); 00195 break; 00196 case 2: 00197 if (w[2] > 0 && abs(psi) > CLHEP::halfpi) correctByPi ( psi, phi ); 00198 if (w[2] < 0 && abs(psi) < CLHEP::halfpi) correctByPi ( psi, phi ); 00199 break; 00200 case 3: 00201 if (w[3] > 0 && abs(phi) > CLHEP::halfpi) correctByPi ( psi, phi ); 00202 if (w[3] < 0 && abs(phi) < CLHEP::halfpi) correctByPi ( psi, phi ); 00203 break; 00204 } 00205 } 00206 00207 00208 HepEulerAngles HepRotation::eulerAngles() const { 00209 00210 // Please see the mathematical justification in eulerAngleComputations.ps 00211 00212 double phi, theta, psi; 00213 double psiPlusPhi, psiMinusPhi; 00214 00215 theta = safe_acos( rzz ); 00216 00217 if (rzz > 1 || rzz < -1) { 00218 ZMthrowC ( ZMxpvImproperRotation ( 00219 "HepRotation::eulerAngles() finds | rzz | > 1 ")); 00220 } 00221 00222 double cosTheta = rzz; 00223 if (cosTheta > 1) cosTheta = 1; 00224 if (cosTheta < -1) cosTheta = -1; 00225 00226 if (cosTheta == 1) { 00227 psiPlusPhi = atan2 ( rxy - ryx, rxx + ryy ); 00228 psiMinusPhi = 0; 00229 00230 } else if (cosTheta >= 0) { 00231 00232 // In this realm, the atan2 expression for psi + phi is numerically stable 00233 psiPlusPhi = atan2 ( rxy - ryx, rxx + ryy ); 00234 00235 // psi - phi is potentially more subtle, but when unstable it is moot 00236 double s = -rxy - ryx; // sin (psi-phi) * (1 - cos theta) 00237 double c = rxx - ryy; // cos (psi-phi) * (1 - cos theta) 00238 psiMinusPhi = atan2 ( s, c ); 00239 00240 } else if (cosTheta > -1) { 00241 00242 // In this realm, the atan2 expression for psi - phi is numerically stable 00243 psiMinusPhi = atan2 ( -rxy - ryx, rxx - ryy ); 00244 00245 // psi + phi is potentially more subtle, but when unstable it is moot 00246 double s = rxy - ryx; // sin (psi+phi) * (1 + cos theta) 00247 double c = rxx + ryy; // cos (psi+phi) * (1 + cos theta) 00248 psiPlusPhi = atan2 ( s, c ); 00249 00250 } else { // cosTheta == -1 00251 00252 psiMinusPhi = atan2 ( -rxy - ryx, rxx - ryy ); 00253 psiPlusPhi = 0; 00254 00255 } 00256 00257 psi = .5 * (psiPlusPhi + psiMinusPhi); 00258 phi = .5 * (psiPlusPhi - psiMinusPhi); 00259 00260 // Now correct by pi if we have managed to get a value of psiPlusPhi 00261 // or psiMinusPhi that was off by 2 pi: 00262 correctPsiPhi ( rxz, rzx, ryz, rzy, psi, phi ); 00263 00264 return HepEulerAngles( phi, theta, psi ); 00265 00266 } // eulerAngles() 00267 00268 00269 00270 void HepRotation::setPhi (double phi) { 00271 set ( phi, theta(), psi() ); 00272 } 00273 00274 void HepRotation::setTheta (double theta) { 00275 set ( phi(), theta, psi() ); 00276 } 00277 00278 void HepRotation::setPsi (double psi) { 00279 set ( phi(), theta(), psi ); 00280 } 00281 00282 } // namespace CLHEP 00283