interface/private/oldTkRotation.h

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