CLHEP VERSION Reference Documentation
CLHEP Home Page CLHEP Documentation CLHEP Bug Reports |
00001 // -*- C++ -*- 00002 // --------------------------------------------------------------------------- 00003 // 00004 // This file is a part of the CLHEP - a Class Library for High Energy Physics. 00005 // 00006 // This is the implementation of methods of the HepRotation class which 00007 // were introduced when ZOOM PhysicsVectors was merged in, which involve 00008 // correcting user-supplied data which is supposed to form a Rotation, or 00009 // rectifying a rotation matrix which may have drifted due to roundoff. 00010 // 00011 00012 #ifdef GNUPRAGMA 00013 #pragma implementation 00014 #endif 00015 00016 #include "CLHEP/Vector/defs.h" 00017 #include "CLHEP/Vector/Rotation.h" 00018 #include "CLHEP/Vector/ZMxpv.h" 00019 00020 #include <cmath> 00021 00022 namespace CLHEP { 00023 00024 // --------- Helper methods (private) for setting from 3 columns: 00025 00026 bool HepRotation::setCols 00027 ( const Hep3Vector & u1, const Hep3Vector & u2, const Hep3Vector & u3, 00028 double u1u2, 00029 Hep3Vector & v1, Hep3Vector & v2, Hep3Vector & v3 ) const { 00030 00031 if ( (1-std::fabs(u1u2)) <= Hep4RotationInterface::tolerance ) { 00032 ZMthrowC (ZMxpvParallelCols( 00033 "All three cols supplied for a Rotation are parallel --" 00034 "\n an arbitrary rotation will be returned")); 00035 setArbitrarily (u1, v1, v2, v3); 00036 return true; 00037 } 00038 00039 v1 = u1; 00040 v2 = Hep3Vector(u2 - u1u2 * u1).unit(); 00041 v3 = v1.cross(v2); 00042 if ( v3.dot(u3) >= 0 ) { 00043 return true; 00044 } else { 00045 return false; // looks more like a reflection in this case! 00046 } 00047 00048 } // HepRotation::setCols 00049 00050 void HepRotation::setArbitrarily (const Hep3Vector & ccolX, 00051 Hep3Vector & v1, Hep3Vector & v2, Hep3Vector & v3) const { 00052 00053 // We have all three col's parallel. Warnings already been given; 00054 // this just supplies a result which is a valid rotation. 00055 00056 v1 = ccolX.unit(); 00057 v2 = v1.cross(Hep3Vector(0,0,1)); 00058 if (v2.mag2() != 0) { 00059 v2 = v2.unit(); 00060 } else { 00061 v2 = Hep3Vector(1,0,0); 00062 } 00063 v3 = v1.cross(v2); 00064 00065 return; 00066 00067 } // HepRotation::setArbitrarily 00068 00069 00070 00071 // ---------- Constructors and Assignment: 00072 00073 // 3 orthogonal columns or rows 00074 00075 HepRotation & HepRotation::set( const Hep3Vector & ccolX, 00076 const Hep3Vector & ccolY, 00077 const Hep3Vector & ccolZ ) { 00078 Hep3Vector ucolX = ccolX.unit(); 00079 Hep3Vector ucolY = ccolY.unit(); 00080 Hep3Vector ucolZ = ccolZ.unit(); 00081 00082 double u1u2 = ucolX.dot(ucolY); 00083 double f12 = std::fabs(u1u2); 00084 if ( f12 > Hep4RotationInterface::tolerance ) { 00085 ZMthrowC (ZMxpvNotOrthogonal( 00086 "col's X and Y supplied for Rotation are not close to orthogonal")); 00087 } 00088 double u1u3 = ucolX.dot(ucolZ); 00089 double f13 = std::fabs(u1u3); 00090 if ( f13 > Hep4RotationInterface::tolerance ) { 00091 ZMthrowC (ZMxpvNotOrthogonal( 00092 "col's X and Z supplied for Rotation are not close to orthogonal")); 00093 } 00094 double u2u3 = ucolY.dot(ucolZ); 00095 double f23 = std::fabs(u2u3); 00096 if ( f23 > Hep4RotationInterface::tolerance ) { 00097 ZMthrowC (ZMxpvNotOrthogonal( 00098 "col's Y and Z supplied for Rotation are not close to orthogonal")); 00099 } 00100 00101 Hep3Vector v1, v2, v3; 00102 bool isRotation; 00103 if ( (f12 <= f13) && (f12 <= f23) ) { 00104 isRotation = setCols ( ucolX, ucolY, ucolZ, u1u2, v1, v2, v3 ); 00105 if ( !isRotation ) { 00106 ZMthrowC (ZMxpvImproperRotation( 00107 "col's X Y and Z supplied form closer to a reflection than a Rotation " 00108 "\n col Z is set to col X cross col Y")); 00109 } 00110 } else if ( f13 <= f23 ) { 00111 isRotation = setCols ( ucolZ, ucolX, ucolY, u1u3, v3, v1, v2 ); 00112 if ( !isRotation ) { 00113 ZMthrowC (ZMxpvImproperRotation( 00114 "col's X Y and Z supplied form closer to a reflection than a Rotation " 00115 "\n col Y is set to col Z cross col X")); 00116 } 00117 } else { 00118 isRotation = setCols ( ucolY, ucolZ, ucolX, u2u3, v2, v3, v1 ); 00119 if ( !isRotation ) { 00120 ZMthrowC (ZMxpvImproperRotation( 00121 "col's X Y and Z supplied form closer to a reflection than a Rotation " 00122 "\n col X is set to col Y cross col Z")); 00123 } 00124 } 00125 00126 rxx = v1.x(); ryx = v1.y(); rzx = v1.z(); 00127 rxy = v2.x(); ryy = v2.y(); rzy = v2.z(); 00128 rxz = v3.x(); ryz = v3.y(); rzz = v3.z(); 00129 00130 return *this; 00131 00132 } // HepRotation::set(colX, colY, colZ) 00133 00134 HepRotation::HepRotation ( const Hep3Vector & ccolX, 00135 const Hep3Vector & ccolY, 00136 const Hep3Vector & ccolZ ) 00137 { 00138 set (ccolX, ccolY, ccolZ); 00139 } 00140 00141 HepRotation & HepRotation::setRows( const Hep3Vector & rrowX, 00142 const Hep3Vector & rrowY, 00143 const Hep3Vector & rrowZ ) { 00144 set (rrowX, rrowY, rrowZ); 00145 invert(); 00146 return *this; 00147 } 00148 00149 00150 // ------- Rectify a near-rotation 00151 00152 void HepRotation::rectify() { 00153 // Assuming the representation of this is close to a true Rotation, 00154 // but may have drifted due to round-off error from many operations, 00155 // this forms an "exact" orthonormal matrix for the rotation again. 00156 00157 // The first step is to average with the transposed inverse. This 00158 // will correct for small errors such as those occuring when decomposing 00159 // a LorentzTransformation. Then we take the bull by the horns and 00160 // formally extract the axis and delta (assuming the Rotation were true) 00161 // and re-setting the rotation according to those. 00162 00163 double det = rxx * ryy * rzz + 00164 rxy * ryz * rzx + 00165 rxz * ryx * rzy - 00166 rxx * ryz * rzy - 00167 rxy * ryx * rzz - 00168 rxz * ryy * rzx ; 00169 if (det <= 0) { 00170 ZMthrowA(ZMxpvImproperRotation( 00171 "Attempt to rectify a Rotation with determinant <= 0\n")); 00172 return; 00173 } 00174 double di = 1.0 / det; 00175 00176 // xx, xy, ... are components of inverse matrix: 00177 double xx1 = (ryy * rzz - ryz * rzy) * di; 00178 double xy1 = (rzy * rxz - rzz * rxy) * di; 00179 double xz1 = (rxy * ryz - rxz * ryy) * di; 00180 double yx1 = (ryz * rzx - ryx * rzz) * di; 00181 double yy1 = (rzz * rxx - rzx * rxz) * di; 00182 double yz1 = (rxz * ryx - rxx * ryz) * di; 00183 double zx1 = (ryx * rzy - ryy * rzx) * di; 00184 double zy1 = (rzx * rxy - rzy * rxx) * di; 00185 double zz1 = (rxx * ryy - rxy * ryx) * di; 00186 00187 // Now average with the TRANSPOSE of that: 00188 rxx = .5*(rxx + xx1); 00189 rxy = .5*(rxy + yx1); 00190 rxz = .5*(rxz + zx1); 00191 ryx = .5*(ryx + xy1); 00192 ryy = .5*(ryy + yy1); 00193 ryz = .5*(ryz + zy1); 00194 rzx = .5*(rzx + xz1); 00195 rzy = .5*(rzy + yz1); 00196 rzz = .5*(rzz + zz1); 00197 00198 // Now force feed this improved rotation 00199 double del = delta(); 00200 Hep3Vector u = axis(); 00201 u = u.unit(); // Because if the rotation is inexact, then the 00202 // axis() returned will not have length 1! 00203 set(u, del); 00204 00205 } // rectify() 00206 00207 } // namespace CLHEP 00208