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