Geant4 Cross Reference

Cross-Referencing   Geant4
Geant4/externals/clhep/src/RotationC.cc

Version: [ ReleaseNotes ] [ 1.0 ] [ 1.1 ] [ 2.0 ] [ 3.0 ] [ 3.1 ] [ 3.2 ] [ 4.0 ] [ 4.0.p1 ] [ 4.0.p2 ] [ 4.1 ] [ 4.1.p1 ] [ 5.0 ] [ 5.0.p1 ] [ 5.1 ] [ 5.1.p1 ] [ 5.2 ] [ 5.2.p1 ] [ 5.2.p2 ] [ 6.0 ] [ 6.0.p1 ] [ 6.1 ] [ 6.2 ] [ 6.2.p1 ] [ 6.2.p2 ] [ 7.0 ] [ 7.0.p1 ] [ 7.1 ] [ 7.1.p1 ] [ 8.0 ] [ 8.0.p1 ] [ 8.1 ] [ 8.1.p1 ] [ 8.1.p2 ] [ 8.2 ] [ 8.2.p1 ] [ 8.3 ] [ 8.3.p1 ] [ 8.3.p2 ] [ 9.0 ] [ 9.0.p1 ] [ 9.0.p2 ] [ 9.1 ] [ 9.1.p1 ] [ 9.1.p2 ] [ 9.1.p3 ] [ 9.2 ] [ 9.2.p1 ] [ 9.2.p2 ] [ 9.2.p3 ] [ 9.2.p4 ] [ 9.3 ] [ 9.3.p1 ] [ 9.3.p2 ] [ 9.4 ] [ 9.4.p1 ] [ 9.4.p2 ] [ 9.4.p3 ] [ 9.4.p4 ] [ 9.5 ] [ 9.5.p1 ] [ 9.5.p2 ] [ 9.6 ] [ 9.6.p1 ] [ 9.6.p2 ] [ 9.6.p3 ] [ 9.6.p4 ] [ 10.0 ] [ 10.0.p1 ] [ 10.0.p2 ] [ 10.0.p3 ] [ 10.0.p4 ] [ 10.1 ] [ 10.1.p1 ] [ 10.1.p2 ] [ 10.1.p3 ] [ 10.2 ] [ 10.2.p1 ] [ 10.2.p2 ] [ 10.2.p3 ] [ 10.3 ] [ 10.3.p1 ] [ 10.3.p2 ] [ 10.3.p3 ] [ 10.4 ] [ 10.4.p1 ] [ 10.4.p2 ] [ 10.4.p3 ] [ 10.5 ] [ 10.5.p1 ] [ 10.6 ] [ 10.6.p1 ] [ 10.6.p2 ] [ 10.6.p3 ] [ 10.7 ] [ 10.7.p1 ] [ 10.7.p2 ] [ 10.7.p3 ] [ 10.7.p4 ] [ 11.0 ] [ 11.0.p1 ] [ 11.0.p2 ] [ 11.0.p3, ] [ 11.0.p4 ] [ 11.1 ] [ 11.1.1 ] [ 11.1.2 ] [ 11.1.3 ] [ 11.2 ] [ 11.2.1 ] [ 11.2.2 ] [ 11.3.0 ]

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