Back to home page

Project CMSSW displayed by LXR

 
 

    


File indexing completed on 2021-02-14 12:53:30

0001 #ifndef Geom_oldTkRotation_H
0002 #define Geom_oldTkRotation_H
0003 
0004 #include "DataFormats/GeometryVector/interface/Basic2DVector.h"
0005 #include "DataFormats/GeometryVector/interface/Basic3DVector.h"
0006 #include "DataFormats/GeometryVector/interface/GlobalVector.h"
0007 /*
0008 #include "DataFormats/GeometrySurface/interface/GlobalError.h"
0009 #include "DataFormats/GeometrySurface/interface/LocalError.h"
0010 */
0011 #include <iosfwd>
0012 
0013 template <class T> class TkRotation;
0014 template <class T> class TkRotation2D;
0015 
0016 template <class T>
0017 std::ostream & operator<<( std::ostream& s, const TkRotation<T>& r);
0018 template <class T>
0019 std::ostream & operator<<( std::ostream& s, const TkRotation2D<T>& r);
0020 
0021 namespace geometryDetails {
0022   void TkRotationErr1();
0023   void TkRotationErr2();
0024 
0025 }
0026 
0027 
0028 /** Rotaion matrix used by Surface.
0029  */
0030 
0031 template <class T>
0032 class TkRotation {
0033 public:
0034   typedef Vector3DBase< T, GlobalTag>  GlobalVector;
0035 
0036   TkRotation() : 
0037     R11( 1), R12( 0), R13( 0), 
0038     R21( 0), R22( 1), R23( 0),
0039     R31( 0), R32( 0), R33( 1) {}
0040 
0041   TkRotation( T xx, T xy, T xz, T yx, T yy, T yz, T zx, T zy, T zz) :
0042     R11(xx), R12(xy), R13(xz), 
0043     R21(yx), R22(yy), R23(yz),
0044     R31(zx), R32(zy), R33(zz) {}
0045 
0046   TkRotation( const T* p) :
0047     R11(p[0]), R12(p[1]), R13(p[2]), 
0048     R21(p[3]), R22(p[4]), R23(p[5]),
0049     R31(p[6]), R32(p[7]), R33(p[8]) {}
0050 
0051   TkRotation( const GlobalVector & aX, const GlobalVector & aY)  {
0052 
0053     GlobalVector uX = aX.unit();
0054     GlobalVector uY = aY.unit();
0055     GlobalVector uZ(uX.cross(uY));
0056  
0057     R11 = uX.x(); R12 = uX.y();  R13 = uX.z();
0058     R21 = uY.x(); R22 = uY.y();  R23 = uY.z();
0059     R31 = uZ.x(); R32 = uZ.y();  R33 = uZ.z();
0060 
0061   }
0062 
0063   /** Construct from global vectors of the x, y and z axes.
0064    *  The axes are assumed to be unit vectors forming
0065    *  a right-handed orthonormal basis. No checks are performed!
0066    */
0067   TkRotation( const GlobalVector & aX, const GlobalVector & aY, 
0068           const GlobalVector & aZ) :
0069     R11( aX.x()), R12( aX.y()), R13( aX.z()), 
0070     R21( aY.x()), R22( aY.y()), R23( aY.z()),
0071     R31( aZ.x()), R32( aZ.y()), R33( aZ.z()) {}
0072     
0073 
0074   /** rotation around abritrary axis by the amount of phi:
0075    *  its constructed by  O^-1(z<->axis) rot_z(phi) O(z<->axis)
0076    *  the frame is rotated such that the z-asis corresponds to the rotation
0077    *  axis desired. THen it's rotated round the "new" z-axis, and then
0078    *  the initial transformation is "taken back" again.
0079    *  unfortuately I'm too stupid to describe such thing directly by 3 Euler
0080    *  angles.. hence I have to construckt it this way...by brute force
0081    */
0082   TkRotation( const Basic3DVector<T>& axis, T phi) :
0083     R11( cos(phi) ), R12( sin(phi)), R13( 0), 
0084     R21( -sin(phi)), R22( cos(phi)), R23( 0),
0085     R31( 0), R32( 0), R33( 1) {
0086 
0087     //rotation around the z-axis by  phi
0088     TkRotation tmpRotz ( cos(axis.phi()), sin(axis.phi()), 0.,
0089                 -sin(axis.phi()), cos(axis.phi()), 0.,
0090                          0.,              0.,              1. );
0091     //rotation around y-axis by theta
0092     TkRotation tmpRoty ( cos(axis.theta()), 0.,-sin(axis.theta()),
0093                          0.,              1.,              0.,
0094                  sin(axis.theta()), 0., cos(axis.theta()) );
0095     (*this)*=tmpRoty;
0096     (*this)*=tmpRotz;      // =  this * tmpRoty * tmpRotz 
0097    
0098     // (tmpRoty * tmpRotz)^-1 * this * tmpRoty * tmpRotz
0099 
0100     *this = (tmpRoty*tmpRotz).multiplyInverse(*this);
0101 
0102   }
0103   /* this is the same thing...derived from the CLHEP ... it gives the
0104      same results MODULO the sign of the rotation....  but I don't want
0105      that... had 
0106   TkRotation (const Basic3DVector<T>& axis, float phi) {
0107     T cx = axis.x();
0108     T cy = axis.y();
0109     T cz = axis.z();
0110     
0111     T ll = axis.mag();
0112     if (ll == 0) {
0113       geometryDetails::TkRotationErr1();
0114     }else{
0115       
0116       float cosa = cos(phi), sina = sin(phi);
0117       cx /= ll; cy /= ll; cz /= ll;   
0118       
0119        R11 = cosa + (1-cosa)*cx*cx;
0120        R12 =        (1-cosa)*cx*cy - sina*cz;
0121        R13 =        (1-cosa)*cx*cz + sina*cy;
0122       
0123        R21 =        (1-cosa)*cy*cx + sina*cz;
0124        R22 = cosa + (1-cosa)*cy*cy; 
0125        R23 =        (1-cosa)*cy*cz - sina*cx;
0126       
0127        R31 =        (1-cosa)*cz*cx - sina*cy;
0128        R32 =        (1-cosa)*cz*cy + sina*cx;
0129        R33 = cosa + (1-cosa)*cz*cz;
0130       
0131     }
0132     
0133   }
0134   */
0135 
0136   template <typename U>
0137   TkRotation( const TkRotation<U>& a) : 
0138     R11(a.xx()), R12(a.xy()), R13(a.xz()), 
0139     R21(a.yx()), R22(a.yy()), R23(a.yz()),
0140     R31(a.zx()), R32(a.zy()), R33(a.zz()) {}
0141 
0142   TkRotation transposed() const {
0143       return TkRotation( R11, R21, R31, 
0144              R12, R22, R32,
0145              R13, R23, R33);
0146   }
0147 
0148   Basic3DVector<T> operator*( const Basic3DVector<T>& v) const {
0149     return rotate(v);
0150   }
0151 
0152   Basic3DVector<T> rotate( const Basic3DVector<T>& v) const {
0153     return Basic3DVector<T>( R11*v.x() + R12*v.y() + R13*v.z(),
0154                  R21*v.x() + R22*v.y() + R23*v.z(),
0155                  R31*v.x() + R32*v.y() + R33*v.z());
0156   }
0157 
0158   Basic3DVector<T> multiplyInverse( const Basic3DVector<T>& v) const {
0159        return rotateBack(v);
0160    }
0161 
0162   Basic3DVector<T> rotateBack( const Basic3DVector<T>& v) const {
0163     return Basic3DVector<T>( R11*v.x() + R21*v.y() + R31*v.z(),
0164                  R12*v.x() + R22*v.y() + R32*v.z(),
0165                  R13*v.x() + R23*v.y() + R33*v.z());
0166   }
0167 
0168 
0169   Basic3DVector<T> operator*( const Basic2DVector<T>& v) const {
0170     return Basic3DVector<T>( R11*v.x() + R12*v.y(),
0171                  R21*v.x() + R22*v.y(),
0172                  R31*v.x() + R32*v.y());
0173   }
0174   Basic3DVector<T> multiplyInverse( const Basic2DVector<T>& v) const {
0175     return Basic3DVector<T>( R11*v.x() + R21*v.y(),
0176                  R12*v.x() + R22*v.y(),
0177                  R13*v.x() + R23*v.y());
0178   }
0179 
0180   
0181 
0182   TkRotation operator*( const TkRotation& b) const {
0183     return TkRotation(R11*b.R11 + R12*b.R21 + R13*b.R31,
0184               R11*b.R12 + R12*b.R22 + R13*b.R32,
0185               R11*b.R13 + R12*b.R23 + R13*b.R33,
0186               R21*b.R11 + R22*b.R21 + R23*b.R31,
0187               R21*b.R12 + R22*b.R22 + R23*b.R32,
0188               R21*b.R13 + R22*b.R23 + R23*b.R33,
0189               R31*b.R11 + R32*b.R21 + R33*b.R31,
0190               R31*b.R12 + R32*b.R22 + R33*b.R32,
0191               R31*b.R13 + R32*b.R23 + R33*b.R33);
0192   }
0193 
0194   TkRotation multiplyInverse( const TkRotation& b) const {
0195     return TkRotation(R11*b.R11 + R21*b.R21 + R31*b.R31,
0196               R11*b.R12 + R21*b.R22 + R31*b.R32,
0197               R11*b.R13 + R21*b.R23 + R31*b.R33,
0198               R12*b.R11 + R22*b.R21 + R32*b.R31,
0199               R12*b.R12 + R22*b.R22 + R32*b.R32,
0200               R12*b.R13 + R22*b.R23 + R32*b.R33,
0201               R13*b.R11 + R23*b.R21 + R33*b.R31,
0202               R13*b.R12 + R23*b.R22 + R33*b.R32,
0203               R13*b.R13 + R23*b.R23 + R33*b.R33);
0204   }
0205 
0206   TkRotation& operator*=( const TkRotation& b) {
0207     return *this = operator * (b);
0208   }
0209 
0210   // Note a *= b; <=> a = a * b; while a.transform(b); <=> a = b * a;
0211   TkRotation& transform(const TkRotation& b) {
0212     return *this = b.operator * (*this);
0213   }
0214 
0215   TkRotation & rotateAxes(const Basic3DVector<T>& newX,
0216               const Basic3DVector<T>& newY,
0217               const Basic3DVector<T>& newZ) {
0218     T del = 0.001;
0219 
0220     if (
0221     
0222     // the check for right-handedness is not needed since
0223     // we want to change the handedness when it's left in cmsim
0224     //
0225     //       fabs(newZ.x()-w.x()) > del ||
0226     //       fabs(newZ.y()-w.y()) > del ||
0227     //       fabs(newZ.z()-w.z()) > del ||
0228     fabs(newX.mag2()-1.) > del ||
0229     fabs(newY.mag2()-1.) > del || 
0230     fabs(newZ.mag2()-1.) > del ||
0231     fabs(newX.dot(newY)) > del ||
0232     fabs(newY.dot(newZ)) > del ||
0233     fabs(newZ.dot(newX)) > del) {
0234       geometryDetails::TkRotationErr2();
0235       return *this;
0236     } else {
0237       return transform(TkRotation(newX.x(), newY.x(), newZ.x(),
0238                   newX.y(), newY.y(), newZ.y(),
0239                   newX.z(), newY.z(), newZ.z()));
0240     }
0241   }
0242 
0243   Basic3DVector<T> x() const { return  Basic3DVector<T>(xx(),xy(),xz());}
0244   Basic3DVector<T> y() const { return  Basic3DVector<T>(yx(),yy(),yz());}
0245   Basic3DVector<T> z() const { return  Basic3DVector<T>(zx(),zy(),zz());}
0246 
0247 
0248   T const &xx() const { return R11;} 
0249   T const &xy() const { return R12;} 
0250   T const &xz() const { return R13;} 
0251   T const &yx() const { return R21;} 
0252   T const &yy() const { return R22;} 
0253   T const &yz() const { return R23;} 
0254   T const &zx() const { return R31;} 
0255   T const &zy() const { return R32;} 
0256   T const &zz() const { return R33;} 
0257 
0258 private:
0259 
0260   T R11, R12, R13;
0261   T R21, R22, R23;
0262   T R31, R32, R33;
0263 };
0264 
0265 
0266 template<>
0267 std::ostream & operator<< <float>( std::ostream& s, const TkRotation<float>& r);
0268 
0269 template<>
0270 std::ostream & operator<< <double>( std::ostream& s, const TkRotation<double>& r);
0271 
0272 
0273 template <class T, class U>
0274 inline Basic3DVector<U> operator*( const TkRotation<T>& r, const Basic3DVector<U>& v) {
0275   return Basic3DVector<U>( r.xx()*v.x() + r.xy()*v.y() + r.xz()*v.z(),
0276                r.yx()*v.x() + r.yy()*v.y() + r.yz()*v.z(),
0277                r.zx()*v.x() + r.zy()*v.y() + r.zz()*v.z());
0278 }
0279 
0280 template <class T, class U>
0281 inline TkRotation<typename PreciseFloatType<T,U>::Type>
0282 operator*( const TkRotation<T>& a, const TkRotation<U>& b) {
0283     typedef TkRotation<typename PreciseFloatType<T,U>::Type> RT;
0284     return RT( a.xx()*b.xx() + a.xy()*b.yx() + a.xz()*b.zx(),
0285            a.xx()*b.xy() + a.xy()*b.yy() + a.xz()*b.zy(),
0286            a.xx()*b.xz() + a.xy()*b.yz() + a.xz()*b.zz(),
0287            a.yx()*b.xx() + a.yy()*b.yx() + a.yz()*b.zx(),
0288            a.yx()*b.xy() + a.yy()*b.yy() + a.yz()*b.zy(),
0289            a.yx()*b.xz() + a.yy()*b.yz() + a.yz()*b.zz(),
0290            a.zx()*b.xx() + a.zy()*b.yx() + a.zz()*b.zx(),
0291            a.zx()*b.xy() + a.zy()*b.yy() + a.zz()*b.zy(),
0292            a.zx()*b.xz() + a.zy()*b.yz() + a.zz()*b.zz());
0293 }
0294 
0295 
0296 template <class T>
0297 class TkRotation2D {
0298 public:
0299 
0300   typedef Basic2DVector<T> BasicVector;
0301 
0302 
0303 
0304   TkRotation2D( ){}
0305   
0306   TkRotation2D( T xx, T xy, T yx, T yy) {
0307     axis[0] = BasicVector(xx,xy);
0308     axis[1] =BasicVector(yx, yy);
0309   }
0310 
0311   TkRotation2D( const T* p) { 
0312     axis[0] = BasicVector(p[0],p[1]);
0313     axis[1] = BasicVector(p[2],p[3]);
0314   }
0315 
0316   TkRotation2D( const BasicVector & aX)  {
0317     
0318     BasicVector uX = aX.unit();
0319     BasicVector uY(-uX.y(),uX.x());
0320     
0321     axis[0]= uX;
0322     axis[1]= uY;
0323     
0324   }
0325 
0326   
0327   TkRotation2D( const BasicVector & uX, const BasicVector & uY) {
0328     axis[0]= uX;
0329     axis[1]= uY;
0330   }
0331   
0332   BasicVector x() const { return axis[0];}
0333   BasicVector y() const { return axis[1];}
0334 
0335 
0336   TkRotation2D transposed() const {
0337     return TkRotation2D(axis[0][0], axis[1][0],
0338             axis[0][1], axis[1][1]
0339             );
0340   }
0341   
0342   BasicVector rotate( const BasicVector& v) const {
0343     return transposed().rotateBack(v);
0344   }
0345 
0346   BasicVector rotateBack( const BasicVector& v) const {
0347     return v[0]*axis[0] +  v[1]*axis[1];
0348   }
0349 
0350 
0351 
0352  private:
0353   
0354   BasicVector axis[2];
0355  
0356 };
0357 
0358 
0359 template<>
0360 std::ostream & operator<< <float>( std::ostream& s, const TkRotation2D<float>& r);
0361 
0362 template<>
0363 std::ostream & operator<< <double>( std::ostream& s, const TkRotation2D<double>& r);
0364 
0365 
0366 #endif
0367 
0368 
0369 
0370