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 register double scale=1.0/r1; 33 double ux = scale*axis.getX(); << 34 register double ux = scale*axis.getX(); 34 double uy = scale*axis.getY(); << 35 register double uy = scale*axis.getY(); 35 double uz = scale*axis.getZ(); << 36 register 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 register 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 { register 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 { register 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 { register 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 register double sinPhi = std::sin( phi1 ), cosPhi = std::cos( phi1 ); 80 double sinTheta = std::sin( theta1 ), cosThe << 83 register double sinTheta = std::sin( theta1 ), cosTheta1 = std::cos( theta1 ); 81 double sinPsi = std::sin( psi1 ), cosPsi << 84 register 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