| Geant4 Cross Reference |
1 // -*- C++ -*- 1 // -*- C++ -*-
>> 2 // $Id:$
2 // ------------------------------------------- 3 // ---------------------------------------------------------------------------
3 // 4 //
4 // This file is a part of the CLHEP - a Class 5 // This file is a part of the CLHEP - a Class Library for High Energy Physics.
5 // 6 //
6 // This is the implementation of the parts of 7 // This is the implementation of the parts of the the HepRotation class which
7 // were present in the original CLHEP before t 8 // were present in the original CLHEP before the merge with ZOOM PhysicsVectors.
8 // 9 //
>> 10
>> 11 #ifdef GNUPRAGMA
>> 12 #pragma implementation
>> 13 #endif
9 14
10 #include "CLHEP/Vector/Rotation.h" 15 #include "CLHEP/Vector/Rotation.h"
11 #include "CLHEP/Units/PhysicalConstants.h" 16 #include "CLHEP/Units/PhysicalConstants.h"
12 17
13 #include <iostream> 18 #include <iostream>
14 #include <cmath> 19 #include <cmath>
15 20
16 namespace CLHEP { 21 namespace CLHEP {
17 22
18 static inline double safe_acos (double x) { 23 static inline double safe_acos (double x) {
19 if (std::abs(x) <= 1.0) return std::acos(x); 24 if (std::abs(x) <= 1.0) return std::acos(x);
20 return ( (x>0) ? 0 : CLHEP::pi ); 25 return ( (x>0) ? 0 : CLHEP::pi );
21 } 26 }
22 27
23 double HepRotation::operator() (int i, int j) 28 double HepRotation::operator() (int i, int j) const {
24 if (i == 0) { 29 if (i == 0) {
25 if (j == 0) { return xx(); } 30 if (j == 0) { return xx(); }
26 if (j == 1) { return xy(); } 31 if (j == 1) { return xy(); }
27 if (j == 2) { return xz(); } 32 if (j == 2) { return xz(); }
28 } else if (i == 1) { 33 } else if (i == 1) {
29 if (j == 0) { return yx(); } 34 if (j == 0) { return yx(); }
30 if (j == 1) { return yy(); } 35 if (j == 1) { return yy(); }
31 if (j == 2) { return yz(); } 36 if (j == 2) { return yz(); }
32 } else if (i == 2) { 37 } else if (i == 2) {
33 if (j == 0) { return zx(); } 38 if (j == 0) { return zx(); }
34 if (j == 1) { return zy(); } 39 if (j == 1) { return zy(); }
35 if (j == 2) { return zz(); } 40 if (j == 2) { return zz(); }
36 } 41 }
37 std::cerr << "HepRotation subscripting: bad 42 std::cerr << "HepRotation subscripting: bad indices "
38 << "(" << i << "," << j << ")" << std:: 43 << "(" << i << "," << j << ")" << std::endl;
39 return 0.0; 44 return 0.0;
40 } 45 }
41 46
42 HepRotation & HepRotation::rotate(double a, co 47 HepRotation & HepRotation::rotate(double a, const Hep3Vector& aaxis) {
43 if (a != 0.0) { 48 if (a != 0.0) {
44 double ll = aaxis.mag(); 49 double ll = aaxis.mag();
45 if (ll == 0.0) { 50 if (ll == 0.0) {
46 std::cerr << "HepRotation::rotate() - " 51 std::cerr << "HepRotation::rotate() - "
47 << "HepRotation: zero axis" << 52 << "HepRotation: zero axis" << std::endl;
48 }else{ 53 }else{
49 double sa = std::sin(a), ca = std::cos(a 54 double sa = std::sin(a), ca = std::cos(a);
50 double dx = aaxis.x()/ll, dy = aaxis.y() 55 double dx = aaxis.x()/ll, dy = aaxis.y()/ll, dz = aaxis.z()/ll;
51 HepRotation m1( 56 HepRotation m1(
52 ca+(1-ca)*dx*dx, (1-ca)*dx*dy-sa*dz 57 ca+(1-ca)*dx*dx, (1-ca)*dx*dy-sa*dz, (1-ca)*dx*dz+sa*dy,
53 (1-ca)*dy*dx+sa*dz, ca+(1-ca)*dy*dy, 58 (1-ca)*dy*dx+sa*dz, ca+(1-ca)*dy*dy, (1-ca)*dy*dz-sa*dx,
54 (1-ca)*dz*dx-sa*dy, (1-ca)*dz*dy+sa*dx 59 (1-ca)*dz*dx-sa*dy, (1-ca)*dz*dy+sa*dx, ca+(1-ca)*dz*dz );
55 transform(m1); 60 transform(m1);
56 } 61 }
57 } 62 }
58 return *this; 63 return *this;
59 } 64 }
60 65
61 HepRotation & HepRotation::rotateX(double a) { 66 HepRotation & HepRotation::rotateX(double a) {
62 double c1 = std::cos(a); 67 double c1 = std::cos(a);
63 double s1 = std::sin(a); 68 double s1 = std::sin(a);
64 double x1 = ryx, y1 = ryy, z1 = ryz; 69 double x1 = ryx, y1 = ryy, z1 = ryz;
65 ryx = c1*x1 - s1*rzx; 70 ryx = c1*x1 - s1*rzx;
66 ryy = c1*y1 - s1*rzy; 71 ryy = c1*y1 - s1*rzy;
67 ryz = c1*z1 - s1*rzz; 72 ryz = c1*z1 - s1*rzz;
68 rzx = s1*x1 + c1*rzx; 73 rzx = s1*x1 + c1*rzx;
69 rzy = s1*y1 + c1*rzy; 74 rzy = s1*y1 + c1*rzy;
70 rzz = s1*z1 + c1*rzz; 75 rzz = s1*z1 + c1*rzz;
71 return *this; 76 return *this;
72 } 77 }
73 78
74 HepRotation & HepRotation::rotateY(double a){ 79 HepRotation & HepRotation::rotateY(double a){
75 double c1 = std::cos(a); 80 double c1 = std::cos(a);
76 double s1 = std::sin(a); 81 double s1 = std::sin(a);
77 double x1 = rzx, y1 = rzy, z1 = rzz; 82 double x1 = rzx, y1 = rzy, z1 = rzz;
78 rzx = c1*x1 - s1*rxx; 83 rzx = c1*x1 - s1*rxx;
79 rzy = c1*y1 - s1*rxy; 84 rzy = c1*y1 - s1*rxy;
80 rzz = c1*z1 - s1*rxz; 85 rzz = c1*z1 - s1*rxz;
81 rxx = s1*x1 + c1*rxx; 86 rxx = s1*x1 + c1*rxx;
82 rxy = s1*y1 + c1*rxy; 87 rxy = s1*y1 + c1*rxy;
83 rxz = s1*z1 + c1*rxz; 88 rxz = s1*z1 + c1*rxz;
84 return *this; 89 return *this;
85 } 90 }
86 91
87 HepRotation & HepRotation::rotateZ(double a) { 92 HepRotation & HepRotation::rotateZ(double a) {
88 double c1 = std::cos(a); 93 double c1 = std::cos(a);
89 double s1 = std::sin(a); 94 double s1 = std::sin(a);
90 double x1 = rxx, y1 = rxy, z1 = rxz; 95 double x1 = rxx, y1 = rxy, z1 = rxz;
91 rxx = c1*x1 - s1*ryx; 96 rxx = c1*x1 - s1*ryx;
92 rxy = c1*y1 - s1*ryy; 97 rxy = c1*y1 - s1*ryy;
93 rxz = c1*z1 - s1*ryz; 98 rxz = c1*z1 - s1*ryz;
94 ryx = s1*x1 + c1*ryx; 99 ryx = s1*x1 + c1*ryx;
95 ryy = s1*y1 + c1*ryy; 100 ryy = s1*y1 + c1*ryy;
96 ryz = s1*z1 + c1*ryz; 101 ryz = s1*z1 + c1*ryz;
97 return *this; 102 return *this;
98 } 103 }
99 104
100 HepRotation & HepRotation::rotateAxes(const He 105 HepRotation & HepRotation::rotateAxes(const Hep3Vector &newX,
101 const Hep3Vector &newY, 106 const Hep3Vector &newY,
102 const Hep3Vector &newZ) { 107 const Hep3Vector &newZ) {
103 double del = 0.001; 108 double del = 0.001;
104 Hep3Vector w = newX.cross(newY); 109 Hep3Vector w = newX.cross(newY);
105 110
106 if (std::abs(newZ.x()-w.x()) > del || 111 if (std::abs(newZ.x()-w.x()) > del ||
107 std::abs(newZ.y()-w.y()) > del || 112 std::abs(newZ.y()-w.y()) > del ||
108 std::abs(newZ.z()-w.z()) > del || 113 std::abs(newZ.z()-w.z()) > del ||
109 std::abs(newX.mag2()-1.) > del || 114 std::abs(newX.mag2()-1.) > del ||
110 std::abs(newY.mag2()-1.) > del || 115 std::abs(newY.mag2()-1.) > del ||
111 std::abs(newZ.mag2()-1.) > del || 116 std::abs(newZ.mag2()-1.) > del ||
112 std::abs(newX.dot(newY)) > del || 117 std::abs(newX.dot(newY)) > del ||
113 std::abs(newY.dot(newZ)) > del || 118 std::abs(newY.dot(newZ)) > del ||
114 std::abs(newZ.dot(newX)) > del) { 119 std::abs(newZ.dot(newX)) > del) {
115 std::cerr << "HepRotation::rotateAxes: bad 120 std::cerr << "HepRotation::rotateAxes: bad axis vectors" << std::endl;
116 return *this; 121 return *this;
117 }else{ 122 }else{
118 return transform(HepRotation(newX.x(), new 123 return transform(HepRotation(newX.x(), newY.x(), newZ.x(),
119 newX.y(), new 124 newX.y(), newY.y(), newZ.y(),
120 newX.z(), new 125 newX.z(), newY.z(), newZ.z()));
121 } 126 }
122 } 127 }
123 128
124 double HepRotation::phiX() const { 129 double HepRotation::phiX() const {
125 return (yx() == 0.0 && xx() == 0.0) ? 0.0 : 130 return (yx() == 0.0 && xx() == 0.0) ? 0.0 : std::atan2(yx(),xx());
126 } 131 }
127 132
128 double HepRotation::phiY() const { 133 double HepRotation::phiY() const {
129 return (yy() == 0.0 && xy() == 0.0) ? 0.0 : 134 return (yy() == 0.0 && xy() == 0.0) ? 0.0 : std::atan2(yy(),xy());
130 } 135 }
131 136
132 double HepRotation::phiZ() const { 137 double HepRotation::phiZ() const {
133 return (yz() == 0.0 && xz() == 0.0) ? 0.0 : 138 return (yz() == 0.0 && xz() == 0.0) ? 0.0 : std::atan2(yz(),xz());
134 } 139 }
135 140
136 double HepRotation::thetaX() const { 141 double HepRotation::thetaX() const {
137 return safe_acos(zx()); 142 return safe_acos(zx());
138 } 143 }
139 144
140 double HepRotation::thetaY() const { 145 double HepRotation::thetaY() const {
141 return safe_acos(zy()); 146 return safe_acos(zy());
142 } 147 }
143 148
144 double HepRotation::thetaZ() const { 149 double HepRotation::thetaZ() const {
145 return safe_acos(zz()); 150 return safe_acos(zz());
146 } 151 }
147 152
148 void HepRotation::getAngleAxis(double &angle, 153 void HepRotation::getAngleAxis(double &angle, Hep3Vector &aaxis) const {
149 double cosa = 0.5*(xx()+yy()+zz()-1); 154 double cosa = 0.5*(xx()+yy()+zz()-1);
150 double cosa1 = 1-cosa; 155 double cosa1 = 1-cosa;
151 if (cosa1 <= 0) { 156 if (cosa1 <= 0) {
152 angle = 0; 157 angle = 0;
153 aaxis = Hep3Vector(0,0,1); 158 aaxis = Hep3Vector(0,0,1);
154 }else{ 159 }else{
155 double x=0, y=0, z=0; 160 double x=0, y=0, z=0;
156 if (xx() > cosa) x = std::sqrt((xx()-cosa) 161 if (xx() > cosa) x = std::sqrt((xx()-cosa)/cosa1);
157 if (yy() > cosa) y = std::sqrt((yy()-cosa) 162 if (yy() > cosa) y = std::sqrt((yy()-cosa)/cosa1);
158 if (zz() > cosa) z = std::sqrt((zz()-cosa) 163 if (zz() > cosa) z = std::sqrt((zz()-cosa)/cosa1);
159 if (zy() < yz()) x = -x; 164 if (zy() < yz()) x = -x;
160 if (xz() < zx()) y = -y; 165 if (xz() < zx()) y = -y;
161 if (yx() < xy()) z = -z; 166 if (yx() < xy()) z = -z;
162 angle = (cosa < -1.) ? std::acos(-1.) : st 167 angle = (cosa < -1.) ? std::acos(-1.) : std::acos(cosa);
163 aaxis = Hep3Vector(x,y,z); 168 aaxis = Hep3Vector(x,y,z);
164 } 169 }
165 } 170 }
166 171
167 bool HepRotation::isIdentity() const { 172 bool HepRotation::isIdentity() const {
168 return (rxx == 1.0 && rxy == 0.0 && rxz == 173 return (rxx == 1.0 && rxy == 0.0 && rxz == 0.0 &&
169 ryx == 0.0 && ryy == 1.0 && ryz == 174 ryx == 0.0 && ryy == 1.0 && ryz == 0.0 &&
170 rzx == 0.0 && rzy == 0.0 && rzz == 175 rzx == 0.0 && rzy == 0.0 && rzz == 1.0) ? true : false;
171 } 176 }
172 177
173 int HepRotation::compare ( const HepRotation & 178 int HepRotation::compare ( const HepRotation & r ) const {
174 if (rzz<r.rzz) return -1; else if (rzz> 179 if (rzz<r.rzz) return -1; else if (rzz>r.rzz) return 1;
175 else if (rzy<r.rzy) return -1; else if (rzy> 180 else if (rzy<r.rzy) return -1; else if (rzy>r.rzy) return 1;
176 else if (rzx<r.rzx) return -1; else if (rzx> 181 else if (rzx<r.rzx) return -1; else if (rzx>r.rzx) return 1;
177 else if (ryz<r.ryz) return -1; else if (ryz> 182 else if (ryz<r.ryz) return -1; else if (ryz>r.ryz) return 1;
178 else if (ryy<r.ryy) return -1; else if (ryy> 183 else if (ryy<r.ryy) return -1; else if (ryy>r.ryy) return 1;
179 else if (ryx<r.ryx) return -1; else if (ryx> 184 else if (ryx<r.ryx) return -1; else if (ryx>r.ryx) return 1;
180 else if (rxz<r.rxz) return -1; else if (rxz> 185 else if (rxz<r.rxz) return -1; else if (rxz>r.rxz) return 1;
181 else if (rxy<r.rxy) return -1; else if (rxy> 186 else if (rxy<r.rxy) return -1; else if (rxy>r.rxy) return 1;
182 else if (rxx<r.rxx) return -1; else if (rxx> 187 else if (rxx<r.rxx) return -1; else if (rxx>r.rxx) return 1;
183 else return 0; 188 else return 0;
184 } 189 }
185 190
186 191
187 const HepRotation HepRotation::IDENTITY; 192 const HepRotation HepRotation::IDENTITY;
188 193
189 } // namespace CLHEP 194 } // namespace CLHEP
190 195
191 196
192 197