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
22namespace CLHEP {
23
24// --------- Helper methods (private) for setting from 3 columns:
25
26bool 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
50void 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
#define ZMthrowC(A)
#define ZMthrowA(A)
double z() const
Hep3Vector unit() const
double x() const
double y() const
double dot(const Hep3Vector &) const
Hep3Vector axis() const
Definition: RotationA.cc:82
HepRotation & setRows(const Hep3Vector &rowX, const Hep3Vector &rowY, const Hep3Vector &rowZ)
Definition: RotationC.cc:141
double delta() const
Definition: RotationA.cc:69
HepRotation & set(const Hep3Vector &axis, double delta)
Definition: RotationA.cc:28
HepRotation & invert()