| 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