Geant4: BasicVector3D.cc Source File

00001 // -*- C++ -*-
00002 // $Id:$
00003 // ---------------------------------------------------------------------------
00004 
00005 #include <cmath>
00006 #include <float.h>
00007 #include <iostream>
00008 #include "CLHEP/Geometry/BasicVector3D.h"
00009 
00010 namespace HepGeom {
00011   //--------------------------------------------------------------------------
00012   template<>
00013   float BasicVector3D<float>::pseudoRapidity() const {
00014     float ma = mag(), dz = z();
00015     if (ma ==  0)  return  0;
00016     if (ma ==  dz) return  FLT_MAX;
00017     if (ma == -dz) return -FLT_MAX;
00018     return 0.5*std::log((ma+dz)/(ma-dz));
00019   }
00020 
00021   //--------------------------------------------------------------------------
00022   template<>
00023   void BasicVector3D<float>::setEta(float a) {
00024     double ma = mag();
00025     if (ma == 0) return;
00026     double tanHalfTheta  = std::exp(-a);
00027     double tanHalfTheta2 = tanHalfTheta * tanHalfTheta;
00028     double cosTheta1      = (1 - tanHalfTheta2) / (1 + tanHalfTheta2);
00029     double rh            = ma * std::sqrt(1 - cosTheta1*cosTheta1);
00030     double ph            = phi();
00031     set(rh*std::cos(ph), rh*std::sin(ph), ma*cosTheta1);
00032   }
00033 
00034   //--------------------------------------------------------------------------
00035   template<>
00036   float BasicVector3D<float>::angle(const BasicVector3D<float> & v) const {
00037     double cosa = 0;
00038     double ptot = mag()*v.mag();
00039     if(ptot > 0) {
00040       cosa = dot(v)/ptot;
00041       if(cosa >  1) cosa =  1;
00042       if(cosa < -1) cosa = -1;
00043     }
00044     return std::acos(cosa);
00045   }
00046 
00047   //--------------------------------------------------------------------------
00048   template<>
00049   BasicVector3D<float> & BasicVector3D<float>::rotateX(float a) {
00050     double sina = std::sin(a), cosa = std::cos(a), dy = y(), dz = z();
00051     setY(dy*cosa-dz*sina);
00052     setZ(dz*cosa+dy*sina);
00053     return *this;
00054   }
00055 
00056   //--------------------------------------------------------------------------
00057   template<>
00058   BasicVector3D<float> & BasicVector3D<float>::rotateY(float a) {
00059     double sina = std::sin(a), cosa = std::cos(a), dz = z(), dx = x();
00060     setZ(dz*cosa-dx*sina);
00061     setX(dx*cosa+dz*sina);
00062     return *this;
00063   }
00064 
00065   //--------------------------------------------------------------------------
00066   template<>
00067   BasicVector3D<float> & BasicVector3D<float>::rotateZ(float a) {
00068     double sina = std::sin(a), cosa = std::cos(a), dx = x(), dy = y();
00069     setX(dx*cosa-dy*sina);
00070     setY(dy*cosa+dx*sina);
00071     return *this;
00072   }
00073 
00074   //--------------------------------------------------------------------------
00075   template<>
00076   BasicVector3D<float> &
00077   BasicVector3D<float>::rotate(float a, const BasicVector3D<float> & v) {
00078     if (a  == 0) return *this;
00079     double cx = v.x(), cy = v.y(), cz = v.z();
00080     double ll = std::sqrt(cx*cx + cy*cy + cz*cz);
00081     if (ll == 0) {
00082       std::cerr << "BasicVector<float>::rotate() : zero axis" << std::endl;
00083       return *this;
00084     }
00085     double cosa = std::cos(a), sina = std::sin(a);
00086     cx /= ll; cy /= ll; cz /= ll;   
00087 
00088     double xx = cosa + (1-cosa)*cx*cx;
00089     double xy =        (1-cosa)*cx*cy - sina*cz;
00090     double xz =        (1-cosa)*cx*cz + sina*cy;
00091     
00092     double yx =        (1-cosa)*cy*cx + sina*cz;
00093     double yy = cosa + (1-cosa)*cy*cy; 
00094     double yz =        (1-cosa)*cy*cz - sina*cx;
00095     
00096     double zx =        (1-cosa)*cz*cx - sina*cy;
00097     double zy =        (1-cosa)*cz*cy + sina*cx;
00098     double zz = cosa + (1-cosa)*cz*cz;
00099 
00100     cx = x(); cy = y(); cz = z();   
00101     set(xx*cx+xy*cy+xz*cz, yx*cx+yy*cy+yz*cz, zx*cx+zy*cy+zz*cz);
00102     return *this;
00103   }
00104 
00105   //--------------------------------------------------------------------------
00106   std::ostream &
00107   operator<<(std::ostream & os, const BasicVector3D<float> & a)
00108   {
00109     return os << "(" << a.x() << "," << a.y() << "," << a.z() << ")";
00110   }
00111 
00112   //--------------------------------------------------------------------------
00113   std::istream &
00114   operator>> (std::istream & is, BasicVector3D<float> & a)
00115   {
00116     // Required format is ( a, b, c ) that is, three numbers, preceded by
00117     // (, followed by ), and separated by commas.  The three numbers are
00118     // taken as x, y, z.
00119 
00120     float x, y, z;
00121     char c;
00122 
00123     is >> std::ws >> c;
00124     // ws is defined to invoke eatwhite(istream & )
00125     // see (Stroustrup gray book) page 333 and 345.
00126     if (is.fail() || c != '(' ) {
00127       std::cerr
00128         << "Could not find required opening parenthesis "
00129         << "in input of a BasicVector3D<float>"
00130         << std::endl;
00131       return is;
00132     }
00133 
00134     is >> x >> std::ws >> c;
00135     if (is.fail() || c != ',' ) {
00136       std::cerr
00137         << "Could not find x value and required trailing comma "
00138         << "in input of a BasicVector3D<float>"
00139         << std::endl; 
00140       return is;
00141     }
00142 
00143     is >> y >> std::ws >> c;
00144     if (is.fail() || c != ',' ) {
00145       std::cerr
00146         << "Could not find y value and required trailing comma "
00147         <<  "in input of a BasicVector3D<float>"
00148         << std::endl;
00149       return is;
00150     }
00151 
00152     is >> z >> std::ws >> c;
00153     if (is.fail() || c != ')' ) {
00154       std::cerr
00155         << "Could not find z value and required close parenthesis "
00156         << "in input of a BasicVector3D<float>"
00157         << std::endl;
00158       return is;
00159     }
00160 
00161     a.setX(x);
00162     a.setY(y);
00163     a.setZ(z);
00164     return is;
00165   }
00166   
00167   //--------------------------------------------------------------------------
00168   template<>
00169   double BasicVector3D<double>::pseudoRapidity() const {
00170     double ma = mag(), dz = z();
00171     if (ma ==  0)  return  0;
00172     if (ma ==  dz) return  DBL_MAX;
00173     if (ma == -dz) return -DBL_MAX;
00174     return 0.5*std::log((ma+dz)/(ma-dz));
00175   }
00176 
00177   //--------------------------------------------------------------------------
00178   template<>
00179   void BasicVector3D<double>::setEta(double a) {
00180     double ma = mag();
00181     if (ma == 0) return;
00182     double tanHalfTheta  = std::exp(-a);
00183     double tanHalfTheta2 = tanHalfTheta * tanHalfTheta;
00184     double cosTheta1      = (1 - tanHalfTheta2) / (1 + tanHalfTheta2);
00185     double rh            = ma * std::sqrt(1 - cosTheta1*cosTheta1);
00186     double ph            = phi();
00187     set(rh*std::cos(ph), rh*std::sin(ph), ma*cosTheta1);
00188   }
00189 
00190   //--------------------------------------------------------------------------
00191   template<>
00192   double BasicVector3D<double>::angle(const BasicVector3D<double> & v) const {
00193     double cosa = 0;
00194     double ptot = mag()*v.mag();
00195     if(ptot > 0) {
00196       cosa = dot(v)/ptot;
00197       if(cosa >  1) cosa =  1;
00198       if(cosa < -1) cosa = -1;
00199     }
00200     return std::acos(cosa);
00201   }
00202 
00203   //--------------------------------------------------------------------------
00204   template<>
00205   BasicVector3D<double> & BasicVector3D<double>::rotateX(double a) {
00206     double sina = std::sin(a), cosa = std::cos(a), dy = y(), dz = z();
00207     setY(dy*cosa-dz*sina);
00208     setZ(dz*cosa+dy*sina);
00209     return *this;
00210   }
00211 
00212   //--------------------------------------------------------------------------
00213   template<>
00214   BasicVector3D<double> & BasicVector3D<double>::rotateY(double a) {
00215     double sina = std::sin(a), cosa = std::cos(a), dz = z(), dx = x();
00216     setZ(dz*cosa-dx*sina);
00217     setX(dx*cosa+dz*sina);
00218     return *this;
00219   }
00220 
00221   //--------------------------------------------------------------------------
00222   template<>
00223   BasicVector3D<double> & BasicVector3D<double>::rotateZ(double a) {
00224     double sina = std::sin(a), cosa = std::cos(a), dx = x(), dy = y();
00225     setX(dx*cosa-dy*sina);
00226     setY(dy*cosa+dx*sina);
00227     return *this;
00228   }
00229 
00230   //--------------------------------------------------------------------------
00231   template<>
00232   BasicVector3D<double> &
00233   BasicVector3D<double>::rotate(double a, const BasicVector3D<double> & v) {
00234     if (a  == 0) return *this;
00235     double cx = v.x(), cy = v.y(), cz = v.z();
00236     double ll = std::sqrt(cx*cx + cy*cy + cz*cz);
00237     if (ll == 0) {
00238       std::cerr << "BasicVector<double>::rotate() : zero axis" << std::endl;
00239       return *this;
00240     }
00241     double cosa = std::cos(a), sina = std::sin(a);
00242     cx /= ll; cy /= ll; cz /= ll;   
00243 
00244     double xx = cosa + (1-cosa)*cx*cx;
00245     double xy =        (1-cosa)*cx*cy - sina*cz;
00246     double xz =        (1-cosa)*cx*cz + sina*cy;
00247     
00248     double yx =        (1-cosa)*cy*cx + sina*cz;
00249     double yy = cosa + (1-cosa)*cy*cy; 
00250     double yz =        (1-cosa)*cy*cz - sina*cx;
00251     
00252     double zx =        (1-cosa)*cz*cx - sina*cy;
00253     double zy =        (1-cosa)*cz*cy + sina*cx;
00254     double zz = cosa + (1-cosa)*cz*cz;
00255 
00256     cx = x(); cy = y(); cz = z();   
00257     set(xx*cx+xy*cy+xz*cz, yx*cx+yy*cy+yz*cz, zx*cx+zy*cy+zz*cz);
00258     return *this;
00259   }
00260 
00261   //--------------------------------------------------------------------------
00262   std::ostream &
00263   operator<< (std::ostream & os, const BasicVector3D<double> & a)
00264   {
00265     return os << "(" << a.x() << "," << a.y() << "," << a.z() << ")";
00266   }
00267   
00268   //--------------------------------------------------------------------------
00269   std::istream &
00270   operator>> (std::istream & is, BasicVector3D<double> & a)
00271   {
00272     // Required format is ( a, b, c ) that is, three numbers, preceded by
00273     // (, followed by ), and separated by commas.  The three numbers are
00274     // taken as x, y, z.
00275     
00276     double x, y, z;
00277     char c;
00278     
00279     is >> std::ws >> c;
00280     // ws is defined to invoke eatwhite(istream & )
00281     // see (Stroustrup gray book) page 333 and 345.
00282     if (is.fail() || c != '(' ) {
00283       std::cerr
00284         << "Could not find required opening parenthesis "
00285         << "in input of a BasicVector3D<double>"
00286         << std::endl;
00287       return is;
00288     }
00289 
00290     is >> x >> std::ws >> c;
00291     if (is.fail() || c != ',' ) {
00292       std::cerr
00293         << "Could not find x value and required trailing comma "
00294         << "in input of a BasicVector3D<double>"
00295         << std::endl; 
00296       return is;
00297     }
00298 
00299     is >> y >> std::ws >> c;
00300     if (is.fail() || c != ',' ) {
00301       std::cerr
00302         << "Could not find y value and required trailing comma "
00303         <<  "in input of a BasicVector3D<double>"
00304         << std::endl;
00305       return is;
00306     }
00307 
00308     is >> z >> std::ws >> c;
00309     if (is.fail() || c != ')' ) {
00310       std::cerr
00311         << "Could not find z value and required close parenthesis "
00312         << "in input of a BasicVector3D<double>"
00313         << std::endl;
00314       return is;
00315     }
00316 
00317     a.setX(x);
00318     a.setY(y);
00319     a.setZ(z);
00320     return is;
00321   }
00322 } /* namespace HepGeom */