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