// @(#)root/physics:$Name:  $:$Id: TQuaternion.cxx,v 1.2 2005/09/04 09:51:19 brun Exp $

// Author: Eric Anciant 28/06/2005



//////////////////////////////////////////////////////////////////////////

//____________________

//                                                                      

//  A Quaternion Class 

// 
//

Quaternion is a 4-component mathematic object quite convenient when dealing with // space rotation (or reference frame transformation).

//

//

In short, think of quaternion Q as a 3-vector augmented by a real number. Q = Q|r + Q|V // //

Quaternion multiplication : //

//

Quaternion multiplication is given by : //
Q.Q' = (Q|r + Q|V )*( Q'|r + Q'|V)
//
= [ Q|r*Q'|r - Q|V*Q'|V ] + [ Q|r*Q'|V + Q'|r*Q|V + Q|V X Q'|V ]
//
//
where :
//
Q|r*Q'|r is a real number product of real numbers
//
Q|V*Q'|V is a real number, scalar product of two 3-vectors
//
Q|r*Q'|V is a 3-vector, scaling of a 3-vector by a real number
//
Q|VXQ'|V is a 3-vector, cross product of two 3-vectors
//
//
Thus, quaternion product is a generalization of real number product and product of a vector by a real number. Product of two pure vectors gives a quaternion whose real part is the opposite of scalar product and the vector part the cross product…
//

// //

The conjugate of a quaternion Q = Q|r + Q|V is Q_bar = Q|r - Q|V //

//

The magnitude of a quaternion Q is given by |Q|² = Q.Q_bar = Q_bar.Q = Q²|r + |Q|V //

//

Therefore, the inverse of a quaternion is Q-1 = Q_bar /|Q|² //

//

"unit" quaternion is a quaternion of magnitude 1 : |Q|² = 1. //
Unit quaternions are a subset of the quaternions set.
//

// //

Quaternion and rotations : //

// //

A rotation of angle f around a given axis, is represented by a unit quaternion Q : //
- The axis of the rotation is given by the vector part of Q.
//
- The ratio between the magnitude of the vector part and the real part of Q equals tan(f/2).
//

//

In other words : Q = Q|r + Q|V = cos(f/2) + sin(f/2).u //
(where u is a unit vector // to the rotation axis,
// cos(f/2) is the real part, sin(f/2).u is the vector part) //
Note : The quaternion of identity is QI = cos(0) + sin(0)*(any vector) = 1.
//

//

The composition of two rotations is described by the product of the two corresponding quaternions. //
As for 3-space rotations, quaternion multiplication is not commutative !
//
//
Q = Q1.Q2 represents the composition of the successive rotation R1 and R2 expressed in the current frame (the axis of rotation hold by Q2 is expressed in the frame as it is after R1 rotation).
//
Q = Q2.Q1 represents the composition of the successive rotation R1 and R2 expressed in the initial reference frame.
//

//

The inverse of a rotation is a rotation about the same axis but of opposite angle, thus if Q is a unit quaternion, //
Q = cos(f/2) + sin(f/2).u = Q|r + Q|V, then :
//
Q-1 =cos(-f/2) + sin(-f/2).u = cos(f/2) - sin(f/2).u = Q|r -Q|V is its inverse quaternion.
//

//

One verifies that : //
Q.Q-1 = Q-1.Q = Q|r*Q|r + Q|V*Q|V + Q|r*Q|V -Q|r*Q|V + Q|VXQ|V
//
= Q²|r + Q²|V = 1
//

//
//

The rotation of a vector V by the rotation described by a unit quaternion Q is obtained by the following operation : V' = Q*V*Q-1, considering V as a quaternion whose real part is null. //

//

Numeric computation considerations : //

//

Numerically, the quaternion multiplication involves 12 additions and 16 multiplications. //
It is therefore faster than 3x3 matrixes multiplication involving 18 additions and 27 multiplications.
//
//
On the contrary, rotation of a vector by the above formula ( Q*V*Q-1 ) involves 18 additions and 24 multiplications, whereas multiplication of a 3-vector by a 3x3 matrix involves only 6 additions and 9 multiplications.
//
//
When dealing with numerous composition of space rotation, it is therefore faster to use quaternion product. On the other hand if a huge set of vectors must be rotated by a given quaternion, it is more optimized to convert the quaternion into a rotation matrix once, and then use that later to rotate the set of vectors.
//

//

More information : //

//

// // en.wikipedia.org/wiki/Quaternions_and_spatial_rotation . //

// // en.wikipedia.org/wiki/Quaternion . //

//

_______________________________________________ //
//

This Class represents all quaternions (unit or non-unit) //
It possesses a Normalize() method to make a given quaternion unit
//
The Rotate(TVector3&) and Rotation(TVector3&) methods can be used even for a non-unit quaternion, in that case, the proper normalization is applied to perform the rotation.
//
//
A TRotation constructor exists than takes a quaternion for parameter (even non-unit), in that cas the proper normalisation is applied.
//

//


#include "TQuaternion.h"

ClassImp(TQuaternion)

/****************** CONSTRUCTORS ****************************************************/
//______________________________________________________________________________

 TQuaternion::TQuaternion(const TQuaternion & q) : TObject(q),
  fRealPart(q.fRealPart), fVectorPart(q.fVectorPart) {}

 TQuaternion::TQuaternion(const TVector3 & vect, Double_t real)
        : fRealPart(real), fVectorPart(vect)  {}

 TQuaternion::TQuaternion(const Double_t * x0)
        : fRealPart(x0[3]), fVectorPart(x0) {}

 TQuaternion::TQuaternion(const Float_t * x0)
        : fRealPart(x0[3]), fVectorPart(x0) {}

 TQuaternion::TQuaternion(Double_t real, Double_t X, Double_t Y, Double_t Z)
        : fRealPart(real), fVectorPart(X,Y,Z) {}

 TQuaternion::~TQuaternion() {}

//______________________________________________________________________________

Double_t TQuaternion::operator () (int i) const {
  switch(i) {
  case 0:
  case 1:
  case 2:
          return fVectorPart(i);
  case 3:
          return fRealPart;
  default:
    Error("operator()(i)", "bad index (%d) returning 0",i);
  }
  return 0.;
}

//______________________________________________________________________________

Double_t & TQuaternion::operator () (int i) {
  switch(i) {
  case 0:
  case 1:
  case 2:
          return fVectorPart(i);
  case 3:
          return fRealPart;
  default:
    Error("operator()(i)", "bad index (%d) returning &fRealPart",i);
  }
  return fRealPart;
}
//_____________________________________

 Double_t TQuaternion::GetQAngle() const {
        // Get angle of quaternion (rad)

        // N.B : this angle is half of the corresponding rotation angle


        if (fRealPart == 0) return 0;
        Double_t denominator = fVectorPart.Mag();
        return atan(denominator/fRealPart);
}

//_____________________________________

 TQuaternion& TQuaternion::SetQAngle(Double_t angle) {
        // Set angle of quaternion (rad) - keep quaternion norm

        // N.B : this angle is half of the corresponding rotation angle

 
        Double_t norm = Norm();
        Double_t normSinV = fVectorPart.Mag();
        if (normSinV != 0) fVectorPart *= (sin(angle)*norm/normSinV);
        fRealPart = cos(angle)*norm;

        return (*this);
}

//_____________________________________

 TQuaternion& TQuaternion::SetAxisQAngle(TVector3& v,Double_t QAngle) {
        // set quaternion from vector and angle (rad)

        // quaternion is set as unitary 

        // N.B : this angle is half of the corresponding rotation angle

        
        fVectorPart = v;
        double norm = v.Mag();
        if (norm>0) fVectorPart*=(1./norm);
        fVectorPart*=sin(QAngle);
        fRealPart = cos(QAngle);

        return (*this);
}

/**************** REAL TO QUATERNION ALGEBRA ****************************************/ 

//_____________________________________

TQuaternion TQuaternion::operator+(Double_t real) const {
        // sum of quaternion with a real


        return TQuaternion(fVectorPart, fRealPart + real);
}

//_____________________________________

TQuaternion TQuaternion::operator-(Double_t real) const {
        // substraction of real to quaternion


        return TQuaternion(fVectorPart, fRealPart - real);
}

//_____________________________________

TQuaternion TQuaternion::operator*(Double_t real) const {
        // product of quaternion with a real 


        return TQuaternion(fRealPart*real,fVectorPart.x()*real,fVectorPart.y()*real,fVectorPart.z()*real);
}


//_____________________________________

TQuaternion TQuaternion::operator/(Double_t real) const {
        // division by a real


        if (real !=0 ) {
                return TQuaternion(fRealPart/real,fVectorPart.x()/real,fVectorPart.y()/real,fVectorPart.z()/real);
        } else {
            Error("operator/(Double_t)", "bad value (%f) ignored",real);
        }

        return (*this);
}

TQuaternion operator + (Double_t r, const TQuaternion & q) { return (q+r); }
TQuaternion operator - (Double_t r, const TQuaternion & q) { return (-q+r); }
TQuaternion operator * (Double_t r, const TQuaternion & q) { return (q*r); }
TQuaternion operator / (Double_t r, const TQuaternion & q) { return (q.Invert()*r); }

/**************** VECTOR TO QUATERNION ALGEBRA ****************************************/ 

//_____________________________________

TQuaternion TQuaternion::operator+(const TVector3 &vect) const {
        // sum of quaternion with a real


        return TQuaternion(fVectorPart + vect, fRealPart);
}

//_____________________________________

TQuaternion TQuaternion::operator-(const TVector3 &vect) const {
        // substraction of real to quaternion


        return TQuaternion(fVectorPart - vect, fRealPart);
}

//_____________________________________

 TQuaternion& TQuaternion::MultiplyLeft(const TVector3 &vect) {
        // left multitplication


        Double_t savedRealPart = fRealPart;
        fRealPart = - (fVectorPart * vect);
        fVectorPart = vect.Cross(fVectorPart);
        fVectorPart += (vect * savedRealPart);

        return (*this);
}

//_____________________________________

TQuaternion& TQuaternion::operator*=(const TVector3 &vect) {
        // right multiplication


        Double_t savedRealPart = fRealPart;
        fRealPart = -(fVectorPart * vect);
        fVectorPart = fVectorPart.Cross(vect);
        fVectorPart += (vect * savedRealPart );

        return (*this);
}

//_____________________________________

 TQuaternion TQuaternion::LeftProduct(const TVector3 &vect) const {
        // left product


        return TQuaternion(vect * fRealPart + vect.Cross(fVectorPart), -(fVectorPart * vect));
}

//_____________________________________

TQuaternion TQuaternion::operator*(const TVector3 &vect) const {
        // right product


        return TQuaternion(vect * fRealPart + fVectorPart.Cross(vect), -(fVectorPart * vect));
}

//_____________________________________

 TQuaternion& TQuaternion::DivideLeft(const TVector3 &vect) {
        // left division


        Double_t norm2 = vect.Mag2();
        MultiplyLeft(vect);
        if (norm2 > 0 ) {
                // use (1./nom2) to be numericaly compliant with LeftQuotient(const TVector3 &)

                (*this) *= -(1./norm2); // minus <- using conjugate of vect
        } else {
            Error("DivideLeft(const TVector3)", "bad norm2 (%f) ignored",norm2);
        }
        return (*this);
}

//_____________________________________

TQuaternion& TQuaternion::operator/=(const TVector3 &vect) {
        // right division


        Double_t norm2 = vect.Mag2();
        (*this) *= vect;
        if (norm2 > 0 ) {
                // use (1./real) to be numericaly compliant with operator/(const TVector3 &)

                (*this) *= - (1./norm2); // minus <- using conjugate of vect
        } else {
            Error("operator/=(const TVector3 &)", "bad norm2 (%f) ignored",norm2);
        }
        return (*this);
}

//_____________________________________

 TQuaternion TQuaternion::LeftQuotient(const TVector3 &vect) const {
        // left quotient


        Double_t norm2 = vect.Mag2();

        if (norm2>0) {
                double invNorm2 = 1./norm2;
                return TQuaternion((vect * -fRealPart - vect.Cross(fVectorPart))*invNorm2,
                                                                                                        (fVectorPart * vect ) * invNorm2 );
        } else {
            Error("LeftQuotient(const TVector3 &)", "bad norm2 (%f) ignored",norm2);
        }
        return (*this);
}

//_____________________________________

TQuaternion TQuaternion::operator/(const TVector3 &vect) const {
        //  right quotient


        Double_t norm2 = vect.Mag2();

        if (norm2>0) {
                double invNorm2 = 1./norm2;
                return TQuaternion((vect * -fRealPart - fVectorPart.Cross(vect)) * invNorm2,
                                                                                                                (fVectorPart * vect) * invNorm2 );
        } else {
            Error("operator/(const TVector3 &)", "bad norm2 (%f) ignored",norm2);
        }
        return (*this);
}

TQuaternion operator + (const TVector3 &V, const TQuaternion &Q) { return (Q+V); }
TQuaternion operator - (const TVector3 &V, const TQuaternion &Q) { return (-Q+V); }
TQuaternion operator * (const TVector3 &V, const TQuaternion &Q) { return Q.LeftProduct(V); }

TQuaternion operator / (const TVector3 &vect, const TQuaternion &quat) {

        TQuaternion res(vect);
        res /= quat;
        return res;
}

/**************** QUATERNION ALGEBRA ****************************************/ 

//_____________________________________

TQuaternion& TQuaternion::operator*=(const TQuaternion &quaternion) {
        // right multiplication


        Double_t saveRP = fRealPart;
        TVector3 cross(fVectorPart.Cross(quaternion.fVectorPart));

        fRealPart = fRealPart * quaternion.fRealPart - fVectorPart * quaternion.fVectorPart;

        fVectorPart *= quaternion.fRealPart;
        fVectorPart += quaternion.fVectorPart * saveRP;
        fVectorPart += cross;
        return (*this);
}

//_____________________________________

 TQuaternion& TQuaternion::MultiplyLeft(const TQuaternion &quaternion) {
        // left multiplication


        Double_t saveRP = fRealPart;
        TVector3 cross(quaternion.fVectorPart.Cross(fVectorPart));

        fRealPart = fRealPart * quaternion.fRealPart - fVectorPart * quaternion.fVectorPart;

        fVectorPart *= quaternion.fRealPart;
        fVectorPart += quaternion.fVectorPart * saveRP;
        fVectorPart += cross;

        return (*this);
}

//_____________________________________

 TQuaternion TQuaternion::LeftProduct(const TQuaternion &quaternion) const {
        // left product


        return TQuaternion( fVectorPart*quaternion.fRealPart + quaternion.fVectorPart*fRealPart
                                        + quaternion.fVectorPart.Cross(fVectorPart),
                                        fRealPart*quaternion.fRealPart - fVectorPart*quaternion.fVectorPart );
}

//_____________________________________

TQuaternion TQuaternion::operator*(const TQuaternion &quaternion) const {
        // right product


        return TQuaternion(        fVectorPart*quaternion.fRealPart + quaternion.fVectorPart*fRealPart
                                            + fVectorPart.Cross(quaternion.fVectorPart),
                                                fRealPart*quaternion.fRealPart - fVectorPart*quaternion.fVectorPart );
}

//_____________________________________

 TQuaternion& TQuaternion::DivideLeft(const TQuaternion &quaternion) {
        // left division


        Double_t norm2 = quaternion.Norm2();

        if (norm2 > 0 ) {
                MultiplyLeft(quaternion.Conjugate());
                (*this) *= (1./norm2);
        } else {
            Error("DivideLeft(const TQuaternion &)", "bad norm2 (%f) ignored",norm2);
        }
        return (*this);
}

//_____________________________________

TQuaternion& TQuaternion::operator/=(const TQuaternion& quaternion) {
        // right division


        Double_t norm2 = quaternion.Norm2();

        if (norm2 > 0 ) {
                (*this) *= quaternion.Conjugate();
                // use (1./norm2) top be numericaly compliant with operator/(const TQuaternion&)

                (*this) *= (1./norm2);
        } else {
            Error("operator/=(const TQuaternion&)", "bad norm2 (%f) ignored",norm2);
        }
        return (*this);
}

//_____________________________________

 TQuaternion TQuaternion::LeftQuotient(const TQuaternion& quaternion) const {
        // left quotient


        Double_t norm2 = quaternion.Norm2();

        if (norm2 > 0 ) {
                double invNorm2 = 1./norm2;
                return TQuaternion(
                        (fVectorPart*quaternion.fRealPart - quaternion.fVectorPart*fRealPart
                                                - quaternion.fVectorPart.Cross(fVectorPart)) * invNorm2,
                        (fRealPart*quaternion.fRealPart + fVectorPart*quaternion.fVectorPart)*invNorm2 );
        } else {
            Error("LeftQuotient(const TQuaternion&)", "bad norm2 (%f) ignored",norm2);
        }
        return (*this);
}

//_____________________________________

TQuaternion TQuaternion::operator/(const TQuaternion &quaternion) const {
        // right quotient


        Double_t norm2 = quaternion.Norm2();

        if (norm2 > 0 ) {
                double invNorm2 = 1./norm2;
                return TQuaternion(
                         (fVectorPart*quaternion.fRealPart - quaternion.fVectorPart*fRealPart
                                                                - fVectorPart.Cross(quaternion.fVectorPart)) * invNorm2,
                                (fRealPart*quaternion.fRealPart + fVectorPart*quaternion.fVectorPart) * invNorm2 );
        } else {
            Error("operator/(const TQuaternion &)", "bad norm2 (%f) ignored",norm2);
        }
        return (*this);
}

//_____________________________________

 TQuaternion TQuaternion::Invert() const {
        // invert


        double norm2 = Norm2();
        if (norm2 > 0 ) {
                double invNorm2 = 1./norm2;
                return TQuaternion(fVectorPart*(-invNorm2), fRealPart*invNorm2 );
        } else {
            Error("Invert()", "bad norm2 (%f) ignored",norm2);
        }
        return (*this);
}

//_____________________________________

 void TQuaternion::Rotate(TVector3 & vect) const {
        // rotate vect by current quaternion


        vect = Rotation(vect);
}

//_____________________________________

 TVector3 TQuaternion::Rotation(const TVector3 & vect) const {
        // rotation of vect by current quaternion


        // Vres = (*this) * vect * (this->Invert());

        double norm2 = Norm2();

        if (norm2>0) {
                TQuaternion quat(*this);
                quat *= vect;

                // only compute vect part : (real part has to be 0 ) : 

                // VECT [ quat * ( this->Conjugate() ) ] = quat.fRealPart * -this->fVectorPart

                //                                                                                        + this->fRealPart * quat.fVectorPart

                //                                                                                        + quat.fVectorPart X (-this->fVectorPart)

                TVector3 cross(fVectorPart.Cross(quat.fVectorPart));
                quat.fVectorPart *=  fRealPart;
                quat.fVectorPart -= fVectorPart * quat.fRealPart;
                quat.fVectorPart += cross;

                return quat.fVectorPart*(1./norm2);
        } else {
            Error("Rotation()", "bad norm2 (%f) ignored",norm2);
        }
        return vect;
}

//_____________________________________

 void TQuaternion::Print(Option_t*) const
{
  Printf("%s %s (r,x,y,z)=(%f,%f,%f,%f) \n (alpha,rho,theta,phi)=(%f,%f,%f,%f)",GetName(),GetTitle(),
                        fRealPart,fVectorPart.X(),fVectorPart.Y(),fVectorPart.Z(),
            GetQAngle()*TMath::RadToDeg(),fVectorPart.Mag(),fVectorPart.Theta()*TMath::RadToDeg(),fVectorPart.Phi()*TMath::RadToDeg());
}


ROOT page - Class index - Class Hierarchy - Top of the page

This page has been automatically generated. If you have any comments or suggestions about the page layout send a mail to ROOT support, or contact the developers with any questions or problems regarding ROOT.