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 those parts o 6 // This is the implementation of those parts of the HepLorentzRotation class 7 // which involve decomposition into Boost*Rota 7 // which involve decomposition into Boost*Rotation. 8 8 9 #include "CLHEP/Vector/LorentzRotation.h" << 9 #ifdef GNUPRAGMA >> 10 #pragma implementation >> 11 #endif 10 12 11 #include <cmath> << 13 #include "CLHEP/Vector/LorentzRotation.h" 12 #include <iostream> << 13 14 14 namespace CLHEP { 15 namespace CLHEP { 15 16 16 // ---------- Decomposition: 17 // ---------- Decomposition: 17 18 18 void HepLorentzRotation::decompose 19 void HepLorentzRotation::decompose 19 (HepBoost & bboost, HepRotation & rotation) 20 (HepBoost & bboost, HepRotation & rotation) const { 20 21 21 // The boost will be the pure boost based on 22 // The boost will be the pure boost based on column 4 of the transformation 22 // matrix. Since the constructor takes the 23 // matrix. Since the constructor takes the beta vector, and not beta*gamma, 23 // we first divide through by gamma = the tt 24 // we first divide through by gamma = the tt element. This of course can 24 // never be zero since the last row has t**2 25 // never be zero since the last row has t**2 - v**2 = +1. 25 26 26 Hep3Vector betaVec ( xt(), yt(), zt() ); 27 Hep3Vector betaVec ( xt(), yt(), zt() ); 27 betaVec *= 1.0 / tt(); 28 betaVec *= 1.0 / tt(); 28 bboost.set( betaVec ); 29 bboost.set( betaVec ); 29 30 30 // The rotation will be inverse of B times T 31 // The rotation will be inverse of B times T. 31 32 32 HepBoost B( -betaVec ); 33 HepBoost B( -betaVec ); 33 HepLorentzRotation R( B * *this ); 34 HepLorentzRotation R( B * *this ); 34 35 35 HepRep3x3 m1 ( R.xx(), R.xy(), R.xz(), 36 HepRep3x3 m1 ( R.xx(), R.xy(), R.xz(), 36 R.yx(), R.yy(), R.yz(), 37 R.yx(), R.yy(), R.yz(), 37 R.zx(), R.zy(), R.zz() ); 38 R.zx(), R.zy(), R.zz() ); 38 rotation.set( m1 ); 39 rotation.set( m1 ); 39 rotation.rectify(); 40 rotation.rectify(); 40 41 41 return; 42 return; 42 43 43 } 44 } 44 45 45 void HepLorentzRotation::decompose 46 void HepLorentzRotation::decompose 46 (Hep3Vector & bboost, HepAxisAngle & rotatio 47 (Hep3Vector & bboost, HepAxisAngle & rotation) const { 47 HepRotation r; 48 HepRotation r; 48 HepBoost b; 49 HepBoost b; 49 decompose(b,r); 50 decompose(b,r); 50 bboost = b.boostVector(); 51 bboost = b.boostVector(); 51 rotation = r.axisAngle(); 52 rotation = r.axisAngle(); 52 return; 53 return; 53 } 54 } 54 55 55 void HepLorentzRotation::decompose 56 void HepLorentzRotation::decompose 56 (HepRotation & rotation, HepBoost & bboost) 57 (HepRotation & rotation, HepBoost & bboost) const { 57 58 58 // In this case the pure boost is based on r 59 // In this case the pure boost is based on row 4 of the matrix. 59 60 60 Hep3Vector betaVec( tx(), ty(), tz() ); 61 Hep3Vector betaVec( tx(), ty(), tz() ); 61 betaVec *= 1.0 / tt(); 62 betaVec *= 1.0 / tt(); 62 bboost.set( betaVec ); 63 bboost.set( betaVec ); 63 64 64 // The rotation will be T times the inverse 65 // The rotation will be T times the inverse of B. 65 66 66 HepBoost B( -betaVec ); 67 HepBoost B( -betaVec ); 67 HepLorentzRotation R( *this * B ); 68 HepLorentzRotation R( *this * B ); 68 69 69 HepRep3x3 m1 ( R.xx(), R.xy(), R.xz(), 70 HepRep3x3 m1 ( R.xx(), R.xy(), R.xz(), 70 R.yx(), R.yy(), R.yz(), 71 R.yx(), R.yy(), R.yz(), 71 R.zx(), R.zy(), R.zz() ); 72 R.zx(), R.zy(), R.zz() ); 72 rotation.set( m1 ); 73 rotation.set( m1 ); 73 rotation.rectify(); 74 rotation.rectify(); 74 return; 75 return; 75 76 76 } 77 } 77 78 78 void HepLorentzRotation::decompose 79 void HepLorentzRotation::decompose 79 (HepAxisAngle & rotation, Hep3Vector & bboos 80 (HepAxisAngle & rotation, Hep3Vector & bboost) const { 80 HepRotation r; 81 HepRotation r; 81 HepBoost b; 82 HepBoost b; 82 decompose(r,b); 83 decompose(r,b); 83 rotation = r.axisAngle(); 84 rotation = r.axisAngle(); 84 bboost = b.boostVector(); 85 bboost = b.boostVector(); 85 return; 86 return; 86 } 87 } 87 88 88 double HepLorentzRotation::distance2( const He 89 double HepLorentzRotation::distance2( const HepBoost & b ) const { 89 HepBoost b1; 90 HepBoost b1; 90 HepRotation r1; 91 HepRotation r1; 91 decompose( b1, r1 ); 92 decompose( b1, r1 ); 92 double db2 = b1.distance2( b ); 93 double db2 = b1.distance2( b ); 93 double dr2 = r1.norm2(); 94 double dr2 = r1.norm2(); 94 return ( db2 + dr2 ); 95 return ( db2 + dr2 ); 95 } 96 } 96 97 97 double HepLorentzRotation::distance2( const He 98 double HepLorentzRotation::distance2( const HepRotation & r ) const { 98 HepBoost b1; 99 HepBoost b1; 99 HepRotation r1; 100 HepRotation r1; 100 decompose( b1, r1 ); 101 decompose( b1, r1 ); 101 double db2 = b1.norm2( ); 102 double db2 = b1.norm2( ); 102 double dr2 = r1.distance2( r ); 103 double dr2 = r1.distance2( r ); 103 return ( db2 + dr2 ); 104 return ( db2 + dr2 ); 104 } 105 } 105 106 106 double HepLorentzRotation::distance2( 107 double HepLorentzRotation::distance2( 107 const HepLorentzRotation & lt ) co 108 const HepLorentzRotation & lt ) const { 108 HepBoost b1; 109 HepBoost b1; 109 HepRotation r1; 110 HepRotation r1; 110 decompose( b1, r1 ); 111 decompose( b1, r1 ); 111 HepBoost b2; 112 HepBoost b2; 112 HepRotation r2; 113 HepRotation r2; 113 lt.decompose (b2, r2); 114 lt.decompose (b2, r2); 114 double db2 = b1.distance2( b2 ); 115 double db2 = b1.distance2( b2 ); 115 double dr2 = r1.distance2( r2 ); 116 double dr2 = r1.distance2( r2 ); 116 return ( db2 + dr2 ); 117 return ( db2 + dr2 ); 117 } 118 } 118 119 119 double HepLorentzRotation::howNear( const HepB 120 double HepLorentzRotation::howNear( const HepBoost & b ) const { 120 return std::sqrt( distance2( b ) ); 121 return std::sqrt( distance2( b ) ); 121 } 122 } 122 double HepLorentzRotation::howNear( const HepR 123 double HepLorentzRotation::howNear( const HepRotation & r ) const { 123 return std::sqrt( distance2( r ) ); 124 return std::sqrt( distance2( r ) ); 124 } 125 } 125 double HepLorentzRotation::howNear( const HepL 126 double HepLorentzRotation::howNear( const HepLorentzRotation & lt )const { 126 return std::sqrt( distance2( lt ) ); 127 return std::sqrt( distance2( lt ) ); 127 } 128 } 128 129 129 bool HepLorentzRotation::isNear( 130 bool HepLorentzRotation::isNear( 130 const HepBoost & b, double epsilon ) const 131 const HepBoost & b, double epsilon ) const { 131 HepBoost b1; 132 HepBoost b1; 132 HepRotation r1; 133 HepRotation r1; 133 decompose( b1, r1 ); 134 decompose( b1, r1 ); 134 double db2 = b1.distance2(b); 135 double db2 = b1.distance2(b); 135 if ( db2 > epsilon*epsilon ) { 136 if ( db2 > epsilon*epsilon ) { 136 return false; // Saves the time-con 137 return false; // Saves the time-consuming Rotation::norm2 137 } 138 } 138 double dr2 = r1.norm2(); 139 double dr2 = r1.norm2(); 139 return ( (db2 + dr2) <= epsilon*epsilon ); 140 return ( (db2 + dr2) <= epsilon*epsilon ); 140 } 141 } 141 142 142 bool HepLorentzRotation::isNear( 143 bool HepLorentzRotation::isNear( 143 const HepRotation & r, double epsilon ) co 144 const HepRotation & r, double epsilon ) const { 144 HepBoost b1; 145 HepBoost b1; 145 HepRotation r1; 146 HepRotation r1; 146 decompose( b1, r1 ); 147 decompose( b1, r1 ); 147 double db2 = b1.norm2(); 148 double db2 = b1.norm2(); 148 if ( db2 > epsilon*epsilon ) { 149 if ( db2 > epsilon*epsilon ) { 149 return false; // Saves the time-con 150 return false; // Saves the time-consuming Rotation::distance2 150 } 151 } 151 double dr2 = r1.distance2(r); 152 double dr2 = r1.distance2(r); 152 return ( (db2 + dr2) <= epsilon*epsilon ); 153 return ( (db2 + dr2) <= epsilon*epsilon ); 153 } 154 } 154 155 155 bool HepLorentzRotation::isNear( 156 bool HepLorentzRotation::isNear( 156 const HepLorentzRotation & lt, double epsi 157 const HepLorentzRotation & lt, double epsilon ) const { 157 HepBoost b1; 158 HepBoost b1; 158 HepRotation r1; 159 HepRotation r1; 159 decompose( b1, r1 ); 160 decompose( b1, r1 ); 160 HepBoost b2; 161 HepBoost b2; 161 HepRotation r2; 162 HepRotation r2; 162 lt.decompose (b2, r2); 163 lt.decompose (b2, r2); 163 double db2 = b1.distance2(b2); 164 double db2 = b1.distance2(b2); 164 if ( db2 > epsilon*epsilon ) { 165 if ( db2 > epsilon*epsilon ) { 165 return false; // Saves the time-con 166 return false; // Saves the time-consuming Rotation::distance2 166 } 167 } 167 double dr2 = r1.distance2(r2); 168 double dr2 = r1.distance2(r2); 168 return ( (db2 + dr2) <= epsilon*epsilon ); 169 return ( (db2 + dr2) <= epsilon*epsilon ); 169 } 170 } 170 171 171 double HepLorentzRotation::norm2() const { 172 double HepLorentzRotation::norm2() const { 172 HepBoost b; 173 HepBoost b; 173 HepRotation r; 174 HepRotation r; 174 decompose( b, r ); 175 decompose( b, r ); 175 return b.norm2() + r.norm2(); 176 return b.norm2() + r.norm2(); 176 } 177 } 177 178 178 void HepLorentzRotation::rectify() { 179 void HepLorentzRotation::rectify() { 179 180 180 // Assuming the representation of this is cl 181 // Assuming the representation of this is close to a true LT, 181 // but may have drifted due to round-off err 182 // but may have drifted due to round-off error from many operations, 182 // this forms an "exact" orthosymplectic mat 183 // this forms an "exact" orthosymplectic matrix for the LT again. 183 184 184 // There are several ways to do this, all eq 185 // There are several ways to do this, all equivalent to lowest order in 185 // the corrected error. We choose to form a 186 // the corrected error. We choose to form an LT based on the inverse boost 186 // extracted from row 4, and left-multiply b 187 // extracted from row 4, and left-multiply by LT to form what would be 187 // a rotation if the LT were kosher. We dro 188 // a rotation if the LT were kosher. We drop the possibly non-zero t 188 // components of that, rectify that rotation 189 // components of that, rectify that rotation and multiply back by the boost. 189 190 190 Hep3Vector beta (tx(), ty(), tz()); 191 Hep3Vector beta (tx(), ty(), tz()); 191 double gam = tt(); // NaN-proofing 192 double gam = tt(); // NaN-proofing 192 if ( gam <= 0 ) { 193 if ( gam <= 0 ) { 193 std::cerr << "HepLorentzRotation::rectify( 194 std::cerr << "HepLorentzRotation::rectify() - " 194 << "rectify() on a transformation with tt() 195 << "rectify() on a transformation with tt() <= 0 - will not help!" 195 << std::endl; 196 << std::endl; 196 gam = 1; 197 gam = 1; 197 } 198 } 198 beta *= 1.0/gam; 199 beta *= 1.0/gam; 199 HepLorentzRotation R = (*this) * HepBoost(-b 200 HepLorentzRotation R = (*this) * HepBoost(-beta); 200 201 201 HepRep3x3 m1 ( R.xx(), R.xy(), R.xz(), 202 HepRep3x3 m1 ( R.xx(), R.xy(), R.xz(), 202 R.yx(), R.yy(), R.yz(), 203 R.yx(), R.yy(), R.yz(), 203 R.zx(), R.zy(), R.zz() ); 204 R.zx(), R.zy(), R.zz() ); 204 205 205 HepRotation Rgood (m1); 206 HepRotation Rgood (m1); 206 Rgood.rectify(); 207 Rgood.rectify(); 207 208 208 set ( Rgood, HepBoost(beta) ); 209 set ( Rgood, HepBoost(beta) ); 209 } 210 } 210 211 211 } // namespace CLHEP 212 } // namespace CLHEP 212 213