Math/GenVector/AxisAngle.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 AxisAngle
0012 //
0013 // Created by: Lorenzo Moneta  at Wed May 11 10:37:10 2005
0014 //
0015 // Last update: Wed May 11 10:37:10 2005
0016 //
0017 #ifndef ROOT_Math_GenVector_AxisAngle
0018 #define ROOT_Math_GenVector_AxisAngle  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 <algorithm>
0026 #include <cassert>
0027
0028
0029 namespace ROOT {
0030 namespace Math {
0031
0032
0033 //__________________________________________________________________________________________
0034    /**
0035       AxisAngle class describing rotation represented with direction axis (3D Vector) and an
0036       angle of rotation around that axis.
0037
0038       @ingroup GenVector
0039
0040       @sa Overview of the @ref GenVector "physics vector library"
0041    */
0042 class AxisAngle {
0043
0044 public:
0045
0046    typedef double Scalar;
0047
0048    /**
0049       definition of vector axis
0050    */
0051    typedef DisplacementVector3D<Cartesian3D<Scalar> > AxisVector;
0052
0053
0054    /**
0055       Default constructor (axis is z and angle is zero)
0056    */
0057    AxisAngle() : fAxis(0,0,1), fAngle(0) { }
0058
0059    /**
0060       Construct from a non-zero vector (x,y,z) and an angle.
0061       Precondition:  the Vector needs to implement x(), y(), z(), and unit()
0062    */
0063    template<class AnyVector>
0064    AxisAngle(const AnyVector & v, Scalar angle) :
0065       fAxis(v.unit()), fAngle(angle) { }
0066
0067    /**
0068       Construct given a pair of pointers or iterators defining the
0069       beginning and end of an array of four Scalars, to be treated as
0070       the x, y, and z components of a unit axis vector, and the angle
0071       of rotation.
0072       Precondition:  The first three components are assumed to represent
0073       the rotation axis vector and the 4-th the rotation angle.
0074       The angle is assumed to be in the range (-pi,pi].
0075       The axis vector is automatically normalized to be a unit vector
0076    */
0077    template<class IT>
0078    AxisAngle(IT begin, IT end) { SetComponents(begin,end); }
0079
0080    // The compiler-generated copy ctor, copy assignment, and dtor are OK.
0081
0082    /**
0083       Re-adjust components to eliminate small deviations from the axis
0084       being a unit vector and angles out of the canonical range (-pi,pi]
0085    */
0086    void Rectify();
0087
0088    // ======== Construction From other Rotation Forms ==================
0089
0090    /**
0091       Construct from another supported rotation type (see gv_detail::convert )
0092    */
0093    template <class OtherRotation>
0094    explicit constexpr AxisAngle(const OtherRotation & r) {gv_detail::convert(r,*this);}
0095
0096
0097    /**
0098       Assign from another supported rotation type (see gv_detail::convert )
0099    */
0100    template <class OtherRotation>
0101    AxisAngle & operator=( OtherRotation const  & r ) {
0102       gv_detail::convert(r,*this);
0103       return *this;
0104    }
0105
0106    // ======== Components ==============
0107
0108    /**
0109       Set the axis and then the angle given a pair of pointers or iterators
0110       defining the beginning and end of an array of four Scalars.
0111       Precondition:  The first three components are assumed to represent
0112       the rotation axis vector and the 4-th the rotation angle.
0113       The angle is assumed to be in the range (-pi,pi].
0114       The axis vector is automatically normalized to be a unit vector
0115    */
0116    template<class IT>
0117    void SetComponents(IT begin, IT end) {
0118       IT a = begin; IT b = ++begin; IT c = ++begin;
0119       fAxis.SetCoordinates(*a,*b,*c);
0120       fAngle = *(++begin);
0121       (void)end;
0122       assert (++begin==end);
0123       // re-normalize the vector
0124       double tot = fAxis.R();
0125       if (tot >  0) fAxis /= tot;
0126    }
0127
0128    /**
0129       Get the axis and then the angle into data specified by an iterator begin
0130       and another to the end of the desired data (4 past start).
0131    */
0132    template<class IT>
0133    void GetComponents(IT begin, IT end) const {
0134       IT a = begin; IT b = ++begin; IT c = ++begin;
0135       fAxis.GetCoordinates(*a,*b,*c);
0136       *(++begin) = fAngle;
0137       (void)end;
0138       assert (++begin==end);
0139    }
0140
0141    /**
0142       Get the axis and then the angle into data specified by an iterator begin
0143    */
0144    template<class IT>
0145    void GetComponents(IT begin) const {
0146       double ax,ay,az = 0;
0147       fAxis.GetCoordinates(ax,ay,az);
0148       *begin++ = ax;
0149       *begin++ = ay;
0150       *begin++ = az;
0151       *begin = fAngle;
0152    }
0153
0154    /**
0155       Set components from a non-zero vector (x,y,z) and an angle.
0156       Precondition:  the Vector needs to implement x(), y(), z(), and unit()
0157    */
0158    template<class AnyVector>
0159    void SetComponents(const AnyVector & v, Scalar angle) {
0160       fAxis=v.unit();
0161       fAngle=angle;
0162    }
0163
0164    /**
0165       Set components into a non-zero vector (x,y,z) and an angle.
0166       The vector is intended to be a cartesian displacement vector
0167       but any vector class assignable from one will work.
0168    */
0169    template<class AnyVector>
0170    void GetComponents(AnyVector & axis, Scalar & angle) const {
0171       axis  = fAxis;
0172       angle = fAngle;
0173    }
0174
0175    /**
0176       access to rotation axis
0177    */
0178    AxisVector Axis() const { return fAxis; }
0179
0180    /**
0181       access to rotation angle
0182    */
0183    Scalar Angle() const { return fAngle; }
0184
0185    // =========== operations ==============
0186
0187    /**
0188       Rotation operation on a cartesian vector
0189    */
0190    typedef  DisplacementVector3D<Cartesian3D<double>, DefaultCoordinateSystemTag > XYZVector;
0191    XYZVector operator() (const XYZVector & v) const;
0192
0193    /**
0194       Rotation operation on a displacement vector in any coordinate system
0195    */
0196    template <class CoordSystem, class Tag>
0197    DisplacementVector3D<CoordSystem, Tag>
0198    operator() (const DisplacementVector3D<CoordSystem, Tag> & v) const {
0199       DisplacementVector3D< Cartesian3D<double> > xyz(v.X(), v.Y(), v.Z());
0200       DisplacementVector3D< Cartesian3D<double> > rxyz = operator()(xyz);
0201       DisplacementVector3D< CoordSystem, Tag > vNew;
0202       vNew.SetXYZ( rxyz.X(), rxyz.Y(), rxyz.Z() );
0203       return vNew;
0204    }
0205
0206    /**
0207       Rotation operation on a position vector in any coordinate system
0208    */
0209    template <class CoordSystem, class Tag>
0210    PositionVector3D<CoordSystem, Tag>
0211    operator() (const PositionVector3D<CoordSystem,Tag> & p) const {
0212       DisplacementVector3D< Cartesian3D<double>,Tag > xyz(p);
0213       DisplacementVector3D< Cartesian3D<double>,Tag > rxyz = operator()(xyz);
0214       return PositionVector3D<CoordSystem,Tag> ( rxyz );
0215    }
0216
0217    /**
0218       Rotation operation on a Lorentz vector in any 4D coordinate system
0219    */
0220    template <class CoordSystem>
0221    LorentzVector<CoordSystem>
0222    operator() (const LorentzVector<CoordSystem> & v) const {
0223       DisplacementVector3D< Cartesian3D<double> > xyz(v.Vect());
0224       xyz = operator()(xyz);
0225       LorentzVector< PxPyPzE4D<double> > xyzt (xyz.X(), xyz.Y(), xyz.Z(), v.E());
0226       return LorentzVector<CoordSystem> ( xyzt );
0227    }
0228
0229
0230    /**
0231       Rotation operation on an arbitrary vector v.
0232       Preconditions:  v must implement methods x(), y(), and z()
0233       and the arbitrary vector type must have a constructor taking (x,y,z)
0234    */
0235    template <class ForeignVector>
0236    ForeignVector
0237    operator() (const  ForeignVector & v) const {
0238       DisplacementVector3D< Cartesian3D<double> > xyz(v);
0239       DisplacementVector3D< Cartesian3D<double> > rxyz = operator()(xyz);
0240       return ForeignVector ( rxyz.X(), rxyz.Y(), rxyz.Z() );
0241    }
0242
0243    /**
0244       Overload operator * for rotation on a vector
0245    */
0246    template <class AVector>
0247    inline
0248    AVector operator* (const AVector & v) const
0249    {
0250       return operator()(v);
0251    }
0252
0253    /**
0254       Invert an AxisAngle rotation in place
0255    */
0256    void Invert() { fAngle = -fAngle; }
0257
0258    /**
0259       Return inverse of an AxisAngle rotation
0260    */
0261    AxisAngle Inverse() const { AxisAngle result(*this); result.Invert(); return result; }
0262
0263    // ========= Multi-Rotation Operations ===============
0264
0265    /**
0266       Multiply (combine) two rotations
0267    */
0268    AxisAngle operator * (const Rotation3D  & r) const;
0269    AxisAngle operator * (const AxisAngle   & a) const;
0270    AxisAngle operator * (const EulerAngles & e) const;
0271    AxisAngle operator * (const Quaternion  & q) const;
0272    AxisAngle operator * (const RotationZYX & r) const;
0273    AxisAngle operator * (const RotationX  & rx) const;
0274    AxisAngle operator * (const RotationY  & ry) const;
0275    AxisAngle operator * (const RotationZ  & rz) const;
0276
0277    /**
0278       Post-Multiply (on right) by another rotation :  T = T*R
0279    */
0280    template <class R>
0281    AxisAngle & operator *= (const R & r) { return *this = (*this)*r; }
0282
0283
0284    /**
0285       Distance between two rotations
0286    */
0287    template <class R>
0288    Scalar Distance ( const R & r ) const {return gv_detail::dist(*this,r);}
0289
0290    /**
0291       Equality/inequality operators
0292    */
0293    bool operator == (const AxisAngle & rhs) const {
0294       if( fAxis  != rhs.fAxis  )  return false;
0295       if( fAngle != rhs.fAngle )  return false;
0296       return true;
0297    }
0298    bool operator != (const AxisAngle & rhs) const {
0299       return ! operator==(rhs);
0300    }
0301
0302 private:
0303
0304    AxisVector  fAxis;      // rotation axis (3D vector)
0305    Scalar      fAngle;     // rotation angle
0306
0307    void RectifyAngle();
0308
0309    static double Pi() { return 3.14159265358979323; }
0310
0311 };  // AxisAngle
0312
0313 // ============ Class AxisAngle ends here ============
0314
0315 /**
0316    Distance between two rotations
0317  */
0318 template <class R>
0319 inline
0320 typename AxisAngle::Scalar
0321 Distance ( const AxisAngle& r1, const R & r2) {return gv_detail::dist(r1,r2);}
0322
0323 /**
0324    Multiplication of an axial rotation by an AxisAngle
0325  */
0326 AxisAngle operator* (RotationX const & r1, AxisAngle const & r2);
0327 AxisAngle operator* (RotationY const & r1, AxisAngle const & r2);
0328 AxisAngle operator* (RotationZ const & r1, AxisAngle const & r2);
0329
0330 /**
0331    Stream Output and Input
0332  */
0333   // TODO - I/O should be put in the manipulator form
0334
0335 std::ostream & operator<< (std::ostream & os, const AxisAngle & a);
0336
0337 } // namespace Math
0338 } // namespace ROOT
0339
0340
0341 #endif /* ROOT_Math_GenVector_AxisAngle  */