Geant4 Cross Reference |
1 // -*- C++ -*- 1 2 // ------------------------------------------- 3 // 4 // This file is a part of the CLHEP - a Class 5 // 6 // This is the implementation of the subset of 7 // class which originated from the ZOOM SpaceV 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 Hep3Vec 25 double ddelta) { 26 double r1 = axis.mag(); 27 if ( r1 == 0 ) { 28 std::cerr << "Hep3Vector::rotate() - " 29 << "Attempt to rotate around a zero vect 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 80 double sinTheta = std::sin( theta1 ), cosThe 81 double sinPsi = std::sin( psi1 ), cosPsi 82 83 rx = (cosPsi * cosPhi - cosTheta1 * sinPs 84 (cosPsi * sinPhi + cosTheta1 * sinPsi * co 85 (sinPsi * sinTheta) * z() ; 86 87 ry = (- sinPsi * cosPhi - cosTheta1 * cosPs 88 (- sinPsi * sinPhi + cosTheta1 * cosPsi * co 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 HepAxis 107 return rotate( ax.getAxis(), ax.delta() ); 108 } 109 110 Hep3Vector & Hep3Vector::rotate (const HepEule 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, 122 Hep3Vector vv(vec); 123 return vv.rotate (ax); 124 } 125 126 Hep3Vector rotationOf (const Hep3Vector & vec, 127 const Hep3Vector & axis 128 Hep3Vector vv(vec); 129 return vv.rotate(axis, ddelta); 130 } 131 132 Hep3Vector rotationOf (const Hep3Vector & vec, 133 Hep3Vector vv(vec); 134 return vv.rotate (ex); 135 } 136 137 Hep3Vector rotationOf (const Hep3Vector & vec, 138 double phi, double thet 139 Hep3Vector vv(vec); 140 return vv.rotate(phi, theta, psi); 141 } 142 143 Hep3Vector rotationXOf (const Hep3Vector & vec 144 Hep3Vector vv(vec); 145 return vv.rotateX (ddelta); 146 } 147 148 Hep3Vector rotationYOf (const Hep3Vector & vec 149 Hep3Vector vv(vec); 150 return vv.rotateY (ddelta); 151 } 152 153 Hep3Vector rotationZOf (const Hep3Vector & vec 154 Hep3Vector vv(vec); 155 return vv.rotateZ (ddelta); 156 } 157 158 } // namespace CLHEP 159