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