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