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