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