Math/GenVector/EulerAngles.h

0001 // @(#)root/mathcore:$Id$
0002 // Authors: W. Brown, M. Fischler, L. Moneta    2005
0003
0004  /**********************************************************************
0005   *                                                                    *
0006   * Copyright (c) 2005 , LCG ROOT MathLib Team                         *
0007   *                                                                    *
0008   *                                                                    *
0009   **********************************************************************/
0010
0011 // Header file for class EulerAngles
0012 //
0013 // Created by: Lorenzo Moneta  at Tue May 10 17:55:10 2005
0014 //
0015 // Last update: Tue May 10 17:55:10 2005
0016 //
0017 #ifndef ROOT_Math_GenVector_EulerAngles
0018 #define ROOT_Math_GenVector_EulerAngles  1
0019
0020 #include "Math/GenVector/Rotation3D.h"
0021 #include "Math/GenVector/DisplacementVector3D.h"
0022 #include "Math/GenVector/PositionVector3D.h"
0023 #include "Math/GenVector/LorentzVector.h"
0024 #include "Math/GenVector/3DConversions.h"
0025 #include "TMath.h"
0026 #include <algorithm>
0027 #include <cassert>
0028
0029 namespace ROOT {
0030 namespace Math {
0031
0032
0033 //__________________________________________________________________________________________
0034    /**
0035       EulerAngles class describing rotation as three angles (Euler Angles).
0036       The Euler angles definition matches that of Classical Mechanics (Goldstein).
0037       It is also the same convention defined in
0038       <A HREF="http://mathworld.wolfram.com/EulerAngles.html">mathworld</A>
0039       and used in Mathematica and CLHEP. Note that the ROOT class TRotation defines
0040       a slightly different convention.
0041
0042       @ingroup GenVector
0043
0044       @see GenVector
0045    */
0046 class EulerAngles {
0047
0048 public:
0049
0050    typedef double Scalar;
0051
0052   /**
0053      Default constructor
0054   */
0055    constexpr EulerAngles() : fPhi(0.0), fTheta(0.0), fPsi(0.0) { }
0056
0057    /**
0058       Constructor from phi, theta and psi
0059    */
0060    EulerAngles( Scalar phi, Scalar theta, Scalar psi ) :
0061       fPhi(phi), fTheta(theta), fPsi(psi)
0062    {Rectify();} // Added 27 Jan. 06   JMM
0063
0064    /**
0065       Construct given a pair of pointers or iterators defining the
0066       beginning and end of an array of three Scalars, to be treated as
0067       the angles phi, theta and psi.
0068    */
0069    template<class IT>
0070    constexpr EulerAngles(IT begin, IT end) { SetComponents(begin,end); }
0071
0072    // The compiler-generated copy ctor, copy assignment, and dtor are OK.
0073
0074    /**
0075       Re-adjust components place angles in canonical ranges
0076    */
0077    void Rectify();
0078
0079
0080    // ======== Construction and assignment from any other rotation ==================
0081
0082    /**
0083       Create from any other supported rotation (see gv_detail::convert )
0084     */
0085    template <class OtherRotation>
0086    explicit constexpr EulerAngles(const OtherRotation & r) {gv_detail::convert(r,*this);}
0087
0088    /**
0089       Assign from any other rotation (see gv_detail::convert )
0090    */
0091    template <class OtherRotation>
0092    EulerAngles &  operator=( OtherRotation const  & r ) {
0093       gv_detail::convert(r,*this);
0094       return *this;
0095    }
0096
0097 #ifdef OLD
0098    explicit EulerAngles(const Rotation3D & r) {gv_detail::convert(r,*this);}
0099
0100    /**
0101       Construct from a rotation matrix
0102    */
0103    explicit EulerAngles(const Rotation3D & r) {gv_detail::convert(r,*this);}
0104
0105    /**
0106       Construct from a rotation represented by a Quaternion
0107    */
0108    explicit EulerAngles(const Quaternion & q) {gv_detail::convert(q,*this);}
0109
0110    /**
0111       Construct from an AxisAngle
0112    */
0113    explicit EulerAngles(const AxisAngle & a ) { gv_detail::convert(a, *this); }
0114
0115    /**
0116       Construct from an axial rotation
0117    */
0118    explicit EulerAngles( RotationZ const & r ) { gv_detail::convert(r, *this); }
0119    explicit EulerAngles( RotationY const & r ) { gv_detail::convert(r, *this); }
0120    explicit EulerAngles( RotationX const & r ) { gv_detail::convert(r, *this); }
0121
0122
0123    /**
0124       Assign from an AxisAngle
0125    */
0126    EulerAngles &
0127    operator=( AxisAngle const & a ) { return operator=(EulerAngles(a)); }
0128
0129    /**
0130       Assign from a Quaternion
0131    */
0132    EulerAngles &
0133    operator=( Quaternion const  & q ) {return operator=(EulerAngles(q)); }
0134
0135    /**
0136       Assign from an axial rotation
0137    */
0138    EulerAngles &
0139    operator=( RotationZ const & r ) { return operator=(EulerAngles(r)); }
0140    EulerAngles &
0141    operator=( RotationY const & r ) { return operator=(EulerAngles(r)); }
0142    EulerAngles &
0143    operator=( RotationX const & r ) { return operator=(EulerAngles(r)); }
0144
0145 #endif
0146
0147    // ======== Components ==============
0148
0149    /**
0150       Set the three Euler angles given a pair of pointers or iterators
0151       defining the beginning and end of an array of three Scalars.
0152    */
0153    template<class IT>
0154    void SetComponents(IT begin, IT end) {
0155       fPhi   = *begin++;
0156       fTheta = *begin++;
0157       fPsi   = *begin++;
0158       (void)end;
0159       assert(begin == end);
0160       Rectify(); // Added 27 Jan. 06   JMM
0161    }
0162
0163    /**
0164       Get the axis and then the angle into data specified by an iterator begin
0165       and another to the end of the desired data (4 past start).
0166    */
0167    template<class IT>
0168    void GetComponents(IT begin, IT end) const {
0169       *begin++ = fPhi;
0170       *begin++ = fTheta;
0171       *begin++ = fPsi;
0172       (void)end;
0173       assert(begin == end);
0174    }
0175
0176    /**
0177       Get the axis and then the angle into data specified by an iterator begin
0178    */
0179    template<class IT>
0180    void GetComponents(IT begin) const {
0181       *begin++ = fPhi;
0182       *begin++ = fTheta;
0183       *begin   = fPsi;
0184    }
0185
0186    /**
0187       Set the components phi, theta, psi based on three Scalars.
0188    */
0189    void SetComponents(Scalar phi, Scalar theta, Scalar psi) {
0190       fPhi=phi; fTheta=theta; fPsi=psi;
0191       Rectify(); // Added 27 Jan. 06   JMM
0192    }
0193
0194    /**
0195       Get the components phi, theta, psi into three Scalars.
0196    */
0197    void GetComponents(Scalar & phi, Scalar & theta, Scalar & psi) const {
0198       phi=fPhi; theta=fTheta; psi=fPsi;
0199    }
0200
0201    /**
0202       Set Phi Euler angle // JMM 30 Jan. 2006
0203    */
0204    void SetPhi(Scalar phi) { fPhi=phi; Rectify(); }
0205
0206    /**
0207       Return Phi Euler angle
0208    */
0209    Scalar Phi() const { return fPhi; }
0210
0211    /**
0212       Set Theta Euler angle // JMM 30 Jan. 2006
0213    */
0214    void SetTheta(Scalar theta) { fTheta=theta; Rectify(); }
0215
0216    /**
0217       Return Theta Euler angle
0218    */
0219    Scalar Theta() const { return fTheta; }
0220
0221    /**
0222       Set Psi Euler angle // JMM 30 Jan. 2006
0223    */
0224    void SetPsi(Scalar psi) { fPsi=psi; Rectify(); }
0225
0226    /**
0227       Return Psi Euler angle
0228    */
0229    Scalar Psi() const { return fPsi; }
0230
0231    // =========== operations ==============
0232
0233
0234    /**
0235       Rotation operation on a displacement vector in any coordinate system and tag
0236    */
0237    template <class CoordSystem, class U>
0238    DisplacementVector3D<CoordSystem,U>
0239    operator() (const DisplacementVector3D<CoordSystem,U> & v) const {
0240       return Rotation3D(*this) ( v );
0241    }
0242
0243    /**
0244       Rotation operation on a position vector in any coordinate system
0245    */
0246    template <class CoordSystem, class U>
0247    PositionVector3D<CoordSystem, U>
0248    operator() (const PositionVector3D<CoordSystem,U> & v) const {
0249       DisplacementVector3D< Cartesian3D<double>,U > xyz(v);
0250       DisplacementVector3D< Cartesian3D<double>,U > rxyz = operator()(xyz);
0251       return PositionVector3D<CoordSystem,U> ( rxyz );
0252    }
0253
0254    /**
0255       Rotation operation on a Lorentz vector in any 4D coordinate system
0256    */
0257    template <class CoordSystem>
0258    LorentzVector<CoordSystem>
0259    operator() (const LorentzVector<CoordSystem> & v) const {
0260       DisplacementVector3D< Cartesian3D<double> > xyz(v.Vect());
0261       xyz = operator()(xyz);
0262       LorentzVector< PxPyPzE4D<double> > xyzt (xyz.X(), xyz.Y(), xyz.Z(), v.E());
0263       return LorentzVector<CoordSystem> ( xyzt );
0264    }
0265
0266    /**
0267       Rotation operation on an arbitrary vector v.
0268       Preconditions:  v must implement methods x(), y(), and z()
0269       and the arbitrary vector type must have a constructor taking (x,y,z)
0270    */
0271    template <class ForeignVector>
0272    ForeignVector
0273    operator() (const  ForeignVector & v) const {
0274       DisplacementVector3D< Cartesian3D<double> > xyz(v);
0275       DisplacementVector3D< Cartesian3D<double> > rxyz = operator()(xyz);
0276       return ForeignVector ( rxyz.X(), rxyz.Y(), rxyz.Z() );
0277    }
0278
0279    /**
0280       Overload operator * for rotation on a vector
0281    */
0282    template <class AVector>
0283    inline
0284    AVector operator* (const AVector & v) const
0285    {
0286       return operator()(v);
0287    }
0288
0289    /**
0290       Invert a rotation in place
0291    */
0292    // theta stays the same and negative rotation in Theta is done via a rotation
0293    // of + PI in phi and Psi
0294    void Invert() {
0295       Scalar tmp = -fPhi;
0296       fPhi = -fPsi + Pi();
0297       fPsi=tmp + Pi();
0298    }
0299
0300    /**
0301       Return inverse of a rotation
0302    */
0303    EulerAngles Inverse() const { return EulerAngles(-fPsi + Pi(), fTheta, -fPhi + Pi()); }
0304
0305    // ========= Multi-Rotation Operations ===============
0306
0307    /**
0308       Multiply (combine) two rotations
0309    */
0310    EulerAngles operator * (const Rotation3D  & r) const;
0311    EulerAngles operator * (const AxisAngle   & a) const;
0312    EulerAngles operator * (const EulerAngles & e) const;
0313    EulerAngles operator * (const Quaternion  & q) const;
0314    EulerAngles operator * (const RotationX  & rx) const;
0315    EulerAngles operator * (const RotationY  & ry) const;
0316    EulerAngles operator * (const RotationZ  & rz) const;
0317
0318    /**
0319       Post-Multiply (on right) by another rotation :  T = T*R
0320    */
0321    template <class R>
0322    EulerAngles & operator *= (const R & r) { return *this = (*this)*r; }
0323
0324    /**
0325       Distance between two rotations
0326    */
0327    template <class R>
0328    Scalar Distance ( const R & r ) const {return gv_detail::dist(*this,r);}
0329
0330    /**
0331       Equality/inequality operators
0332    */
0333    bool operator == (const EulerAngles & rhs) const {
0334       if( fPhi   != rhs.fPhi   ) return false;
0335       if( fTheta != rhs.fTheta ) return false;
0336       if( fPsi   != rhs.fPsi   ) return false;
0337       return true;
0338    }
0339    bool operator != (const EulerAngles & rhs) const {
0340       return ! operator==(rhs);
0341    }
0342
0343 private:
0344
0345    double fPhi;     // Z rotation angle (first)  defined in [-PI,PI]
0346    double fTheta;   // X rotation angle (second) defined only [0,PI]
0347    double fPsi;     // Z rotation angle (third)  defined in [-PI,PI]
0348
0349    static double Pi() { return TMath::Pi(); }
0350
0351 };  // EulerAngles
0352
0353 /**
0354    Distance between two rotations
0355  */
0356 template <class R>
0357 inline
0358 typename EulerAngles::Scalar
0359 Distance ( const EulerAngles& r1, const R & r2) {return gv_detail::dist(r1,r2);}
0360
0361 /**
0362    Multiplication of an axial rotation by an AxisAngle
0363  */
0364 EulerAngles operator* (RotationX const & r1, EulerAngles const & r2);
0365 EulerAngles operator* (RotationY const & r1, EulerAngles const & r2);
0366 EulerAngles operator* (RotationZ const & r1, EulerAngles const & r2);
0367
0368 /**
0369    Stream Output and Input
0370  */
0371   // TODO - I/O should be put in the manipulator form
0372
0373 std::ostream & operator<< (std::ostream & os, const EulerAngles & e);
0374
0375 } // namespace Math
0376 } // namespace ROOT
0377
0378
0379 #endif /* ROOT_Math_GenVector_EulerAngles  */