00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011 #ifdef GNUPRAGMA
00012 #pragma implementation
00013 #endif
00014
00015 #include "CLHEP/Vector/Rotation.h"
00016 #include "CLHEP/Units/PhysicalConstants.h"
00017
00018 #include <iostream>
00019 #include <cmath>
00020
00021 namespace CLHEP {
00022
00023
00024
00025
00026
00027 HepRotation & HepRotation::set( const Hep3Vector & aaxis, double ddelta ) {
00028
00029 register double sinDelta = std::sin(ddelta), cosDelta = std::cos(ddelta);
00030 register double oneMinusCosDelta = 1.0 - cosDelta;
00031
00032 Hep3Vector u = aaxis.unit();
00033
00034 register double uX = u.getX();
00035 register double uY = u.getY();
00036 register double uZ = u.getZ();
00037
00038 rxx = oneMinusCosDelta * uX * uX + cosDelta;
00039 rxy = oneMinusCosDelta * uX * uY - sinDelta * uZ;
00040 rxz = oneMinusCosDelta * uX * uZ + sinDelta * uY;
00041
00042 ryx = oneMinusCosDelta * uY * uX + sinDelta * uZ;
00043 ryy = oneMinusCosDelta * uY * uY + cosDelta;
00044 ryz = oneMinusCosDelta * uY * uZ - sinDelta * uX;
00045
00046 rzx = oneMinusCosDelta * uZ * uX - sinDelta * uY;
00047 rzy = oneMinusCosDelta * uZ * uY + sinDelta * uX;
00048 rzz = oneMinusCosDelta * uZ * uZ + cosDelta;
00049
00050 return *this;
00051
00052 }
00053
00054 HepRotation::HepRotation ( const Hep3Vector & aaxis, double ddelta )
00055 {
00056 set( aaxis, ddelta );
00057 }
00058 HepRotation & HepRotation::set( const HepAxisAngle & ax ) {
00059 return set ( ax.axis(), ax.delta() );
00060 }
00061 HepRotation::HepRotation ( const HepAxisAngle & ax )
00062 {
00063 set ( ax.axis(), ax.delta() );
00064 }
00065
00066 double HepRotation::delta() const {
00067
00068 double cosdelta = (rxx + ryy + rzz - 1.0) / 2.0;
00069 if (cosdelta > 1.0) {
00070 return 0;
00071 } else if (cosdelta < -1.0) {
00072 return CLHEP::pi;
00073 } else {
00074 return std::acos( cosdelta );
00075 }
00076
00077 }
00078
00079 Hep3Vector HepRotation::axis () const {
00080
00081
00082
00083
00084 double Uz = ryx - rxy;
00085 double Uy = rxz - rzx;
00086 double Ux = rzy - ryz;
00087
00088 if ( (Uz==0) && (Uy==0) && (Ux==0) ) {
00089 if ( rzz>0 ) {
00090 return Hep3Vector(0,0,1);
00091 } else if ( ryy>0 ) {
00092 return Hep3Vector(0,1,0);
00093 } else {
00094 return Hep3Vector(1,0,0);
00095 }
00096 } else {
00097 return Hep3Vector( Ux, Uy, Uz ).unit();
00098 }
00099
00100 }
00101
00102 HepAxisAngle HepRotation::axisAngle() const {
00103
00104 return HepAxisAngle (axis(), delta());
00105
00106 }
00107
00108
00109 void HepRotation::setAxis (const Hep3Vector & aaxis) {
00110 set ( aaxis, delta() );
00111 }
00112
00113 void HepRotation::setDelta (double ddelta) {
00114 set ( axis(), ddelta );
00115 }
00116
00117 }