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