00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011 #ifdef GNUPRAGMA
00012 #pragma implementation
00013 #endif
00014
00015 #include "CLHEP/Vector/Rotation.h"
00016 #include "CLHEP/Units/PhysicalConstants.h"
00017
00018 #include <iostream>
00019 #include <cmath>
00020
00021 namespace CLHEP {
00022
00023 static inline double safe_acos (double x) {
00024 if (std::abs(x) <= 1.0) return std::acos(x);
00025 return ( (x>0) ? 0 : CLHEP::pi );
00026 }
00027
00028 double HepRotation::operator() (int i, int j) const {
00029 if (i == 0) {
00030 if (j == 0) { return xx(); }
00031 if (j == 1) { return xy(); }
00032 if (j == 2) { return xz(); }
00033 } else if (i == 1) {
00034 if (j == 0) { return yx(); }
00035 if (j == 1) { return yy(); }
00036 if (j == 2) { return yz(); }
00037 } else if (i == 2) {
00038 if (j == 0) { return zx(); }
00039 if (j == 1) { return zy(); }
00040 if (j == 2) { return zz(); }
00041 }
00042 std::cerr << "HepRotation subscripting: bad indices "
00043 << "(" << i << "," << j << ")" << std::endl;
00044 return 0.0;
00045 }
00046
00047 HepRotation & HepRotation::rotate(double a, const Hep3Vector& aaxis) {
00048 if (a != 0.0) {
00049 double ll = aaxis.mag();
00050 if (ll == 0.0) {
00051 std::cerr << "HepRotation::rotate() - "
00052 << "HepRotation: zero axis" << std::endl;
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 }
00195
00196