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