00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014 #ifdef GNUPRAGMA
00015 #pragma implementation
00016 #endif
00017
00018 #include "CLHEP/Vector/Rotation.h"
00019 #include "CLHEP/Vector/EulerAngles.h"
00020 #include "CLHEP/Units/PhysicalConstants.h"
00021
00022 #include <cmath>
00023
00024 namespace CLHEP {
00025
00026 static inline double safe_acos (double x) {
00027 if (std::abs(x) <= 1.0) return std::acos(x);
00028 return ( (x>0) ? 0 : CLHEP::pi );
00029 }
00030
00031
00032
00033
00034
00035 HepRotation & HepRotation::set(double phi1, double theta1, double psi1) {
00036
00037 register double sinPhi = std::sin( phi1 ), cosPhi = std::cos( phi1 );
00038 register double sinTheta = std::sin( theta1 ), cosTheta = std::cos( theta1 );
00039 register double sinPsi = std::sin( psi1 ), cosPsi = std::cos( psi1 );
00040
00041 rxx = cosPsi * cosPhi - cosTheta * sinPhi * sinPsi;
00042 rxy = cosPsi * sinPhi + cosTheta * cosPhi * sinPsi;
00043 rxz = sinPsi * sinTheta;
00044
00045 ryx = - sinPsi * cosPhi - cosTheta * sinPhi * cosPsi;
00046 ryy = - sinPsi * sinPhi + cosTheta * cosPhi * cosPsi;
00047 ryz = cosPsi * sinTheta;
00048
00049 rzx = sinTheta * sinPhi;
00050 rzy = - sinTheta * cosPhi;
00051 rzz = cosTheta;
00052
00053 return *this;
00054
00055 }
00056
00057 HepRotation::HepRotation( double phi1, double theta1, double psi1 )
00058 {
00059 set (phi1, theta1, psi1);
00060 }
00061 HepRotation & HepRotation::set( const HepEulerAngles & e ) {
00062 return set(e.phi(), e.theta(), e.psi());
00063 }
00064 HepRotation::HepRotation ( const HepEulerAngles & e )
00065 {
00066 set(e.phi(), e.theta(), e.psi());
00067 }
00068
00069
00070 double HepRotation::phi () const {
00071
00072 double s2 = 1.0 - rzz*rzz;
00073 if (s2 < 0) {
00074 std::cerr << "HepRotation::phi() - "
00075 << "HepRotation::phi() finds | rzz | > 1 " << std::endl;
00076 s2 = 0;
00077 }
00078 const double sinTheta = std::sqrt( s2 );
00079
00080 if (sinTheta < .01) {
00081
00082 HepEulerAngles ea = eulerAngles();
00083 return ea.phi();
00084 }
00085
00086 const double cscTheta = 1/sinTheta;
00087 double cosabsphi = - rzy * cscTheta;
00088 if ( std::fabs(cosabsphi) > 1 ) {
00089 std::cerr << "HepRotation::phi() - "
00090 << "HepRotation::phi() finds | cos phi | > 1 " << std::endl;
00091 cosabsphi = 1;
00092 }
00093 const double absPhi = std::acos ( cosabsphi );
00094 if (rzx > 0) {
00095 return absPhi;
00096 } else if (rzx < 0) {
00097 return -absPhi;
00098 } else {
00099 return (rzy < 0) ? 0 : CLHEP::pi;
00100 }
00101
00102 }
00103
00104 double HepRotation::theta() const {
00105
00106 return safe_acos( rzz );
00107
00108 }
00109
00110 double HepRotation::psi () const {
00111
00112 double sinTheta;
00113 if ( std::fabs(rzz) > 1 ) {
00114 std::cerr << "HepRotation::psi() - "
00115 << "HepRotation::psi() finds | rzz | > 1" << std::endl;
00116 sinTheta = 0;
00117 } else {
00118 sinTheta = std::sqrt( 1.0 - rzz*rzz );
00119 }
00120
00121 if (sinTheta < .01) {
00122
00123 HepEulerAngles ea = eulerAngles();
00124 return ea.psi();
00125 }
00126
00127 const double cscTheta = 1/sinTheta;
00128 double cosabspsi = ryz * cscTheta;
00129 if ( std::fabs(cosabspsi) > 1 ) {
00130 std::cerr << "HepRotation::psi() - "
00131 << "HepRotation::psi() finds | cos psi | > 1" << std::endl;
00132 cosabspsi = 1;
00133 }
00134 const double absPsi = std::acos ( cosabspsi );
00135 if (rxz > 0) {
00136 return absPsi;
00137 } else if (rxz < 0) {
00138 return -absPsi;
00139 } else {
00140 return (ryz > 0) ? 0 : CLHEP::pi;
00141 }
00142
00143 }
00144
00145
00146
00147 static
00148 void correctByPi ( double& psi1, double& phi1 ) {
00149 if (psi1 > 0) {
00150 psi1 -= CLHEP::pi;
00151 } else {
00152 psi1 += CLHEP::pi;
00153 }
00154 if (phi1 > 0) {
00155 phi1 -= CLHEP::pi;
00156 } else {
00157 phi1 += CLHEP::pi;
00158 }
00159 }
00160
00161 static
00162 void correctPsiPhi ( double rxz, double rzx, double ryz, double rzy,
00163 double& psi1, double& phi1 ) {
00164
00165
00166
00167 double w[4];
00168 w[0] = rxz; w[1] = rzx; w[2] = ryz; w[3] = -rzy;
00169
00170
00171 double maxw = std::abs(w[0]);
00172 int imax = 0;
00173 for (int i = 1; i < 4; ++i) {
00174 if (std::abs(w[i]) > maxw) {
00175 maxw = std::abs(w[i]);
00176 imax = i;
00177 }
00178 }
00179
00180
00181 switch (imax) {
00182 case 0:
00183 if (w[0] > 0 && psi1 < 0) correctByPi ( psi1, phi1 );
00184 if (w[0] < 0 && psi1 > 0) correctByPi ( psi1, phi1 );
00185 break;
00186 case 1:
00187 if (w[1] > 0 && phi1 < 0) correctByPi ( psi1, phi1 );
00188 if (w[1] < 0 && phi1 > 0) correctByPi ( psi1, phi1 );
00189 break;
00190 case 2:
00191 if (w[2] > 0 && std::abs(psi1) > CLHEP::halfpi) correctByPi ( psi1, phi1 );
00192 if (w[2] < 0 && std::abs(psi1) < CLHEP::halfpi) correctByPi ( psi1, phi1 );
00193 break;
00194 case 3:
00195 if (w[3] > 0 && std::abs(phi1) > CLHEP::halfpi) correctByPi ( psi1, phi1 );
00196 if (w[3] < 0 && std::abs(phi1) < CLHEP::halfpi) correctByPi ( psi1, phi1 );
00197 break;
00198 }
00199 }
00200
00201 HepEulerAngles HepRotation::eulerAngles() const {
00202
00203
00204
00205 double phi1, theta1, psi1;
00206 double psiPlusPhi, psiMinusPhi;
00207
00208 theta1 = safe_acos( rzz );
00209
00210
00211
00212
00213
00214
00215 double cosTheta = rzz;
00216 if (cosTheta > 1) cosTheta = 1;
00217 if (cosTheta < -1) cosTheta = -1;
00218
00219 if (cosTheta == 1) {
00220 psiPlusPhi = std::atan2 ( rxy - ryx, rxx + ryy );
00221 psiMinusPhi = 0;
00222
00223 } else if (cosTheta >= 0) {
00224
00225
00226 psiPlusPhi = std::atan2 ( rxy - ryx, rxx + ryy );
00227
00228
00229 double s1 = -rxy - ryx;
00230 double c1 = rxx - ryy;
00231 psiMinusPhi = std::atan2 ( s1, c1 );
00232
00233 } else if (cosTheta > -1) {
00234
00235
00236 psiMinusPhi = std::atan2 ( -rxy - ryx, rxx - ryy );
00237
00238
00239 double s1 = rxy - ryx;
00240 double c1 = rxx + ryy;
00241 psiPlusPhi = std::atan2 ( s1, c1 );
00242
00243 } else {
00244
00245 psiMinusPhi = std::atan2 ( -rxy - ryx, rxx - ryy );
00246 psiPlusPhi = 0;
00247
00248 }
00249
00250 psi1 = .5 * (psiPlusPhi + psiMinusPhi);
00251 phi1 = .5 * (psiPlusPhi - psiMinusPhi);
00252
00253
00254
00255 correctPsiPhi ( rxz, rzx, ryz, rzy, psi1, phi1 );
00256
00257 return HepEulerAngles( phi1, theta1, psi1 );
00258
00259 }
00260
00261
00262 void HepRotation::setPhi (double phi1) {
00263 set ( phi1, theta(), psi() );
00264 }
00265
00266 void HepRotation::setTheta (double theta1) {
00267 set ( phi(), theta1, psi() );
00268 }
00269
00270 void HepRotation::setPsi (double psi1) {
00271 set ( phi(), theta(), psi1 );
00272 }
00273
00274 }
00275