CLHEP VERSION Reference Documentation
   
CLHEP Home Page     CLHEP Documentation     CLHEP Bug Reports

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/defs.h"
17 #include "CLHEP/Vector/Rotation.h"
18 #include "CLHEP/Vector/ZMxpv.h"
19 
20 #include <cmath>
21 
22 namespace CLHEP {
23 
24 // --------- Helper methods (private) for setting from 3 columns:
25 
26 bool HepRotation::setCols
27  ( const Hep3Vector & u1, const Hep3Vector & u2, const Hep3Vector & u3,
28  double u1u2,
29  Hep3Vector & v1, Hep3Vector & v2, Hep3Vector & v3 ) const {
30 
31  if ( (1-std::fabs(u1u2)) <= Hep4RotationInterface::tolerance ) {
32  ZMthrowC (ZMxpvParallelCols(
33  "All three cols supplied for a Rotation are parallel --"
34  "\n an arbitrary rotation will be returned"));
35  setArbitrarily (u1, v1, v2, v3);
36  return true;
37  }
38 
39  v1 = u1;
40  v2 = Hep3Vector(u2 - u1u2 * u1).unit();
41  v3 = v1.cross(v2);
42  if ( v3.dot(u3) >= 0 ) {
43  return true;
44  } else {
45  return false; // looks more like a reflection in this case!
46  }
47 
48 } // HepRotation::setCols
49 
50 void HepRotation::setArbitrarily (const Hep3Vector & ccolX,
51  Hep3Vector & v1, Hep3Vector & v2, Hep3Vector & v3) const {
52 
53  // We have all three col's parallel. Warnings already been given;
54  // this just supplies a result which is a valid rotation.
55 
56  v1 = ccolX.unit();
57  v2 = v1.cross(Hep3Vector(0,0,1));
58  if (v2.mag2() != 0) {
59  v2 = v2.unit();
60  } else {
61  v2 = Hep3Vector(1,0,0);
62  }
63  v3 = v1.cross(v2);
64 
65  return;
66 
67 } // HepRotation::setArbitrarily
68 
69 
70 
71 // ---------- Constructors and Assignment:
72 
73 // 3 orthogonal columns or rows
74 
76  const Hep3Vector & ccolY,
77  const Hep3Vector & ccolZ ) {
78  Hep3Vector ucolX = ccolX.unit();
79  Hep3Vector ucolY = ccolY.unit();
80  Hep3Vector ucolZ = ccolZ.unit();
81 
82  double u1u2 = ucolX.dot(ucolY);
83  double f12 = std::fabs(u1u2);
85  ZMthrowC (ZMxpvNotOrthogonal(
86  "col's X and Y supplied for Rotation are not close to orthogonal"));
87  }
88  double u1u3 = ucolX.dot(ucolZ);
89  double f13 = std::fabs(u1u3);
91  ZMthrowC (ZMxpvNotOrthogonal(
92  "col's X and Z supplied for Rotation are not close to orthogonal"));
93  }
94  double u2u3 = ucolY.dot(ucolZ);
95  double f23 = std::fabs(u2u3);
97  ZMthrowC (ZMxpvNotOrthogonal(
98  "col's Y and Z supplied for Rotation are not close to orthogonal"));
99  }
100 
101  Hep3Vector v1, v2, v3;
102  bool isRotation;
103  if ( (f12 <= f13) && (f12 <= f23) ) {
104  isRotation = setCols ( ucolX, ucolY, ucolZ, u1u2, v1, v2, v3 );
105  if ( !isRotation ) {
106  ZMthrowC (ZMxpvImproperRotation(
107  "col's X Y and Z supplied form closer to a reflection than a Rotation "
108  "\n col Z is set to col X cross col Y"));
109  }
110  } else if ( f13 <= f23 ) {
111  isRotation = setCols ( ucolZ, ucolX, ucolY, u1u3, v3, v1, v2 );
112  if ( !isRotation ) {
113  ZMthrowC (ZMxpvImproperRotation(
114  "col's X Y and Z supplied form closer to a reflection than a Rotation "
115  "\n col Y is set to col Z cross col X"));
116  }
117  } else {
118  isRotation = setCols ( ucolY, ucolZ, ucolX, u2u3, v2, v3, v1 );
119  if ( !isRotation ) {
120  ZMthrowC (ZMxpvImproperRotation(
121  "col's X Y and Z supplied form closer to a reflection than a Rotation "
122  "\n col X is set to col Y cross col Z"));
123  }
124  }
125 
126  rxx = v1.x(); ryx = v1.y(); rzx = v1.z();
127  rxy = v2.x(); ryy = v2.y(); rzy = v2.z();
128  rxz = v3.x(); ryz = v3.y(); rzz = v3.z();
129 
130  return *this;
131 
132 } // HepRotation::set(colX, colY, colZ)
133 
135  const Hep3Vector & ccolY,
136  const Hep3Vector & ccolZ )
137 {
138  set (ccolX, ccolY, ccolZ);
139 }
140 
142  const Hep3Vector & rrowY,
143  const Hep3Vector & rrowZ ) {
144  set (rrowX, rrowY, rrowZ);
145  invert();
146  return *this;
147 }
148 
149 
150 // ------- Rectify a near-rotation
151 
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  ZMthrowA(ZMxpvImproperRotation(
171  "Attempt to rectify a Rotation with determinant <= 0\n"));
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 
Hep3Vector axis() const
Definition: RotationA.cc:82
double z() const
double dot(const Hep3Vector &) const
HepRotation & set(const Hep3Vector &axis, double delta)
Definition: RotationA.cc:28
#define ZMthrowC(A)
double delta() const
Definition: RotationA.cc:69
#define ZMthrowA(A)
Hep3Vector unit() const
double x() const
HepRotation & setRows(const Hep3Vector &rowX, const Hep3Vector &rowY, const Hep3Vector &rowZ)
Definition: RotationC.cc:141
double y() const
HepRotation & invert()