| Geant4 Cross Reference |
1 // -*- C++ -*- 1 // -*- C++ -*-
2 // ------------------------------------------- 2 // ---------------------------------------------------------------------------
3 // 3 //
4 // This file is a part of the CLHEP - a Class 4 // This file is a part of the CLHEP - a Class Library for High Energy Physics.
5 // 5 //
6 // This is the implementation of those methods 6 // This is the implementation of those methods of the HepRotation class which
7 // were introduced when ZOOM PhysicsVectors wa 7 // were introduced when ZOOM PhysicsVectors was merged in, and which involve
8 // the angle/axis representation of a Rotation 8 // the angle/axis representation of a Rotation.
9 // 9 //
10 10
>> 11 #ifdef GNUPRAGMA
>> 12 #pragma implementation
>> 13 #endif
>> 14
11 #include "CLHEP/Vector/Rotation.h" 15 #include "CLHEP/Vector/Rotation.h"
12 #include "CLHEP/Units/PhysicalConstants.h" 16 #include "CLHEP/Units/PhysicalConstants.h"
13 17
14 #include <iostream> 18 #include <iostream>
15 #include <cmath> 19 #include <cmath>
16 20
17 namespace CLHEP { 21 namespace CLHEP {
18 22
19 // ---------- Constructors and Assignment: 23 // ---------- Constructors and Assignment:
20 24
21 // axis and angle 25 // axis and angle
22 26
23 HepRotation & HepRotation::set( const Hep3Vect 27 HepRotation & HepRotation::set( const Hep3Vector & aaxis, double ddelta ) {
24 28
25 double sinDelta = std::sin(ddelta), cosDelta 29 double sinDelta = std::sin(ddelta), cosDelta = std::cos(ddelta);
26 double oneMinusCosDelta = 1.0 - cosDelta; 30 double oneMinusCosDelta = 1.0 - cosDelta;
27 31
28 Hep3Vector u = aaxis.unit(); 32 Hep3Vector u = aaxis.unit();
29 33
30 double uX = u.getX(); 34 double uX = u.getX();
31 double uY = u.getY(); 35 double uY = u.getY();
32 double uZ = u.getZ(); 36 double uZ = u.getZ();
33 37
34 rxx = oneMinusCosDelta * uX * uX + cosDelt 38 rxx = oneMinusCosDelta * uX * uX + cosDelta;
35 rxy = oneMinusCosDelta * uX * uY - sinDelt 39 rxy = oneMinusCosDelta * uX * uY - sinDelta * uZ;
36 rxz = oneMinusCosDelta * uX * uZ + sinDelt 40 rxz = oneMinusCosDelta * uX * uZ + sinDelta * uY;
37 41
38 ryx = oneMinusCosDelta * uY * uX + sinDelt 42 ryx = oneMinusCosDelta * uY * uX + sinDelta * uZ;
39 ryy = oneMinusCosDelta * uY * uY + cosDelt 43 ryy = oneMinusCosDelta * uY * uY + cosDelta;
40 ryz = oneMinusCosDelta * uY * uZ - sinDelt 44 ryz = oneMinusCosDelta * uY * uZ - sinDelta * uX;
41 45
42 rzx = oneMinusCosDelta * uZ * uX - sinDelt 46 rzx = oneMinusCosDelta * uZ * uX - sinDelta * uY;
43 rzy = oneMinusCosDelta * uZ * uY + sinDelt 47 rzy = oneMinusCosDelta * uZ * uY + sinDelta * uX;
44 rzz = oneMinusCosDelta * uZ * uZ + cosDelt 48 rzz = oneMinusCosDelta * uZ * uZ + cosDelta;
45 49
46 return *this; 50 return *this;
47 51
48 } // HepRotation::set(axis, delta) 52 } // HepRotation::set(axis, delta)
49 53
50 HepRotation::HepRotation ( const Hep3Vector & 54 HepRotation::HepRotation ( const Hep3Vector & aaxis, double ddelta )
51 { 55 {
52 set( aaxis, ddelta ); 56 set( aaxis, ddelta );
53 } 57 }
54 HepRotation & HepRotation::set( const HepAxisA 58 HepRotation & HepRotation::set( const HepAxisAngle & ax ) {
55 return set ( ax.axis(), ax.delta() ); 59 return set ( ax.axis(), ax.delta() );
56 } 60 }
57 HepRotation::HepRotation ( const HepAxisAngle 61 HepRotation::HepRotation ( const HepAxisAngle & ax )
58 { 62 {
59 set ( ax.axis(), ax.delta() ); 63 set ( ax.axis(), ax.delta() );
60 } 64 }
61 65
62 double HepRotation::delta() const { 66 double HepRotation::delta() const {
63 67
64 double cosdelta = (rxx + ryy + rzz - 1.0) / 68 double cosdelta = (rxx + ryy + rzz - 1.0) / 2.0;
65 if (cosdelta > 1.0) { 69 if (cosdelta > 1.0) {
66 return 0; 70 return 0;
67 } else if (cosdelta < -1.0) { 71 } else if (cosdelta < -1.0) {
68 return CLHEP::pi; 72 return CLHEP::pi;
69 } else { 73 } else {
70 return std::acos( cosdelta ); // Already 74 return std::acos( cosdelta ); // Already safe due to the cosdelta > 1 check
71 } 75 }
72 76
73 } // delta() 77 } // delta()
74 78
75 Hep3Vector HepRotation::axis () const { 79 Hep3Vector HepRotation::axis () const {
76 80
77 const double eps = 1e-15; << 81 // Determine 2*std::sin(delta) times the u components (I call this uX, uY, Uz)
>> 82 // Normalization is not needed; it will be done when returning the 3-Vector
78 83
79 double Ux = rzy - ryz; << 84 double Uz = ryx - rxy;
80 double Uy = rxz - rzx; << 85 double Uy = rxz - rzx;
81 double Uz = ryx - rxy; << 86 double Ux = rzy - ryz;
82 if (std::abs(Ux) < eps && std::abs(Uy) < eps << 87
83 << 88 if ( (Uz==0) && (Uy==0) && (Ux==0) ) {
84 double cosdelta = (rxx + ryy + rzz - 1.0) << 89 if ( rzz>0 ) {
85 if (cosdelta > 0.0) return Hep3Vector(0,0, << 90 return Hep3Vector(0,0,1);
86 << 91 } else if ( ryy>0 ) {
87 double mxx = (rxx + 1)/2; << 92 return Hep3Vector(0,1,0);
88 double myy = (ryy + 1)/2; <<
89 double mzz = (rzz + 1)/2; <<
90 double mxy = (rxy + ryx)/4; <<
91 double mxz = (rxz + rzx)/4; <<
92 double myz = (ryz + rzy)/4; <<
93 double x, y, z; <<
94 <<
95 if (mxx > ryy && mxx > rzz) { <<
96 x = std::sqrt(mxx); <<
97 if (rzy - ryz < 0) x = -x; <<
98 y = mxy/x; <<
99 z = mxz/x; <<
100 return Hep3Vector( x, y, z ).unit(); <<
101 } else if (myy > mzz) { <<
102 y = std::sqrt(myy); <<
103 if (rxz - rzx < 0) y = -y; <<
104 x = mxy/y; <<
105 z = myz/y; <<
106 return Hep3Vector( x, y, z ).unit(); <<
107 } else { 93 } else {
108 z = std::sqrt(mzz); << 94 return Hep3Vector(1,0,0);
109 if (ryx - rxy < 0) z = -z; <<
110 x = mxz/z; <<
111 y = myz/z; <<
112 return Hep3Vector( x, y, z ).unit(); <<
113 } 95 }
114 } else { 96 } else {
115 return Hep3Vector( Ux, Uy, Uz ).unit(); 97 return Hep3Vector( Ux, Uy, Uz ).unit();
116 } 98 }
117 99
118 } // axis() 100 } // axis()
119 101
120 HepAxisAngle HepRotation::axisAngle() const { 102 HepAxisAngle HepRotation::axisAngle() const {
121 103
122 return HepAxisAngle (axis(), delta()); 104 return HepAxisAngle (axis(), delta());
123 105
124 } // axisAngle() 106 } // axisAngle()
125 107
126 108
127 void HepRotation::setAxis (const Hep3Vector & 109 void HepRotation::setAxis (const Hep3Vector & aaxis) {
128 set ( aaxis, delta() ); 110 set ( aaxis, delta() );
129 } 111 }
130 112
131 void HepRotation::setDelta (double ddelta) { 113 void HepRotation::setDelta (double ddelta) {
132 set ( axis(), ddelta ); 114 set ( axis(), ddelta );
133 } 115 }
134 116
135 } // namespace CLHEP 117 } // namespace CLHEP
136 118