Geant4 Cross Reference |
1 // ------------------------------------------- 1 // ---------------------------------------------------------------------- 2 // 2 // 3 // AxisAngle.cc 3 // AxisAngle.cc 4 // 4 // 5 // History: 5 // History: 6 // 23-Jan-1998 WEB Initial draft 6 // 23-Jan-1998 WEB Initial draft 7 // 13-Mar-1998 WEB Corrected ZMpvAxisAngle 7 // 13-Mar-1998 WEB Corrected ZMpvAxisAngleRep 8 // 15-Jun-1998 WEB Added namespace support 8 // 15-Jun-1998 WEB Added namespace support 9 // 26-Jul-2000 MF CLHEP version 9 // 26-Jul-2000 MF CLHEP version 10 // 12-Apr-2001 MF NaN-proofing 10 // 12-Apr-2001 MF NaN-proofing 11 // 11 // 12 // ------------------------------------------- 12 // ---------------------------------------------------------------------- 13 13 14 #include "CLHEP/Vector/AxisAngle.h" 14 #include "CLHEP/Vector/AxisAngle.h" 15 #include "CLHEP/Vector/ThreeVector.h" << 16 << 17 #include <cmath> << 18 #include <ostream> << 19 15 20 namespace CLHEP { 16 namespace CLHEP { 21 17 22 double HepAxisAngle::tolerance = Hep3Vector::T 18 double HepAxisAngle::tolerance = Hep3Vector::ToleranceTicks * 1.0e-08; 23 19 24 static void ZMpvAxisAngleRep( const HepAxisAng 20 static void ZMpvAxisAngleRep( const HepAxisAngle & aa, double array[] ) { 25 21 26 double sinDelta = std::sin( aa.delta() ); 22 double sinDelta = std::sin( aa.delta() ); 27 double cosDelta = std::cos( aa.delta() ); 23 double cosDelta = std::cos( aa.delta() ); 28 double oneMinusCosDelta = 1.0 - cosDelta; 24 double oneMinusCosDelta = 1.0 - cosDelta; 29 25 30 double uX = aa.getAxis().getX(); 26 double uX = aa.getAxis().getX(); 31 double uY = aa.getAxis().getY(); 27 double uY = aa.getAxis().getY(); 32 double uZ = aa.getAxis().getZ(); 28 double uZ = aa.getAxis().getZ(); 33 29 34 array[0] = oneMinusCosDelta * uX * uX + co 30 array[0] = oneMinusCosDelta * uX * uX + cosDelta; 35 array[1] = oneMinusCosDelta * uX * uY - si 31 array[1] = oneMinusCosDelta * uX * uY - sinDelta * uZ; 36 array[2] = oneMinusCosDelta * uX * uZ + si 32 array[2] = oneMinusCosDelta * uX * uZ + sinDelta * uY; 37 33 38 array[3] = oneMinusCosDelta * uY * uX + si 34 array[3] = oneMinusCosDelta * uY * uX + sinDelta * uZ; 39 array[4] = oneMinusCosDelta * uY * uY + co 35 array[4] = oneMinusCosDelta * uY * uY + cosDelta; 40 array[5] = oneMinusCosDelta * uY * uZ - si 36 array[5] = oneMinusCosDelta * uY * uZ - sinDelta * uX; 41 37 42 array[6] = oneMinusCosDelta * uZ * uX - si 38 array[6] = oneMinusCosDelta * uZ * uX - sinDelta * uY; 43 array[7] = oneMinusCosDelta * uZ * uY + si 39 array[7] = oneMinusCosDelta * uZ * uY + sinDelta * uX; 44 array[8] = oneMinusCosDelta * uZ * uZ + co 40 array[8] = oneMinusCosDelta * uZ * uZ + cosDelta; 45 41 46 } // ZMpvAxisAngleRep 42 } // ZMpvAxisAngleRep 47 43 48 44 49 double HepAxisAngle::distance( const AA & aa ) 45 double HepAxisAngle::distance( const AA & aa ) const { 50 46 51 double thisRep[9]; 47 double thisRep[9]; 52 double aaRep[9]; 48 double aaRep[9]; 53 49 54 ZMpvAxisAngleRep( *this, thisRep ); 50 ZMpvAxisAngleRep( *this, thisRep ); 55 ZMpvAxisAngleRep( aa, aaRep ); 51 ZMpvAxisAngleRep( aa, aaRep ); 56 52 57 double sum = 0.0; 53 double sum = 0.0; 58 for ( int i = 0; i < 9; i++ ) { 54 for ( int i = 0; i < 9; i++ ) { 59 sum += thisRep[i] * aaRep[i]; 55 sum += thisRep[i] * aaRep[i]; 60 } 56 } 61 57 62 double d = 3.0 - sum; // NaN-proofing: 58 double d = 3.0 - sum; // NaN-proofing: 63 return (d >= 0) ? d : 0; // std 59 return (d >= 0) ? d : 0; // std::sqrt(distance) is used in howNear() 64 60 65 } // HepAxisAngle::distance() 61 } // HepAxisAngle::distance() 66 62 67 63 68 bool HepAxisAngle::isNear( const AA & aa, Scal 64 bool HepAxisAngle::isNear( const AA & aa, Scalar epsilon ) const { 69 65 70 return distance( aa ) <= epsilon * epsilon; 66 return distance( aa ) <= epsilon * epsilon; 71 67 72 } // HepAxisAngle::isNear() 68 } // HepAxisAngle::isNear() 73 69 74 70 75 double HepAxisAngle::howNear( const AA & aa ) 71 double HepAxisAngle::howNear( const AA & aa ) const { 76 72 77 return std::sqrt( distance( aa ) ); 73 return std::sqrt( distance( aa ) ); 78 74 79 } // HepAxisAngle::howNear() 75 } // HepAxisAngle::howNear() 80 76 81 77 82 //-******************** 78 //-******************** 83 // 79 // 84 // Global methods 80 // Global methods 85 // 81 // 86 //-******************** 82 //-******************** 87 83 88 84 89 std::ostream & operator<<(std::ostream & os, c 85 std::ostream & operator<<(std::ostream & os, const HepAxisAngle & aa) { 90 os << '(' << aa.axis() << ", " << aa.delta() 86 os << '(' << aa.axis() << ", " << aa.delta() << ')'; 91 return os; 87 return os; 92 } // operator<<() 88 } // operator<<() 93 89 94 90 95 void ZMinputAxisAngle ( std::istream & is, 91 void ZMinputAxisAngle ( std::istream & is, 96 double & x, double & y, double & z, 92 double & x, double & y, double & z, 97 double & delta ); 93 double & delta ); 98 94 99 std::istream & operator>>(std::istream & is, H 95 std::istream & operator>>(std::istream & is, HepAxisAngle & aa) { 100 Hep3Vector axis; 96 Hep3Vector axis; 101 double delta; 97 double delta; 102 double x,y,z; 98 double x,y,z; 103 ZMinputAxisAngle ( is, x, y, z, delta ); 99 ZMinputAxisAngle ( is, x, y, z, delta ); 104 axis.set(x,y,z); 100 axis.set(x,y,z); 105 aa.set ( axis, delta ); 101 aa.set ( axis, delta ); 106 return is; 102 return is; 107 } // operator>>() 103 } // operator>>() 108 104 109 } // namespace CLHEP 105 } // namespace CLHEP 110 106