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