Geant4 Cross Reference |
1 // -*- C++ -*- 1 2 // ------------------------------------------- 3 // 4 // This file is a part of the CLHEP - a Class 5 // 6 // This is the implementation of those methods 7 // were introduced when ZOOM PhysicsVectors wa 8 // the angle/axis representation of a Rotation 9 // 10 11 #include "CLHEP/Vector/Rotation.h" 12 #include "CLHEP/Units/PhysicalConstants.h" 13 14 #include <iostream> 15 #include <cmath> 16 17 namespace CLHEP { 18 19 // ---------- Constructors and Assignment: 20 21 // axis and angle 22 23 HepRotation & HepRotation::set( const Hep3Vect 24 25 double sinDelta = std::sin(ddelta), cosDelta 26 double oneMinusCosDelta = 1.0 - cosDelta; 27 28 Hep3Vector u = aaxis.unit(); 29 30 double uX = u.getX(); 31 double uY = u.getY(); 32 double uZ = u.getZ(); 33 34 rxx = oneMinusCosDelta * uX * uX + cosDelt 35 rxy = oneMinusCosDelta * uX * uY - sinDelt 36 rxz = oneMinusCosDelta * uX * uZ + sinDelt 37 38 ryx = oneMinusCosDelta * uY * uX + sinDelt 39 ryy = oneMinusCosDelta * uY * uY + cosDelt 40 ryz = oneMinusCosDelta * uY * uZ - sinDelt 41 42 rzx = oneMinusCosDelta * uZ * uX - sinDelt 43 rzy = oneMinusCosDelta * uZ * uY + sinDelt 44 rzz = oneMinusCosDelta * uZ * uZ + cosDelt 45 46 return *this; 47 48 } // HepRotation::set(axis, delta) 49 50 HepRotation::HepRotation ( const Hep3Vector & 51 { 52 set( aaxis, ddelta ); 53 } 54 HepRotation & HepRotation::set( const HepAxisA 55 return set ( ax.axis(), ax.delta() ); 56 } 57 HepRotation::HepRotation ( const HepAxisAngle 58 { 59 set ( ax.axis(), ax.delta() ); 60 } 61 62 double HepRotation::delta() const { 63 64 double cosdelta = (rxx + ryy + rzz - 1.0) / 65 if (cosdelta > 1.0) { 66 return 0; 67 } else if (cosdelta < -1.0) { 68 return CLHEP::pi; 69 } else { 70 return std::acos( cosdelta ); // Already 71 } 72 73 } // delta() 74 75 Hep3Vector HepRotation::axis () const { 76 77 const double eps = 1e-15; 78 79 double Ux = rzy - ryz; 80 double Uy = rxz - rzx; 81 double Uz = ryx - rxy; 82 if (std::abs(Ux) < eps && std::abs(Uy) < eps 83 84 double cosdelta = (rxx + ryy + rzz - 1.0) 85 if (cosdelta > 0.0) return Hep3Vector(0,0, 86 87 double mxx = (rxx + 1)/2; 88 double myy = (ryy + 1)/2; 89 double mzz = (rzz + 1)/2; 90 double mxy = (rxy + ryx)/4; 91 double mxz = (rxz + rzx)/4; 92 double myz = (ryz + rzy)/4; 93 double x, y, z; 94 95 if (mxx > ryy && mxx > rzz) { 96 x = std::sqrt(mxx); 97 if (rzy - ryz < 0) x = -x; 98 y = mxy/x; 99 z = mxz/x; 100 return Hep3Vector( x, y, z ).unit(); 101 } else if (myy > mzz) { 102 y = std::sqrt(myy); 103 if (rxz - rzx < 0) y = -y; 104 x = mxy/y; 105 z = myz/y; 106 return Hep3Vector( x, y, z ).unit(); 107 } else { 108 z = std::sqrt(mzz); 109 if (ryx - rxy < 0) z = -z; 110 x = mxz/z; 111 y = myz/z; 112 return Hep3Vector( x, y, z ).unit(); 113 } 114 } else { 115 return Hep3Vector( Ux, Uy, Uz ).unit(); 116 } 117 118 } // axis() 119 120 HepAxisAngle HepRotation::axisAngle() const { 121 122 return HepAxisAngle (axis(), delta()); 123 124 } // axisAngle() 125 126 127 void HepRotation::setAxis (const Hep3Vector & 128 set ( aaxis, delta() ); 129 } 130 131 void HepRotation::setDelta (double ddelta) { 132 set ( axis(), ddelta ); 133 } 134 135 } // namespace CLHEP 136