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 methods of th 6 // This is the implementation of methods of the HepRotation class which 7 // were introduced when ZOOM PhysicsVectors wa 7 // were introduced when ZOOM PhysicsVectors was merged in, which involve 8 // correcting user-supplied data which is supp 8 // correcting user-supplied data which is supposed to form a Rotation, or 9 // rectifying a rotation matrix which may have 9 // rectifying a rotation matrix which may have drifted due to roundoff. 10 // 10 // 11 11 >> 12 #ifdef GNUPRAGMA >> 13 #pragma implementation >> 14 #endif >> 15 12 #include "CLHEP/Vector/Rotation.h" 16 #include "CLHEP/Vector/Rotation.h" 13 17 14 #include <cmath> 18 #include <cmath> 15 #include <iostream> << 16 19 17 namespace CLHEP { 20 namespace CLHEP { 18 21 19 // --------- Helper methods (private) for sett 22 // --------- Helper methods (private) for setting from 3 columns: 20 23 21 bool HepRotation::setCols 24 bool HepRotation::setCols 22 ( const Hep3Vector & u1, const Hep3Vector 25 ( const Hep3Vector & u1, const Hep3Vector & u2, const Hep3Vector & u3, 23 double u1u2, 26 double u1u2, 24 Hep3Vector & v1, Hep3Vector & v2, Hep3Ve 27 Hep3Vector & v1, Hep3Vector & v2, Hep3Vector & v3 ) const { 25 28 26 if ( (1-std::fabs(u1u2)) <= Hep4RotationInte 29 if ( (1-std::fabs(u1u2)) <= Hep4RotationInterface::tolerance ) { 27 std::cerr << "HepRotation::setCols() - " 30 std::cerr << "HepRotation::setCols() - " 28 << "All three cols supplied for a Rotati 31 << "All three cols supplied for a Rotation are parallel --" 29 << "\n an arbitrary rotation will be 32 << "\n an arbitrary rotation will be returned" << std::endl; 30 setArbitrarily (u1, v1, v2, v3); 33 setArbitrarily (u1, v1, v2, v3); 31 return true; 34 return true; 32 } 35 } 33 36 34 v1 = u1; 37 v1 = u1; 35 v2 = Hep3Vector(u2 - u1u2 * u1).unit(); 38 v2 = Hep3Vector(u2 - u1u2 * u1).unit(); 36 v3 = v1.cross(v2); 39 v3 = v1.cross(v2); 37 if ( v3.dot(u3) >= 0 ) { 40 if ( v3.dot(u3) >= 0 ) { 38 return true; 41 return true; 39 } else { 42 } else { 40 return false; // looks more like a reflect 43 return false; // looks more like a reflection in this case! 41 } 44 } 42 45 43 } // HepRotation::setCols 46 } // HepRotation::setCols 44 47 45 void HepRotation::setArbitrarily (const Hep3Ve 48 void HepRotation::setArbitrarily (const Hep3Vector & ccolX, 46 Hep3Vector & v1, Hep3Vector & v2, Hep3Vecto 49 Hep3Vector & v1, Hep3Vector & v2, Hep3Vector & v3) const { 47 50 48 // We have all three col's parallel. Warnin 51 // We have all three col's parallel. Warnings already been given; 49 // this just supplies a result which is a va 52 // this just supplies a result which is a valid rotation. 50 53 51 v1 = ccolX.unit(); 54 v1 = ccolX.unit(); 52 v2 = v1.cross(Hep3Vector(0,0,1)); 55 v2 = v1.cross(Hep3Vector(0,0,1)); 53 if (v2.mag2() != 0) { 56 if (v2.mag2() != 0) { 54 v2 = v2.unit(); 57 v2 = v2.unit(); 55 } else { 58 } else { 56 v2 = Hep3Vector(1,0,0); 59 v2 = Hep3Vector(1,0,0); 57 } 60 } 58 v3 = v1.cross(v2); 61 v3 = v1.cross(v2); 59 62 60 return; 63 return; 61 64 62 } // HepRotation::setArbitrarily 65 } // HepRotation::setArbitrarily 63 66 64 67 65 68 66 // ---------- Constructors and Assignment: 69 // ---------- Constructors and Assignment: 67 70 68 // 3 orthogonal columns or rows 71 // 3 orthogonal columns or rows 69 72 70 HepRotation & HepRotation::set( const Hep3Vect 73 HepRotation & HepRotation::set( const Hep3Vector & ccolX, 71 const Hep3Vector 74 const Hep3Vector & ccolY, 72 const Hep3Vector & 75 const Hep3Vector & ccolZ ) { 73 Hep3Vector ucolX = ccolX.unit(); 76 Hep3Vector ucolX = ccolX.unit(); 74 Hep3Vector ucolY = ccolY.unit(); 77 Hep3Vector ucolY = ccolY.unit(); 75 Hep3Vector ucolZ = ccolZ.unit(); 78 Hep3Vector ucolZ = ccolZ.unit(); 76 79 77 double u1u2 = ucolX.dot(ucolY); 80 double u1u2 = ucolX.dot(ucolY); 78 double f12 = std::fabs(u1u2); 81 double f12 = std::fabs(u1u2); 79 if ( f12 > Hep4RotationInterface::tolerance 82 if ( f12 > Hep4RotationInterface::tolerance ) { 80 std::cerr << "HepRotation::set() - " 83 std::cerr << "HepRotation::set() - " 81 << "col's X and Y supplied for Rotation 84 << "col's X and Y supplied for Rotation are not close to orthogonal" 82 << std::endl; 85 << std::endl; 83 } 86 } 84 double u1u3 = ucolX.dot(ucolZ); 87 double u1u3 = ucolX.dot(ucolZ); 85 double f13 = std::fabs(u1u3); 88 double f13 = std::fabs(u1u3); 86 if ( f13 > Hep4RotationInterface::tolerance 89 if ( f13 > Hep4RotationInterface::tolerance ) { 87 std::cerr << "HepRotation::set() - " 90 std::cerr << "HepRotation::set() - " 88 << "col's X and Z supplied for Rotation 91 << "col's X and Z supplied for Rotation are not close to orthogonal" 89 << std::endl; 92 << std::endl; 90 } 93 } 91 double u2u3 = ucolY.dot(ucolZ); 94 double u2u3 = ucolY.dot(ucolZ); 92 double f23 = std::fabs(u2u3); 95 double f23 = std::fabs(u2u3); 93 if ( f23 > Hep4RotationInterface::tolerance 96 if ( f23 > Hep4RotationInterface::tolerance ) { 94 std::cerr << "HepRotation::set() - " 97 std::cerr << "HepRotation::set() - " 95 << "col's Y and Z supplied for Rotation 98 << "col's Y and Z supplied for Rotation are not close to orthogonal" 96 << std::endl; 99 << std::endl; 97 } 100 } 98 101 99 Hep3Vector v1, v2, v3; 102 Hep3Vector v1, v2, v3; 100 bool isRotation; 103 bool isRotation; 101 if ( (f12 <= f13) && (f12 <= f23) ) { 104 if ( (f12 <= f13) && (f12 <= f23) ) { 102 isRotation = setCols ( ucolX, ucolY, ucolZ 105 isRotation = setCols ( ucolX, ucolY, ucolZ, u1u2, v1, v2, v3 ); 103 if ( !isRotation ) { 106 if ( !isRotation ) { 104 std::cerr << "HepRotation::set() - " 107 std::cerr << "HepRotation::set() - " 105 << "col's X Y and Z supplied form clos 108 << "col's X Y and Z supplied form closer to a reflection than a Rotation " 106 << "\n col Z is set to col X cross 109 << "\n col Z is set to col X cross col Y" << std::endl; 107 } 110 } 108 } else if ( f13 <= f23 ) { 111 } else if ( f13 <= f23 ) { 109 isRotation = setCols ( ucolZ, ucolX, ucolY 112 isRotation = setCols ( ucolZ, ucolX, ucolY, u1u3, v3, v1, v2 ); 110 if ( !isRotation ) { 113 if ( !isRotation ) { 111 std::cerr << "HepRotation::set() - " 114 std::cerr << "HepRotation::set() - " 112 << "col's X Y and Z supplied form clos 115 << "col's X Y and Z supplied form closer to a reflection than a Rotation " 113 << "\n col Y is set to col Z cross 116 << "\n col Y is set to col Z cross col X" << std::endl; 114 } 117 } 115 } else { 118 } else { 116 isRotation = setCols ( ucolY, ucolZ, ucolX 119 isRotation = setCols ( ucolY, ucolZ, ucolX, u2u3, v2, v3, v1 ); 117 if ( !isRotation ) { 120 if ( !isRotation ) { 118 std::cerr << "HepRotation::set() - " 121 std::cerr << "HepRotation::set() - " 119 << "col's X Y and Z supplied form clos 122 << "col's X Y and Z supplied form closer to a reflection than a Rotation " 120 << "\n col X is set to col Y cross 123 << "\n col X is set to col Y cross col Z" << std::endl; 121 } 124 } 122 } 125 } 123 126 124 rxx = v1.x(); ryx = v1.y(); rzx = v1.z(); 127 rxx = v1.x(); ryx = v1.y(); rzx = v1.z(); 125 rxy = v2.x(); ryy = v2.y(); rzy = v2.z(); 128 rxy = v2.x(); ryy = v2.y(); rzy = v2.z(); 126 rxz = v3.x(); ryz = v3.y(); rzz = v3.z(); 129 rxz = v3.x(); ryz = v3.y(); rzz = v3.z(); 127 130 128 return *this; 131 return *this; 129 132 130 } // HepRotation::set(colX, colY, colZ) 133 } // HepRotation::set(colX, colY, colZ) 131 134 132 HepRotation::HepRotation ( const Hep3Vector & 135 HepRotation::HepRotation ( const Hep3Vector & ccolX, 133 const Hep3Vector & ccolY, 136 const Hep3Vector & ccolY, 134 const Hep3Vector & ccolZ ) 137 const Hep3Vector & ccolZ ) 135 { 138 { 136 set (ccolX, ccolY, ccolZ); 139 set (ccolX, ccolY, ccolZ); 137 } 140 } 138 141 139 HepRotation & HepRotation::setRows( const Hep3 142 HepRotation & HepRotation::setRows( const Hep3Vector & rrowX, 140 const Hep3Vect 143 const Hep3Vector & rrowY, 141 const Hep3 144 const Hep3Vector & rrowZ ) { 142 set (rrowX, rrowY, rrowZ); 145 set (rrowX, rrowY, rrowZ); 143 invert(); 146 invert(); 144 return *this; 147 return *this; 145 } 148 } 146 149 147 // ------- Rectify a near-rotation 150 // ------- Rectify a near-rotation 148 151 149 void HepRotation::rectify() { 152 void HepRotation::rectify() { 150 // Assuming the representation of this is cl 153 // Assuming the representation of this is close to a true Rotation, 151 // but may have drifted due to round-off err 154 // but may have drifted due to round-off error from many operations, 152 // this forms an "exact" orthonormal matrix 155 // this forms an "exact" orthonormal matrix for the rotation again. 153 156 154 // The first step is to average with the tra 157 // The first step is to average with the transposed inverse. This 155 // will correct for small errors such as tho 158 // will correct for small errors such as those occuring when decomposing 156 // a LorentzTransformation. Then we take th 159 // a LorentzTransformation. Then we take the bull by the horns and 157 // formally extract the axis and delta (assu 160 // formally extract the axis and delta (assuming the Rotation were true) 158 // and re-setting the rotation according to 161 // and re-setting the rotation according to those. 159 162 160 double det = rxx * ryy * rzz + 163 double det = rxx * ryy * rzz + 161 rxy * ryz * rzx + 164 rxy * ryz * rzx + 162 rxz * ryx * rzy - 165 rxz * ryx * rzy - 163 rxx * ryz * rzy - 166 rxx * ryz * rzy - 164 rxy * ryx * rzz - 167 rxy * ryx * rzz - 165 rxz * ryy * rzx ; 168 rxz * ryy * rzx ; 166 if (det <= 0) { 169 if (det <= 0) { 167 std::cerr << "HepRotation::rectify() - " 170 std::cerr << "HepRotation::rectify() - " 168 << "Attempt to rectify a Rotation with 171 << "Attempt to rectify a Rotation with determinant <= 0" << std::endl; 169 return; 172 return; 170 } 173 } 171 double di = 1.0 / det; 174 double di = 1.0 / det; 172 175 173 // xx, xy, ... are components of inverse mat 176 // xx, xy, ... are components of inverse matrix: 174 double xx1 = (ryy * rzz - ryz * rzy) * di; 177 double xx1 = (ryy * rzz - ryz * rzy) * di; 175 double xy1 = (rzy * rxz - rzz * rxy) * di; 178 double xy1 = (rzy * rxz - rzz * rxy) * di; 176 double xz1 = (rxy * ryz - rxz * ryy) * di; 179 double xz1 = (rxy * ryz - rxz * ryy) * di; 177 double yx1 = (ryz * rzx - ryx * rzz) * di; 180 double yx1 = (ryz * rzx - ryx * rzz) * di; 178 double yy1 = (rzz * rxx - rzx * rxz) * di; 181 double yy1 = (rzz * rxx - rzx * rxz) * di; 179 double yz1 = (rxz * ryx - rxx * ryz) * di; 182 double yz1 = (rxz * ryx - rxx * ryz) * di; 180 double zx1 = (ryx * rzy - ryy * rzx) * di; 183 double zx1 = (ryx * rzy - ryy * rzx) * di; 181 double zy1 = (rzx * rxy - rzy * rxx) * di; 184 double zy1 = (rzx * rxy - rzy * rxx) * di; 182 double zz1 = (rxx * ryy - rxy * ryx) * di; 185 double zz1 = (rxx * ryy - rxy * ryx) * di; 183 186 184 // Now average with the TRANSPOSE of that: 187 // Now average with the TRANSPOSE of that: 185 rxx = .5*(rxx + xx1); 188 rxx = .5*(rxx + xx1); 186 rxy = .5*(rxy + yx1); 189 rxy = .5*(rxy + yx1); 187 rxz = .5*(rxz + zx1); 190 rxz = .5*(rxz + zx1); 188 ryx = .5*(ryx + xy1); 191 ryx = .5*(ryx + xy1); 189 ryy = .5*(ryy + yy1); 192 ryy = .5*(ryy + yy1); 190 ryz = .5*(ryz + zy1); 193 ryz = .5*(ryz + zy1); 191 rzx = .5*(rzx + xz1); 194 rzx = .5*(rzx + xz1); 192 rzy = .5*(rzy + yz1); 195 rzy = .5*(rzy + yz1); 193 rzz = .5*(rzz + zz1); 196 rzz = .5*(rzz + zz1); 194 197 195 // Now force feed this improved rotation 198 // Now force feed this improved rotation 196 double del = delta(); 199 double del = delta(); 197 Hep3Vector u = axis(); 200 Hep3Vector u = axis(); 198 u = u.unit(); // Because if the rotation is 201 u = u.unit(); // Because if the rotation is inexact, then the 199 // axis() returned will not ha 202 // axis() returned will not have length 1! 200 set(u, del); 203 set(u, del); 201 204 202 } // rectify() 205 } // rectify() 203 206 204 } // namespace CLHEP 207 } // namespace CLHEP 205 208 206 209