| Geant4 Cross Reference |
1 // ------------------------------------------- 1 // ----------------------------------------------------------------------
2 // 2 //
3 // EulerAngles.cc 3 // EulerAngles.cc
4 // 4 //
5 // Methods for classes, and instances of globa 5 // Methods for classes, and instances of globals, declared in EulerAngles.h
6 // 6 //
7 // History: 7 // History:
8 // 8 //
9 // 04-Dec-1997 MF Stub with just PI 9 // 04-Dec-1997 MF Stub with just PI
10 // 12-Jan-1998 WEB PI now found in ZMutility; 10 // 12-Jan-1998 WEB PI now found in ZMutility; used ZMutility headers
11 // where available 11 // where available
12 // 16-Mar-1998 WEB Corrected ZMpvEulerAnglesR 12 // 16-Mar-1998 WEB Corrected ZMpvEulerAnglesRep
13 // 15-Jun-1998 WEB Added namespace support 13 // 15-Jun-1998 WEB Added namespace support
14 // 26-Jul-2000 MF CLHEP version 14 // 26-Jul-2000 MF CLHEP version
15 // 12-Apr-2001 MF NaN-proofing 15 // 12-Apr-2001 MF NaN-proofing
16 // 19-Nov-2001 MF Correction to ZMpvEulerA 16 // 19-Nov-2001 MF Correction to ZMpvEulerAnglesRep, which was affecting
17 // .isNear(). array[3] had been incorrec 17 // .isNear(). array[3] had been incorrect.
18 // Note - the correct form was used in al 18 // Note - the correct form was used in all other places
19 // including Rotation.set(phi, theta, psi 19 // including Rotation.set(phi, theta, psi).
20 // 20 //
21 // ------------------------------------------- 21 // ----------------------------------------------------------------------
22 22
23 23
24 #include "CLHEP/Vector/EulerAngles.h" 24 #include "CLHEP/Vector/EulerAngles.h"
25 25
26 #include "CLHEP/Vector/ThreeVector.h" 26 #include "CLHEP/Vector/ThreeVector.h"
27 27
28 #include <cmath> 28 #include <cmath>
29 #include <iostream> 29 #include <iostream>
30 30
31 namespace CLHEP { 31 namespace CLHEP {
32 32
33 //-************* 33 //-*************
34 // static consts 34 // static consts
35 //-************* 35 //-*************
36 36
37 double HepEulerAngles::tolerance = Hep3Vector: 37 double HepEulerAngles::tolerance = Hep3Vector::ToleranceTicks * 1.0e-8;
38 38
39 //-******************* 39 //-*******************
40 // measure of distance 40 // measure of distance
41 //-******************* 41 //-*******************
42 42
43 43
44 static void ZMpvEulerAnglesRep ( const HepEule 44 static void ZMpvEulerAnglesRep ( const HepEulerAngles & ex, double array[] ) {
45 45
46 double sinPhi = std::sin( ex.phi() ) , co 46 double sinPhi = std::sin( ex.phi() ) , cosPhi = std::cos( ex.phi() );
47 double sinTheta = std::sin( ex.theta() ), co 47 double sinTheta = std::sin( ex.theta() ), cosTheta = std::cos( ex.theta() );
48 double sinPsi = std::sin( ex.psi() ) , co 48 double sinPsi = std::sin( ex.psi() ) , cosPsi = std::cos( ex.psi() );
49 49
50 array[0] = cosPsi * cosPhi - sinPsi * co 50 array[0] = cosPsi * cosPhi - sinPsi * cosTheta * sinPhi;
51 array[1] = cosPsi * sinPhi + sinPsi * co 51 array[1] = cosPsi * sinPhi + sinPsi * cosTheta * cosPhi;
52 array[2] = sinPsi * sinTheta; 52 array[2] = sinPsi * sinTheta;
53 53
54 array[3] = - sinPsi * cosPhi - cosPsi * cosT 54 array[3] = - sinPsi * cosPhi - cosPsi * cosTheta * sinPhi;
55 array[4] = - sinPsi * sinPhi + cosPsi * co 55 array[4] = - sinPsi * sinPhi + cosPsi * cosTheta * cosPhi;
56 array[5] = cosPsi * sinTheta; 56 array[5] = cosPsi * sinTheta;
57 57
58 array[6] = sinTheta * sinPhi; 58 array[6] = sinTheta * sinPhi;
59 array[7] = - sinTheta * cosPhi; 59 array[7] = - sinTheta * cosPhi;
60 array[8] = cosTheta; 60 array[8] = cosTheta;
61 61
62 } // ZMpvEulerAnglesRep 62 } // ZMpvEulerAnglesRep
63 63
64 64
65 double HepEulerAngles::distance( const EA & ex 65 double HepEulerAngles::distance( const EA & ex ) const {
66 66
67 double thisRep[9]; 67 double thisRep[9];
68 double exRep[9]; 68 double exRep[9];
69 69
70 ZMpvEulerAnglesRep ( *this, thisRep ); 70 ZMpvEulerAnglesRep ( *this, thisRep );
71 ZMpvEulerAnglesRep ( ex, exRep ); 71 ZMpvEulerAnglesRep ( ex, exRep );
72 72
73 double sum = 0.0; 73 double sum = 0.0;
74 for (int i = 0; i < 9; i++) { 74 for (int i = 0; i < 9; i++) {
75 sum += thisRep[i] * exRep[i]; 75 sum += thisRep[i] * exRep[i];
76 } 76 }
77 77
78 double d = 3.0 - sum; // NaN-proofing: 78 double d = 3.0 - sum; // NaN-proofing:
79 return (d >= 0) ? d : 0; // sqrt(distance 79 return (d >= 0) ? d : 0; // sqrt(distance) is used in howNear()
80 80
81 } // HepEulerAngles::distance() 81 } // HepEulerAngles::distance()
82 82
83 83
84 bool HepEulerAngles::isNear( const EA & ex, do 84 bool HepEulerAngles::isNear( const EA & ex, double epsilon ) const {
85 85
86 return distance( ex ) <= epsilon*epsilon ; 86 return distance( ex ) <= epsilon*epsilon ;
87 87
88 } // HepEulerAngles::isNear() 88 } // HepEulerAngles::isNear()
89 89
90 90
91 double HepEulerAngles::howNear( const EA & ex 91 double HepEulerAngles::howNear( const EA & ex ) const {
92 92
93 return std::sqrt( distance( ex ) ); 93 return std::sqrt( distance( ex ) );
94 94
95 } // HepEulerAngles::howNear() 95 } // HepEulerAngles::howNear()
96 96
97 //-************** 97 //-**************
98 // Global Methods 98 // Global Methods
99 //-************** 99 //-**************
100 100
101 std::ostream & operator<<(std::ostream & os, c 101 std::ostream & operator<<(std::ostream & os, const HepEulerAngles & ea)
102 { 102 {
103 os << "(" << ea.phi() << ", " << ea.theta() 103 os << "(" << ea.phi() << ", " << ea.theta() << ", " << ea.psi() << ")";
104 return os; 104 return os;
105 } // operator<<() 105 } // operator<<()
106 106
107 void ZMinput3doubles ( std::istream & is, cons 107 void ZMinput3doubles ( std::istream & is, const char * type,
108 double & x, double & y, 108 double & x, double & y, double & z );
109 109
110 std::istream & operator>>(std::istream & is, H 110 std::istream & operator>>(std::istream & is, HepEulerAngles & ea) {
111 double thePhi; 111 double thePhi;
112 double theTheta; 112 double theTheta;
113 double thePsi; 113 double thePsi;
114 ZMinput3doubles ( is, "HepEulerAngle", thePh 114 ZMinput3doubles ( is, "HepEulerAngle", thePhi , theTheta , thePsi );
115 ea.set ( thePhi , theTheta , thePsi ); 115 ea.set ( thePhi , theTheta , thePsi );
116 return is; 116 return is;
117 } // operator>>() 117 } // operator>>()
118 118
119 } // namespace CLHEP 119 } // namespace CLHEP
120 120
121 121
122 122