CLHEP VERSION Reference Documentation
CLHEP Home Page CLHEP Documentation CLHEP Bug Reports |
00001 // -*- C++ -*- 00002 // $Id: Rotation.cc,v 1.4 2003/08/13 20:00:14 garren Exp $ 00003 // --------------------------------------------------------------------------- 00004 // 00005 // This file is a part of the CLHEP - a Class Library for High Energy Physics. 00006 // 00007 // This is the implementation of the parts of the the HepRotation class which 00008 // were present in the original CLHEP before the merge with ZOOM PhysicsVectors. 00009 // 00010 00011 #ifdef GNUPRAGMA 00012 #pragma implementation 00013 #endif 00014 00015 #include "CLHEP/Vector/defs.h" 00016 #include "CLHEP/Vector/Rotation.h" 00017 #include "CLHEP/Units/PhysicalConstants.h" 00018 00019 #include <iostream> 00020 #include <cmath> 00021 00022 namespace CLHEP { 00023 00024 static inline double safe_acos (double x) { 00025 if (std::abs(x) <= 1.0) return std::acos(x); 00026 return ( (x>0) ? 0 : CLHEP::pi ); 00027 } 00028 00029 double HepRotation::operator() (int i, int j) const { 00030 if (i == 0) { 00031 if (j == 0) { return xx(); } 00032 if (j == 1) { return xy(); } 00033 if (j == 2) { return xz(); } 00034 } else if (i == 1) { 00035 if (j == 0) { return yx(); } 00036 if (j == 1) { return yy(); } 00037 if (j == 2) { return yz(); } 00038 } else if (i == 2) { 00039 if (j == 0) { return zx(); } 00040 if (j == 1) { return zy(); } 00041 if (j == 2) { return zz(); } 00042 } 00043 std::cerr << "HepRotation subscripting: bad indices " 00044 << "(" << i << "," << j << ")" << std::endl; 00045 return 0.0; 00046 } 00047 00048 HepRotation & HepRotation::rotate(double a, const Hep3Vector& aaxis) { 00049 if (a != 0.0) { 00050 double ll = aaxis.mag(); 00051 if (ll == 0.0) { 00052 ZMthrowC (ZMxpvZeroVector("HepRotation: zero axis")); 00053 }else{ 00054 double sa = std::sin(a), ca = std::cos(a); 00055 double dx = aaxis.x()/ll, dy = aaxis.y()/ll, dz = aaxis.z()/ll; 00056 HepRotation m1( 00057 ca+(1-ca)*dx*dx, (1-ca)*dx*dy-sa*dz, (1-ca)*dx*dz+sa*dy, 00058 (1-ca)*dy*dx+sa*dz, ca+(1-ca)*dy*dy, (1-ca)*dy*dz-sa*dx, 00059 (1-ca)*dz*dx-sa*dy, (1-ca)*dz*dy+sa*dx, ca+(1-ca)*dz*dz ); 00060 transform(m1); 00061 } 00062 } 00063 return *this; 00064 } 00065 00066 HepRotation & HepRotation::rotateX(double a) { 00067 double c1 = std::cos(a); 00068 double s1 = std::sin(a); 00069 double x1 = ryx, y1 = ryy, z1 = ryz; 00070 ryx = c1*x1 - s1*rzx; 00071 ryy = c1*y1 - s1*rzy; 00072 ryz = c1*z1 - s1*rzz; 00073 rzx = s1*x1 + c1*rzx; 00074 rzy = s1*y1 + c1*rzy; 00075 rzz = s1*z1 + c1*rzz; 00076 return *this; 00077 } 00078 00079 HepRotation & HepRotation::rotateY(double a){ 00080 double c1 = std::cos(a); 00081 double s1 = std::sin(a); 00082 double x1 = rzx, y1 = rzy, z1 = rzz; 00083 rzx = c1*x1 - s1*rxx; 00084 rzy = c1*y1 - s1*rxy; 00085 rzz = c1*z1 - s1*rxz; 00086 rxx = s1*x1 + c1*rxx; 00087 rxy = s1*y1 + c1*rxy; 00088 rxz = s1*z1 + c1*rxz; 00089 return *this; 00090 } 00091 00092 HepRotation & HepRotation::rotateZ(double a) { 00093 double c1 = std::cos(a); 00094 double s1 = std::sin(a); 00095 double x1 = rxx, y1 = rxy, z1 = rxz; 00096 rxx = c1*x1 - s1*ryx; 00097 rxy = c1*y1 - s1*ryy; 00098 rxz = c1*z1 - s1*ryz; 00099 ryx = s1*x1 + c1*ryx; 00100 ryy = s1*y1 + c1*ryy; 00101 ryz = s1*z1 + c1*ryz; 00102 return *this; 00103 } 00104 00105 HepRotation & HepRotation::rotateAxes(const Hep3Vector &newX, 00106 const Hep3Vector &newY, 00107 const Hep3Vector &newZ) { 00108 double del = 0.001; 00109 Hep3Vector w = newX.cross(newY); 00110 00111 if (std::abs(newZ.x()-w.x()) > del || 00112 std::abs(newZ.y()-w.y()) > del || 00113 std::abs(newZ.z()-w.z()) > del || 00114 std::abs(newX.mag2()-1.) > del || 00115 std::abs(newY.mag2()-1.) > del || 00116 std::abs(newZ.mag2()-1.) > del || 00117 std::abs(newX.dot(newY)) > del || 00118 std::abs(newY.dot(newZ)) > del || 00119 std::abs(newZ.dot(newX)) > del) { 00120 std::cerr << "HepRotation::rotateAxes: bad axis vectors" << std::endl; 00121 return *this; 00122 }else{ 00123 return transform(HepRotation(newX.x(), newY.x(), newZ.x(), 00124 newX.y(), newY.y(), newZ.y(), 00125 newX.z(), newY.z(), newZ.z())); 00126 } 00127 } 00128 00129 double HepRotation::phiX() const { 00130 return (yx() == 0.0 && xx() == 0.0) ? 0.0 : std::atan2(yx(),xx()); 00131 } 00132 00133 double HepRotation::phiY() const { 00134 return (yy() == 0.0 && xy() == 0.0) ? 0.0 : std::atan2(yy(),xy()); 00135 } 00136 00137 double HepRotation::phiZ() const { 00138 return (yz() == 0.0 && xz() == 0.0) ? 0.0 : std::atan2(yz(),xz()); 00139 } 00140 00141 double HepRotation::thetaX() const { 00142 return safe_acos(zx()); 00143 } 00144 00145 double HepRotation::thetaY() const { 00146 return safe_acos(zy()); 00147 } 00148 00149 double HepRotation::thetaZ() const { 00150 return safe_acos(zz()); 00151 } 00152 00153 void HepRotation::getAngleAxis(double &angle, Hep3Vector &aaxis) const { 00154 double cosa = 0.5*(xx()+yy()+zz()-1); 00155 double cosa1 = 1-cosa; 00156 if (cosa1 <= 0) { 00157 angle = 0; 00158 aaxis = Hep3Vector(0,0,1); 00159 }else{ 00160 double x=0, y=0, z=0; 00161 if (xx() > cosa) x = std::sqrt((xx()-cosa)/cosa1); 00162 if (yy() > cosa) y = std::sqrt((yy()-cosa)/cosa1); 00163 if (zz() > cosa) z = std::sqrt((zz()-cosa)/cosa1); 00164 if (zy() < yz()) x = -x; 00165 if (xz() < zx()) y = -y; 00166 if (yx() < xy()) z = -z; 00167 angle = (cosa < -1.) ? std::acos(-1.) : std::acos(cosa); 00168 aaxis = Hep3Vector(x,y,z); 00169 } 00170 } 00171 00172 bool HepRotation::isIdentity() const { 00173 return (rxx == 1.0 && rxy == 0.0 && rxz == 0.0 && 00174 ryx == 0.0 && ryy == 1.0 && ryz == 0.0 && 00175 rzx == 0.0 && rzy == 0.0 && rzz == 1.0) ? true : false; 00176 } 00177 00178 int HepRotation::compare ( const HepRotation & r ) const { 00179 if (rzz<r.rzz) return -1; else if (rzz>r.rzz) return 1; 00180 else if (rzy<r.rzy) return -1; else if (rzy>r.rzy) return 1; 00181 else if (rzx<r.rzx) return -1; else if (rzx>r.rzx) return 1; 00182 else if (ryz<r.ryz) return -1; else if (ryz>r.ryz) return 1; 00183 else if (ryy<r.ryy) return -1; else if (ryy>r.ryy) return 1; 00184 else if (ryx<r.ryx) return -1; else if (ryx>r.ryx) return 1; 00185 else if (rxz<r.rxz) return -1; else if (rxz>r.rxz) return 1; 00186 else if (rxy<r.rxy) return -1; else if (rxy>r.rxy) return 1; 00187 else if (rxx<r.rxx) return -1; else if (rxx>r.rxx) return 1; 00188 else return 0; 00189 } 00190 00191 00192 const HepRotation HepRotation::IDENTITY; 00193 00194 } // namespace CLHEP 00195 00196