Math/GenVector/Rotation3D.h

0001 // @(#)root/mathcore:$Id$
0002 // Authors: W. Brown, M. Fischler, L. Moneta    2005
0003
0004  /**********************************************************************
0005   *                                                                    *
0006   * Copyright (c) 2005 , LCG ROOT FNAL MathLib Team                    *
0007   *                                                                    *
0008   *                                                                    *
0009   **********************************************************************/
0010
0011 // Header file for class Rotation in 3 dimensions, represented by 3x3 matrix
0012 //
0013 // Created by: Mark Fischler Thurs June 9  2005
0014 //
0015 // Last update: $Id$
0016 //
0017 #ifndef ROOT_Math_GenVector_Rotation3D
0018 #define ROOT_Math_GenVector_Rotation3D  1
0019
0020
0021 #include "Math/GenVector/Cartesian3D.h"
0022 #include "Math/GenVector/DisplacementVector3D.h"
0023 #include "Math/GenVector/PositionVector3D.h"
0024 #include "Math/GenVector/PxPyPzE4D.h"
0025 #include "Math/GenVector/LorentzVector.h"
0026 #include "Math/GenVector/3DConversions.h"
0027 #include "Math/GenVector/3DDistances.h"
0028
0029 #include "Math/GenVector/Rotation3Dfwd.h"
0030 #include "Math/GenVector/AxisAnglefwd.h"
0031 #include "Math/GenVector/EulerAnglesfwd.h"
0032 #include "Math/GenVector/Quaternionfwd.h"
0033 #include "Math/GenVector/RotationXfwd.h"
0034 #include "Math/GenVector/RotationYfwd.h"
0035 #include "Math/GenVector/RotationZfwd.h"
0036
0037 #include "Math/GenVector/GenVector_exception.h"
0038
0039 #include <algorithm>
0040 #include <cassert>
0041 #include <iostream>
0042
0043
0044 namespace ROOT {
0045 namespace Math {
0046
0047
0048 //__________________________________________________________________________________________
0049   /**
0050      Rotation class with the (3D) rotation represented by
0051      a 3x3 orthogonal matrix.
0052      This is the optimal representation for application to vectors.
0053      See also ROOT::Math::AxisAngle, ROOT::Math::EulerAngles, and ROOT::Math::Quaternion for
0054      classes which have conversion operators to Rotation3D.
0055
0056      All Rotations types (not only Rotation3D) can be applied to all 3D Vector classes
0057      (like ROOT::Math::DisplacementVector3D and ROOT::Math::PositionVector3D)
0058      and also to the 4D Vectors (ROOT::Math::LorentzVector classes), acting on the 3D components.
0059      A rotation operation is applied by using the operator() or the operator *.
0060      With the operator * is possible also to combine rotations.
0061      Note that the operator is NOT commutative, the order how the rotations are applied is relevant.
0062
0063      @ingroup GenVector
0064
0065      @see GenVector
0066   */
0067
0068 class Rotation3D {
0069
0070 public:
0071
0072    typedef double Scalar;
0073
0074    enum ERotation3DMatrixIndex {
0075       kXX = 0, kXY = 1, kXZ = 2
0076       , kYX = 3, kYY = 4, kYZ = 5
0077       , kZX = 6, kZY = 7, kZZ = 8
0078    };
0079
0080    // ========== Constructors and Assignment =====================
0081
0082    /**
0083       Default constructor (identity rotation)
0084    */
0085    Rotation3D();
0086
0087    /**
0088       Construct given a pair of pointers or iterators defining the
0089       beginning and end of an array of nine Scalars
0090    */
0091    template<class IT>
0092    Rotation3D(IT begin, IT end) { SetComponents(begin,end); }
0093
0094    /**
0095       copy constructor
0096    */
0097    Rotation3D ( Rotation3D const   & r ) {
0098       *this = r;
0099    }
0100
0101    /**
0102       Construct from an AxisAngle
0103    */
0104    explicit Rotation3D( AxisAngle const   & a ) { gv_detail::convert(a, *this); }
0105
0106    /**
0107       Construct from EulerAngles
0108    */
0109    explicit Rotation3D( EulerAngles const & e ) { gv_detail::convert(e, *this); }
0110
0111    /**
0112       Construct from RotationZYX
0113    */
0114    explicit Rotation3D( RotationZYX const & e ) { gv_detail::convert(e, *this); }
0115
0116    /**
0117       Construct from a Quaternion
0118    */
0119    explicit Rotation3D( Quaternion const  & q ) { gv_detail::convert(q, *this); }
0120
0121    /**
0122       Construct from an axial rotation
0123    */
0124    explicit Rotation3D( RotationZ const & r ) { gv_detail::convert(r, *this); }
0125    explicit Rotation3D( RotationY const & r ) { gv_detail::convert(r, *this); }
0126    explicit Rotation3D( RotationX const & r ) { gv_detail::convert(r, *this); }
0127
0128    /**
0129       Construct from a linear algebra matrix of size at least 3x3,
0130       which must support operator()(i,j) to obtain elements (0,0) thru (2,2).
0131       Precondition:  The matrix is assumed to be orthonormal.  No checking
0132       or re-adjusting is performed.
0133    */
0134    template<class ForeignMatrix>
0135    explicit constexpr Rotation3D(const ForeignMatrix & m) { SetComponents(m); }
0136
0137    /**
0138       Construct from three orthonormal vectors (which must have methods
0139       x(), y() and z()) which will be used as the columns of the rotation
0140       matrix.  The orthonormality will be checked, and values adjusted
0141       so that the result will always be a good rotation matrix.
0142    */
0143    template<class ForeignVector>
0144    Rotation3D(const ForeignVector& v1,
0145               const ForeignVector& v2,
0146               const ForeignVector& v3 ) { SetComponents(v1, v2, v3); }
0147
0148    // compiler generated destruuctor is ok
0149
0150    /**
0151       Raw constructor from nine Scalar components (without any checking)
0152    */
0153    Rotation3D(Scalar  xx, Scalar  xy, Scalar  xz,
0154               Scalar  yx, Scalar  yy, Scalar  yz,
0155               Scalar  zx, Scalar  zy, Scalar  zz)
0156    {
0157       SetComponents (xx, xy, xz, yx, yy, yz, zx, zy, zz);
0158    }
0159
0160    // need to implement assignment operator to avoid using the templated one
0161
0162    /**
0163       Assignment operator
0164    */
0165    Rotation3D &
0166    operator=( Rotation3D const   & rhs ) {
0167       SetComponents( rhs.fM[0], rhs.fM[1], rhs.fM[2],
0168                      rhs.fM[3], rhs.fM[4], rhs.fM[5],
0169                      rhs.fM[6], rhs.fM[7], rhs.fM[8] );
0170       return *this;
0171    }
0172
0173    /**
0174       Assign from an AxisAngle
0175    */
0176    Rotation3D &
0177    operator=( AxisAngle const   & a ) { return operator=(Rotation3D(a)); }
0178
0179    /**
0180       Assign from EulerAngles
0181    */
0182    Rotation3D &
0183    operator=( EulerAngles const & e ) { return operator=(Rotation3D(e)); }
0184
0185    /**
0186       Assign from RotationZYX
0187    */
0188    Rotation3D &
0189    operator=( RotationZYX const & r ) { return operator=(Rotation3D(r)); }
0190
0191    /**
0192       Assign from a Quaternion
0193    */
0194    Rotation3D &
0195    operator=( Quaternion const  & q ) {return operator=(Rotation3D(q)); }
0196
0197    /**
0198       Assign from an axial rotation
0199    */
0200    Rotation3D &
0201    operator=( RotationZ const & r ) { return operator=(Rotation3D(r)); }
0202    Rotation3D &
0203    operator=( RotationY const & r ) { return operator=(Rotation3D(r)); }
0204    Rotation3D &
0205    operator=( RotationX const & r ) { return operator=(Rotation3D(r)); }
0206
0207    /**
0208       Assign from an orthonormal linear algebra matrix of size 3x3,
0209       which must support operator()(i,j) to obtain elements (0,0) thru (2,2).
0210    */
0211    template<class ForeignMatrix>
0212    Rotation3D &
0213    operator=(const ForeignMatrix & m) {
0214       SetComponents( m(0,0), m(0,1), m(0,2),
0215                      m(1,0), m(1,1), m(1,2),
0216                      m(2,0), m(2,1), m(2,2) );
0217       return *this;
0218    }
0219
0220    /**
0221       Re-adjust components to eliminate small deviations from perfect
0222       orthonormality.
0223    */
0224    void Rectify();
0225
0226    // ======== Components ==============
0227
0228    /**
0229       Set components from three orthonormal vectors (which must have methods
0230       x(), y() and z()) which will be used as the columns of the rotation
0231       matrix.  The orthonormality will be checked, and values adjusted
0232       so that the result will always be a good rotation matrix.
0233    */
0234    template<class ForeignVector>
0235    void
0236    SetComponents (const ForeignVector& v1,
0237                   const ForeignVector& v2,
0238                   const ForeignVector& v3 ) {
0239       fM[kXX]=v1.x();  fM[kXY]=v2.x();  fM[kXZ]=v3.x();
0240       fM[kYX]=v1.y();  fM[kYY]=v2.y();  fM[kYZ]=v3.y();
0241       fM[kZX]=v1.z();  fM[kZY]=v2.z();  fM[kZZ]=v3.z();
0242       Rectify();
0243    }
0244
0245    /**
0246       Get components into three vectors which will be the (orthonormal)
0247       columns of the rotation matrix.  (The vector class must have a
0248       constructor from 3 Scalars.)
0249    */
0250    template<class ForeignVector>
0251    void
0252    GetComponents ( ForeignVector& v1,
0253                    ForeignVector& v2,
0254                    ForeignVector& v3 ) const {
0255       v1 = ForeignVector ( fM[kXX], fM[kYX], fM[kZX] );
0256       v2 = ForeignVector ( fM[kXY], fM[kYY], fM[kZY] );
0257       v3 = ForeignVector ( fM[kXZ], fM[kYZ], fM[kZZ] );
0258    }
0259
0260    /**
0261       Set the 9 matrix components given an iterator to the start of
0262       the desired data, and another to the end (9 past start).
0263    */
0264    template<class IT>
0265    void SetComponents(IT begin, IT end) {
0266       for (int i = 0; i <9; ++i) {
0267          fM[i] = *begin;
0268          ++begin;
0269       }
0270       (void)end;
0271       assert (end==begin);
0272    }
0273
0274    /**
0275       Get the 9 matrix components into data specified by an iterator begin
0276       and another to the end of the desired data (9 past start).
0277    */
0278    template<class IT>
0279    void GetComponents(IT begin, IT end) const {
0280       for (int i = 0; i <9; ++i) {
0281          *begin = fM[i];
0282          ++begin;
0283       }
0284       (void)end;
0285       assert (end==begin);
0286    }
0287
0288    /**
0289       Get the 9 matrix components into data specified by an iterator begin
0290    */
0291    template<class IT>
0292    void GetComponents(IT begin) const {
0293       std::copy ( fM, fM+9, begin );
0294    }
0295
0296    /**
0297       Set components from a linear algebra matrix of size at least 3x3,
0298       which must support operator()(i,j) to obtain elements (0,0) thru (2,2).
0299       Precondition:  The matrix is assumed to be orthonormal.  NO checking
0300       or re-adjusting is performed.
0301    */
0302    template<class ForeignMatrix>
0303    void
0304    SetRotationMatrix (const ForeignMatrix & m) {
0305       fM[kXX]=m(0,0);  fM[kXY]=m(0,1);  fM[kXZ]=m(0,2);
0306       fM[kYX]=m(1,0);  fM[kYY]=m(1,1);  fM[kYZ]=m(1,2);
0307       fM[kZX]=m(2,0);  fM[kZY]=m(2,1);  fM[kZZ]=m(2,2);
0308    }
0309
0310    /**
0311       Get components into a linear algebra matrix of size at least 3x3,
0312       which must support operator()(i,j) for write access to elements
0313       (0,0) thru (2,2).
0314    */
0315    template<class ForeignMatrix>
0316    void
0317    GetRotationMatrix (ForeignMatrix & m) const {
0318       m(0,0)=fM[kXX];  m(0,1)=fM[kXY];  m(0,2)=fM[kXZ];
0319       m(1,0)=fM[kYX];  m(1,1)=fM[kYY];  m(1,2)=fM[kYZ];
0320       m(2,0)=fM[kZX];  m(2,1)=fM[kZY];  m(2,2)=fM[kZZ];
0321    }
0322
0323    /**
0324       Set the components from nine scalars -- UNCHECKED for orthonormaility
0325    */
0326    void
0327    SetComponents (Scalar  xx, Scalar  xy, Scalar  xz,
0328                   Scalar  yx, Scalar  yy, Scalar  yz,
0329                   Scalar  zx, Scalar  zy, Scalar  zz) {
0330       fM[kXX]=xx;  fM[kXY]=xy;  fM[kXZ]=xz;
0331       fM[kYX]=yx;  fM[kYY]=yy;  fM[kYZ]=yz;
0332       fM[kZX]=zx;  fM[kZY]=zy;  fM[kZZ]=zz;
0333    }
0334
0335    /**
0336       Get the nine components into nine scalars
0337    */
0338    void
0339    GetComponents (Scalar &xx, Scalar &xy, Scalar &xz,
0340                   Scalar &yx, Scalar &yy, Scalar &yz,
0341                   Scalar &zx, Scalar &zy, Scalar &zz) const {
0342       xx=fM[kXX];  xy=fM[kXY];  xz=fM[kXZ];
0343       yx=fM[kYX];  yy=fM[kYY];  yz=fM[kYZ];
0344       zx=fM[kZX];  zy=fM[kZY];  zz=fM[kZZ];
0345    }
0346
0347    // =========== operations ==============
0348
0349    /**
0350       Access operator, used to have direct access to rotation matrix's entries
0351       \param i row index in {0,1,2}
0352       \param j column index in {0,1,2}
0353    */
0354    Scalar operator()(size_t i, size_t j) const
0355    {
0356       if (i < 3 && j < 3)
0357          return fM[i + 3 * j];
0358       else
0359          GenVector_Throw("Rotation3D::operator(size_t i, size_t j):\n"
0360                          "    indices i and j must range in {0,1,2}");
0361       return 0.0;
0362    }
0363
0364    /**
0365       Rotation operation on a displacement vector in any coordinate system
0366    */
0367    template <class CoordSystem, class U>
0368    DisplacementVector3D<CoordSystem,U>
0369    operator() (const DisplacementVector3D<CoordSystem,U> & v) const {
0370       DisplacementVector3D< Cartesian3D<double>,U > xyz;
0371       xyz.SetXYZ( fM[kXX] * v.X() + fM[kXY] * v.Y() + fM[kXZ] * v.Z() ,
0372                   fM[kYX] * v.X() + fM[kYY] * v.Y() + fM[kYZ] * v.Z() ,
0373                   fM[kZX] * v.X() + fM[kZY] * v.Y() + fM[kZZ] * v.Z() );
0374       return  DisplacementVector3D<CoordSystem,U>( xyz );
0375    }
0376
0377    /**
0378       Rotation operation on a position vector in any coordinate system
0379    */
0380    template <class CoordSystem, class U>
0381    PositionVector3D<CoordSystem,U>
0382    operator() (const PositionVector3D<CoordSystem,U> & v) const {
0383       DisplacementVector3D< Cartesian3D<double>,U > xyz(v);
0384       DisplacementVector3D< Cartesian3D<double>,U > rxyz = operator()(xyz);
0385       return PositionVector3D<CoordSystem,U> ( rxyz );
0386    }
0387
0388    /**
0389       Rotation operation on a Lorentz vector in any spatial coordinate system
0390    */
0391    template <class CoordSystem>
0392    LorentzVector<CoordSystem>
0393    operator() (const LorentzVector<CoordSystem> & v) const {
0394       DisplacementVector3D< Cartesian3D<double> > xyz(v.Vect());
0395       xyz = operator()(xyz);
0396       LorentzVector< PxPyPzE4D<double> > xyzt (xyz.X(), xyz.Y(), xyz.Z(), v.E());
0397       return LorentzVector<CoordSystem> ( xyzt );
0398    }
0399
0400    /**
0401       Rotation operation on an arbitrary vector v.
0402       Preconditions:  v must implement methods x(), y(), and z()
0403       and the arbitrary vector type must have a constructor taking (x,y,z)
0404    */
0405    template <class ForeignVector>
0406    ForeignVector
0407    operator() (const  ForeignVector & v) const {
0408       DisplacementVector3D< Cartesian3D<double> > xyz(v);
0409       DisplacementVector3D< Cartesian3D<double> > rxyz = operator()(xyz);
0410       return ForeignVector ( rxyz.X(), rxyz.Y(), rxyz.Z() );
0411    }
0412
0413    /**
0414       Overload operator * for rotation on a vector
0415    */
0416    template <class AVector>
0417    inline
0418    AVector operator* (const AVector & v) const
0419    {
0420       return operator()(v);
0421    }
0422
0423    /**
0424       Invert a rotation in place
0425    */
0426    void Invert();
0427
0428    /**
0429       Return inverse of  a rotation
0430    */
0431    Rotation3D Inverse() const { Rotation3D t(*this); t.Invert(); return t; }
0432
0433    // ========= Multi-Rotation Operations ===============
0434
0435    /**
0436       Multiply (combine) two rotations
0437    */
0438    Rotation3D operator * (const Rotation3D  & r) const {
0439    return Rotation3D
0440    (  fM[kXX]*r.fM[kXX] + fM[kXY]*r.fM[kYX] + fM[kXZ]*r.fM[kZX]
0441     , fM[kXX]*r.fM[kXY] + fM[kXY]*r.fM[kYY] + fM[kXZ]*r.fM[kZY]
0442     , fM[kXX]*r.fM[kXZ] + fM[kXY]*r.fM[kYZ] + fM[kXZ]*r.fM[kZZ]
0443
0444     , fM[kYX]*r.fM[kXX] + fM[kYY]*r.fM[kYX] + fM[kYZ]*r.fM[kZX]
0445     , fM[kYX]*r.fM[kXY] + fM[kYY]*r.fM[kYY] + fM[kYZ]*r.fM[kZY]
0446     , fM[kYX]*r.fM[kXZ] + fM[kYY]*r.fM[kYZ] + fM[kYZ]*r.fM[kZZ]
0447
0448     , fM[kZX]*r.fM[kXX] + fM[kZY]*r.fM[kYX] + fM[kZZ]*r.fM[kZX]
0449     , fM[kZX]*r.fM[kXY] + fM[kZY]*r.fM[kYY] + fM[kZZ]*r.fM[kZY]
0450     , fM[kZX]*r.fM[kXZ] + fM[kZY]*r.fM[kYZ] + fM[kZZ]*r.fM[kZZ]   );
0451
0452    }
0453
0454
0455    /**
0456       Multiplication with arbitrary rotations
0457     */
0458     // note: cannot have a  template method since it is ambiguous with the operator * on vectors
0459
0460    Rotation3D operator * (const AxisAngle   & a) const;
0461    Rotation3D operator * (const EulerAngles & e) const;
0462    Rotation3D operator * (const Quaternion  & q) const;
0463    Rotation3D operator * (const RotationZYX & r) const;
0464    Rotation3D operator * (const RotationX  & rx) const;
0465    Rotation3D operator * (const RotationY  & ry) const;
0466    Rotation3D operator * (const RotationZ  & rz) const;
0467
0468    /**
0469       Post-Multiply (on right) by another rotation :  T = T*R
0470    */
0471    template <class R>
0472    Rotation3D & operator *= (const R & r) { return *this = (*this)*r; }
0473
0474    /**
0475                     Equality/inequality operators
0476    */
0477    bool operator == (const Rotation3D & rhs) const {
0478       if( fM[0] != rhs.fM[0] )  return false;
0479       if( fM[1] != rhs.fM[1] )  return false;
0480       if( fM[2] != rhs.fM[2] )  return false;
0481       if( fM[3] != rhs.fM[3] )  return false;
0482       if( fM[4] != rhs.fM[4] )  return false;
0483       if( fM[5] != rhs.fM[5] )  return false;
0484       if( fM[6] != rhs.fM[6] )  return false;
0485       if( fM[7] != rhs.fM[7] )  return false;
0486       if( fM[8] != rhs.fM[8] )  return false;
0487       return true;
0488    }
0489    bool operator != (const Rotation3D & rhs) const {
0490       return ! operator==(rhs);
0491    }
0492
0493 private:
0494
0495    Scalar fM[9];  // 9 elements (3x3 matrix) representing the rotation
0496
0497 };  // Rotation3D
0498
0499 // ============ Class Rotation3D ends here ============
0500
0501 /**
0502    Distance between two rotations
0503  */
0504 template <class R>
0505 inline
0506 typename Rotation3D::Scalar
0507 Distance ( const Rotation3D& r1, const R & r2) {return gv_detail::dist(r1,r2);}
0508
0509 /**
0510    Multiplication of an axial rotation by a Rotation3D
0511  */
0512 Rotation3D operator* (RotationX const & r1, Rotation3D const & r2);
0513 Rotation3D operator* (RotationY const & r1, Rotation3D const & r2);
0514 Rotation3D operator* (RotationZ const & r1, Rotation3D const & r2);
0515
0516 /**
0517    Multiplication of an axial rotation by another axial Rotation
0518  */
0519 Rotation3D operator* (RotationX const & r1, RotationY const & r2);
0520 Rotation3D operator* (RotationX const & r1, RotationZ const & r2);
0521
0522 Rotation3D operator* (RotationY const & r1, RotationX const & r2);
0523 Rotation3D operator* (RotationY const & r1, RotationZ const & r2);
0524
0525 Rotation3D operator* (RotationZ const & r1, RotationX const & r2);
0526 Rotation3D operator* (RotationZ const & r1, RotationY const & r2);
0527
0528 /**
0529    Stream Output and Input
0530  */
0531   // TODO - I/O should be put in the manipulator form
0532
0533 std::ostream & operator<< (std::ostream & os, const Rotation3D & r);
0534
0535 } // namespace Math
0536 } // namespace ROOT
0537
0538 #endif // ROOT_Math_GenVector_Rotation3D