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