Math/GenVector/Quaternion.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 rotation in 3 dimensions, represented by a quaternion
0012 // Created by: Mark Fischler Thurs June 9  2005
0013 //
0014 // Last update: $Id$
0015 //
0016 #ifndef ROOT_Math_GenVector_Quaternion
0017 #define ROOT_Math_GenVector_Quaternion  1
0018
0019
0020 #include "Math/GenVector/Cartesian3D.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 "Math/GenVector/3DDistances.h"
0026
0027 #include <algorithm>
0028 #include <cassert>
0029
0030
0031 namespace ROOT {
0032 namespace Math {
0033
0034
0035 //__________________________________________________________________________________________
0036    /**
0037       Rotation class with the (3D) rotation represented by
0038       a unit quaternion (u, i, j, k).
0039       This is the optimal representation for multiplication of multiple
0040       rotations, and for computation of group-manifold-invariant distance
0041       between two rotations.
0042       See also ROOT::Math::AxisAngle, ROOT::Math::EulerAngles, and ROOT::Math::Rotation3D.
0043
0044       @ingroup GenVector
0045
0046       @sa Overview of the @ref GenVector "physics vector library"
0047    */
0048
0049 class Quaternion {
0050
0051 public:
0052
0053   typedef double Scalar;
0054
0055   // ========== Constructors and Assignment =====================
0056
0057   /**
0058       Default constructor (identity rotation)
0059   */
0060    Quaternion()
0061       : fU(1.0)
0062       , fI(0.0)
0063       , fJ(0.0)
0064       , fK(0.0)
0065    { }
0066
0067    /**
0068       Construct given a pair of pointers or iterators defining the
0069       beginning and end of an array of four Scalars
0070    */
0071    template<class IT>
0072    Quaternion(IT begin, IT end) { SetComponents(begin,end); }
0073
0074    // ======== Construction From other Rotation Forms ==================
0075
0076    /**
0077       Construct from another supported rotation type (see gv_detail::convert )
0078    */
0079    template <class OtherRotation>
0080    explicit constexpr Quaternion(const OtherRotation & r) {gv_detail::convert(r,*this);}
0081
0082
0083    /**
0084       Construct from four Scalars representing the coefficients of u, i, j, k
0085    */
0086    Quaternion(Scalar u, Scalar i, Scalar j, Scalar k) :
0087       fU(u), fI(i), fJ(j), fK(k) { }
0088
0089    // The compiler-generated copy ctor, copy assignment, and dtor are OK.
0090
0091    /**
0092       Re-adjust components to eliminate small deviations from |Q| = 1
0093       orthonormality.
0094    */
0095    void Rectify();
0096
0097    /**
0098       Assign from another supported rotation type (see gv_detail::convert )
0099    */
0100    template <class OtherRotation>
0101    Quaternion & operator=( OtherRotation const  & r ) {
0102       gv_detail::convert(r,*this);
0103       return *this;
0104    }
0105
0106    // ======== Components ==============
0107
0108    /**
0109       Set the four components given an iterator to the start of
0110       the desired data, and another to the end (4 past start).
0111    */
0112    template<class IT>
0113    void SetComponents(IT begin, IT end) {
0114       fU = *begin++;
0115       fI = *begin++;
0116       fJ = *begin++;
0117       fK = *begin++;
0118       (void)end;
0119       assert (end==begin);
0120    }
0121
0122    /**
0123       Get the components into data specified by an iterator begin
0124       and another to the end of the desired data (4 past start).
0125    */
0126    template<class IT>
0127    void GetComponents(IT begin, IT end) const {
0128       *begin++ = fU;
0129       *begin++ = fI;
0130       *begin++ = fJ;
0131       *begin++ = fK;
0132       (void)end;
0133       assert (end==begin);
0134    }
0135
0136    /**
0137       Get the components into data specified by an iterator begin
0138    */
0139    template<class IT>
0140    void GetComponents(IT begin ) const {
0141       *begin++ = fU;
0142       *begin++ = fI;
0143       *begin++ = fJ;
0144       *begin   = fK;
0145    }
0146
0147    /**
0148       Set the components based on four Scalars.  The sum of the squares of
0149       these Scalars should be 1; no checking is done.
0150    */
0151    void SetComponents(Scalar u, Scalar i, Scalar j, Scalar k) {
0152       fU=u; fI=i; fJ=j; fK=k;
0153    }
0154
0155    /**
0156       Get the components into four Scalars.
0157    */
0158    void GetComponents(Scalar & u, Scalar & i, Scalar & j, Scalar & k) const {
0159       u=fU; i=fI; j=fJ; k=fK;
0160    }
0161
0162    /**
0163       Access to the four quaternion components:
0164       U() is the coefficient of the identity Pauli matrix,
0165       I(), J() and K() are the coefficients of sigma_x, sigma_y, sigma_z
0166    */
0167    Scalar U() const { return fU; }
0168    Scalar I() const { return fI; }
0169    Scalar J() const { return fJ; }
0170    Scalar K() const { return fK; }
0171
0172    // =========== operations ==============
0173
0174    /**
0175       Rotation operation on a cartesian vector
0176    */
0177    typedef  DisplacementVector3D<Cartesian3D<double>, DefaultCoordinateSystemTag > XYZVector;
0178    XYZVector operator() (const XYZVector & v) const {
0179
0180       const Scalar alpha = fU*fU - fI*fI - fJ*fJ - fK*fK;
0181       const Scalar twoQv = 2*(fI*v.X() + fJ*v.Y() + fK*v.Z());
0182       const Scalar twoU  = 2 * fU;
0183       return XYZVector  (  alpha * v.X() + twoU * (fJ*v.Z() - fK*v.Y()) + twoQv * fI ,
0184                            alpha * v.Y() + twoU * (fK*v.X() - fI*v.Z()) + twoQv * fJ ,
0185                            alpha * v.Z() + twoU * (fI*v.Y() - fJ*v.X()) + twoQv * fK );
0186    }
0187
0188    /**
0189       Rotation operation on a displacement vector in any coordinate system
0190    */
0191    template <class CoordSystem,class Tag>
0192    DisplacementVector3D<CoordSystem,Tag>
0193    operator() (const DisplacementVector3D<CoordSystem,Tag> & v) const {
0194       DisplacementVector3D< Cartesian3D<double> > xyz(v.X(), v.Y(), v.Z());
0195       DisplacementVector3D< Cartesian3D<double> > rxyz = operator()(xyz);
0196       DisplacementVector3D< CoordSystem,Tag > vNew;
0197       vNew.SetXYZ( rxyz.X(), rxyz.Y(), rxyz.Z() );
0198       return vNew;
0199    }
0200
0201    /**
0202       Rotation operation on a position vector in any coordinate system
0203    */
0204    template <class CoordSystem, class Tag>
0205    PositionVector3D<CoordSystem,Tag>
0206    operator() (const PositionVector3D<CoordSystem,Tag> & p) const {
0207       DisplacementVector3D< Cartesian3D<double>,Tag > xyz(p);
0208       DisplacementVector3D< Cartesian3D<double>,Tag > rxyz = operator()(xyz);
0209       return PositionVector3D<CoordSystem,Tag> ( rxyz );
0210    }
0211
0212    /**
0213       Rotation operation on a Lorentz vector in any 4D coordinate system
0214    */
0215    template <class CoordSystem>
0216    LorentzVector<CoordSystem>
0217    operator() (const LorentzVector<CoordSystem> & v) const {
0218       DisplacementVector3D< Cartesian3D<double> > xyz(v.Vect());
0219       xyz = operator()(xyz);
0220       LorentzVector< PxPyPzE4D<double> > xyzt (xyz.X(), xyz.Y(), xyz.Z(), v.E());
0221       return LorentzVector<CoordSystem> ( xyzt );
0222    }
0223
0224    /**
0225       Rotation operation on an arbitrary vector v.
0226       Preconditions:  v must implement methods x(), y(), and z()
0227       and the arbitrary vector type must have a constructor taking (x,y,z)
0228    */
0229    template <class ForeignVector>
0230    ForeignVector
0231    operator() (const  ForeignVector & v) const {
0232       DisplacementVector3D< Cartesian3D<double> > xyz(v);
0233       DisplacementVector3D< Cartesian3D<double> > rxyz = operator()(xyz);
0234       return ForeignVector ( rxyz.X(), rxyz.Y(), rxyz.Z() );
0235    }
0236
0237    /**
0238       Overload operator * for rotation on a vector
0239    */
0240    template <class AVector>
0241    inline
0242    AVector operator* (const AVector & v) const
0243    {
0244       return operator()(v);
0245    }
0246
0247    /**
0248       Invert a rotation in place
0249    */
0250    void Invert() { fI = -fI; fJ = -fJ; fK = -fK; }
0251
0252    /**
0253       Return inverse of a rotation
0254    */
0255    Quaternion Inverse() const { return Quaternion(fU, -fI, -fJ, -fK); }
0256
0257    // ========= Multi-Rotation Operations ===============
0258
0259    /**
0260       Multiply (combine) two rotations
0261    */
0262    /**
0263       Multiply (combine) two rotations
0264    */
0265    Quaternion operator * (const Quaternion  & q) const {
0266       return Quaternion  (   fU*q.fU - fI*q.fI - fJ*q.fJ - fK*q.fK ,
0267                              fU*q.fI + fI*q.fU + fJ*q.fK - fK*q.fJ ,
0268                              fU*q.fJ - fI*q.fK + fJ*q.fU + fK*q.fI ,
0269                              fU*q.fK + fI*q.fJ - fJ*q.fI + fK*q.fU  );
0270    }
0271
0272    Quaternion operator * (const Rotation3D  & r) const;
0273    Quaternion operator * (const AxisAngle   & a) const;
0274    Quaternion operator * (const EulerAngles & e) const;
0275    Quaternion operator * (const RotationZYX & r) const;
0276    Quaternion operator * (const RotationX  & rx) const;
0277    Quaternion operator * (const RotationY  & ry) const;
0278    Quaternion operator * (const RotationZ  & rz) const;
0279
0280    /**
0281       Post-Multiply (on right) by another rotation :  T = T*R
0282    */
0283    template <class R>
0284    Quaternion & operator *= (const R & r) { return *this = (*this)*r; }
0285
0286
0287    /**
0288       Distance between two rotations in Quaternion form
0289       Note:  The rotation group is isomorphic to a 3-sphere
0290       with diametrically opposite points identified.
0291       The (rotation group-invariant) is the smaller
0292       of the two possible angles between the images of
0293       the two totations on that sphere.  Thus the distance
0294       is never greater than pi/2.
0295    */
0296
0297    Scalar Distance(const Quaternion & q) const ;
0298
0299    /**
0300       Equality/inequality operators
0301    */
0302    bool operator == (const Quaternion & rhs) const {
0303       if( fU != rhs.fU )  return false;
0304       if( fI != rhs.fI )  return false;
0305       if( fJ != rhs.fJ )  return false;
0306       if( fK != rhs.fK )  return false;
0307       return true;
0308    }
0309    bool operator != (const Quaternion & rhs) const {
0310       return ! operator==(rhs);
0311    }
0312
0313 private:
0314
0315    Scalar fU;
0316    Scalar fI;
0317    Scalar fJ;
0318    Scalar fK;
0319
0320 };  // Quaternion
0321
0322 // ============ Class Quaternion ends here ============
0323
0324 /**
0325    Distance between two rotations
0326  */
0327 template <class R>
0328 inline
0329 typename Quaternion::Scalar
0330 Distance ( const Quaternion& r1, const R & r2) {return gv_detail::dist(r1,r2);}
0331
0332 /**
0333    Multiplication of an axial rotation by an AxisAngle
0334  */
0335 Quaternion operator* (RotationX const & r1, Quaternion const & r2);
0336 Quaternion operator* (RotationY const & r1, Quaternion const & r2);
0337 Quaternion operator* (RotationZ const & r1, Quaternion const & r2);
0338
0339 /**
0340    Stream Output and Input
0341  */
0342   // TODO - I/O should be put in the manipulator form
0343
0344 std::ostream & operator<< (std::ostream & os, const Quaternion & q);
0345
0346
0347 }  // namespace Math
0348 }  // namespace ROOT
0349
0350 #endif // ROOT_Math_GenVector_Quaternion