| 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 the subset of 6 // This is the implementation of the subset of those methods of the Hep3Vector
7 // class which originated from the ZOOM SpaceV 7 // class which originated from the ZOOM SpaceVector class *and* which involve
8 // the concepts of rotation. 8 // the concepts of rotation.
9 // 9 //
10 10
11 #include "CLHEP/Vector/ThreeVector.h" 11 #include "CLHEP/Vector/ThreeVector.h"
12 #include "CLHEP/Vector/AxisAngle.h" 12 #include "CLHEP/Vector/AxisAngle.h"
13 #include "CLHEP/Vector/EulerAngles.h" 13 #include "CLHEP/Vector/EulerAngles.h"
14 14
15 #include <cmath> 15 #include <cmath>
16 #include <iostream> 16 #include <iostream>
17 17
18 namespace CLHEP { 18 namespace CLHEP {
19 19
20 //-************************ 20 //-************************
21 // rotate about axis 21 // rotate about axis
22 //-************************ 22 //-************************
23 23
24 Hep3Vector & Hep3Vector::rotate (const Hep3Vec 24 Hep3Vector & Hep3Vector::rotate (const Hep3Vector & axis,
25 double ddelta) { 25 double ddelta) {
26 double r1 = axis.mag(); 26 double r1 = axis.mag();
27 if ( r1 == 0 ) { 27 if ( r1 == 0 ) {
28 std::cerr << "Hep3Vector::rotate() - " 28 std::cerr << "Hep3Vector::rotate() - "
29 << "Attempt to rotate around a zero vect 29 << "Attempt to rotate around a zero vector axis! " << std::endl;
30 return *this; 30 return *this;
31 } 31 }
32 double scale=1.0/r1; 32 double scale=1.0/r1;
33 double ux = scale*axis.getX(); 33 double ux = scale*axis.getX();
34 double uy = scale*axis.getY(); 34 double uy = scale*axis.getY();
35 double uz = scale*axis.getZ(); 35 double uz = scale*axis.getZ();
36 double cd = std::cos(ddelta); 36 double cd = std::cos(ddelta);
37 double sd = std::sin(ddelta); 37 double sd = std::sin(ddelta);
38 double ocd = 1 - cd; 38 double ocd = 1 - cd;
39 double rx; 39 double rx;
40 double ry; 40 double ry;
41 double rz; 41 double rz;
42 42
43 { double ocdux = ocd * ux; 43 { double ocdux = ocd * ux;
44 rx = x() * ( cd + ocdux * ux ) + 44 rx = x() * ( cd + ocdux * ux ) +
45 y() * ( ocdux * uy - sd * uz ) + 45 y() * ( ocdux * uy - sd * uz ) +
46 z() * ( ocdux * uz + sd * uy ) ; 46 z() * ( ocdux * uz + sd * uy ) ;
47 } 47 }
48 48
49 { double ocduy = ocd * uy; 49 { double ocduy = ocd * uy;
50 ry = y() * ( cd + ocduy * uy ) + 50 ry = y() * ( cd + ocduy * uy ) +
51 z() * ( ocduy * uz - sd * ux ) + 51 z() * ( ocduy * uz - sd * ux ) +
52 x() * ( ocduy * ux + sd * uz ) ; 52 x() * ( ocduy * ux + sd * uz ) ;
53 } 53 }
54 54
55 { double ocduz = ocd * uz; 55 { double ocduz = ocd * uz;
56 rz = z() * ( cd + ocduz * uz ) + 56 rz = z() * ( cd + ocduz * uz ) +
57 x() * ( ocduz * ux - sd * uy ) + 57 x() * ( ocduz * ux - sd * uy ) +
58 y() * ( ocduz * uy + sd * ux ) ; 58 y() * ( ocduz * uy + sd * ux ) ;
59 } 59 }
60 60
61 set(rx, ry, rz); 61 set(rx, ry, rz);
62 return *this; 62 return *this;
63 } /* rotate */ 63 } /* rotate */
64 64
65 65
66 //-**************************** 66 //-****************************
67 // rotate by three euler angles 67 // rotate by three euler angles
68 //-**************************** 68 //-****************************
69 69
70 70
71 Hep3Vector & Hep3Vector::rotate (double phi1, 71 Hep3Vector & Hep3Vector::rotate (double phi1,
72 double theta1, 72 double theta1,
73 double psi1) { 73 double psi1) {
74 74
75 double rx; 75 double rx;
76 double ry; 76 double ry;
77 double rz; 77 double rz;
78 78
79 double sinPhi = std::sin( phi1 ), cosPhi 79 double sinPhi = std::sin( phi1 ), cosPhi = std::cos( phi1 );
80 double sinTheta = std::sin( theta1 ), cosThe 80 double sinTheta = std::sin( theta1 ), cosTheta1 = std::cos( theta1 );
81 double sinPsi = std::sin( psi1 ), cosPsi 81 double sinPsi = std::sin( psi1 ), cosPsi = std::cos( psi1 );
82 82
83 rx = (cosPsi * cosPhi - cosTheta1 * sinPs 83 rx = (cosPsi * cosPhi - cosTheta1 * sinPsi * sinPhi) * x() +
84 (cosPsi * sinPhi + cosTheta1 * sinPsi * co 84 (cosPsi * sinPhi + cosTheta1 * sinPsi * cosPhi) * y() +
85 (sinPsi * sinTheta) * z() ; 85 (sinPsi * sinTheta) * z() ;
86 86
87 ry = (- sinPsi * cosPhi - cosTheta1 * cosPs 87 ry = (- sinPsi * cosPhi - cosTheta1 * cosPsi * sinPhi) * x() +
88 (- sinPsi * sinPhi + cosTheta1 * cosPsi * co 88 (- sinPsi * sinPhi + cosTheta1 * cosPsi * cosPhi) * y() +
89 (cosPsi * sinTheta) * z() ; 89 (cosPsi * sinTheta) * z() ;
90 90
91 rz = (sinTheta * sinPhi) * x() + 91 rz = (sinTheta * sinPhi) * x() +
92 (- sinTheta * cosPhi) * y() + 92 (- sinTheta * cosPhi) * y() +
93 (cosTheta1) * z() ; 93 (cosTheta1) * z() ;
94 94
95 set(rx, ry, rz); 95 set(rx, ry, rz);
96 return *this; 96 return *this;
97 97
98 } /* rotate */ 98 } /* rotate */
99 99
100 100
101 //-******************* 101 //-*******************
102 // rotate(HepAxisAngle) 102 // rotate(HepAxisAngle)
103 // rotate(HepEulerAngles) 103 // rotate(HepEulerAngles)
104 //-******************* 104 //-*******************
105 105
106 Hep3Vector & Hep3Vector::rotate (const HepAxis 106 Hep3Vector & Hep3Vector::rotate (const HepAxisAngle & ax ) {
107 return rotate( ax.getAxis(), ax.delta() ); 107 return rotate( ax.getAxis(), ax.delta() );
108 } 108 }
109 109
110 Hep3Vector & Hep3Vector::rotate (const HepEule 110 Hep3Vector & Hep3Vector::rotate (const HepEulerAngles & ex ) {
111 return rotate( ex.phi(), ex.theta(), ex.psi( 111 return rotate( ex.phi(), ex.theta(), ex.psi() );
112 } 112 }
113 113
114 114
115 //-*********************** 115 //-***********************
116 // rotationOf(HepAxisAngle) 116 // rotationOf(HepAxisAngle)
117 // rotationOf(HepEulerAngles) 117 // rotationOf(HepEulerAngles)
118 // and coordinate axis rotations 118 // and coordinate axis rotations
119 //-*********************** 119 //-***********************
120 120
121 Hep3Vector rotationOf (const Hep3Vector & vec, 121 Hep3Vector rotationOf (const Hep3Vector & vec, const HepAxisAngle & ax) {
122 Hep3Vector vv(vec); 122 Hep3Vector vv(vec);
123 return vv.rotate (ax); 123 return vv.rotate (ax);
124 } 124 }
125 125
126 Hep3Vector rotationOf (const Hep3Vector & vec, 126 Hep3Vector rotationOf (const Hep3Vector & vec,
127 const Hep3Vector & axis 127 const Hep3Vector & axis, double ddelta) {
128 Hep3Vector vv(vec); 128 Hep3Vector vv(vec);
129 return vv.rotate(axis, ddelta); 129 return vv.rotate(axis, ddelta);
130 } 130 }
131 131
132 Hep3Vector rotationOf (const Hep3Vector & vec, 132 Hep3Vector rotationOf (const Hep3Vector & vec, const HepEulerAngles & ex) {
133 Hep3Vector vv(vec); 133 Hep3Vector vv(vec);
134 return vv.rotate (ex); 134 return vv.rotate (ex);
135 } 135 }
136 136
137 Hep3Vector rotationOf (const Hep3Vector & vec, 137 Hep3Vector rotationOf (const Hep3Vector & vec,
138 double phi, double thet 138 double phi, double theta, double psi) {
139 Hep3Vector vv(vec); 139 Hep3Vector vv(vec);
140 return vv.rotate(phi, theta, psi); 140 return vv.rotate(phi, theta, psi);
141 } 141 }
142 142
143 Hep3Vector rotationXOf (const Hep3Vector & vec 143 Hep3Vector rotationXOf (const Hep3Vector & vec, double ddelta) {
144 Hep3Vector vv(vec); 144 Hep3Vector vv(vec);
145 return vv.rotateX (ddelta); 145 return vv.rotateX (ddelta);
146 } 146 }
147 147
148 Hep3Vector rotationYOf (const Hep3Vector & vec 148 Hep3Vector rotationYOf (const Hep3Vector & vec, double ddelta) {
149 Hep3Vector vv(vec); 149 Hep3Vector vv(vec);
150 return vv.rotateY (ddelta); 150 return vv.rotateY (ddelta);
151 } 151 }
152 152
153 Hep3Vector rotationZOf (const Hep3Vector & vec 153 Hep3Vector rotationZOf (const Hep3Vector & vec, double ddelta) {
154 Hep3Vector vv(vec); 154 Hep3Vector vv(vec);
155 return vv.rotateZ (ddelta); 155 return vv.rotateZ (ddelta);
156 } 156 }
157 157
158 } // namespace CLHEP 158 } // namespace CLHEP
159 159