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