TAPsQuaternion.cpp

Go to the documentation of this file.

/******************************************************************************
TAPsQuaternion.cpp

Quaternion class (cpp file).

SUKITTI PUNAK   (07/12/2009)
UPDATE          (08/20/2010)
******************************************************************************/
#include "TAPsQuaternion.hpp"
// Using Inclusion Model (i.e. definitions are included in declarations)
//                       (this name.cpp is included in name.hpp)
// Each friend is defined directly inside its declaration.

BEGIN_NAMESPACE_TAPs
//=============================================================================

// Default Constructor
template <typename T>
Quaternion<T>::Quaternion ()
    : m_r( 1 ), m_i( 0 ), m_j( 0 ), m_k( 0 )
{}

// Initialize Constructor
template <typename T>
Quaternion<T>::Quaternion ( T r, T i, T j, T k )
    : m_r( r ), m_i( i ), m_j( j ), m_k( k )
{}

// Initialize Constructor
template <typename T>
Quaternion<T>::Quaternion ( T q[4] )
    : m_r( q[0] ), m_i( q[1] ), m_j( q[2] ), m_k( q[3] )
{}

// Initialize Constructor
template <typename T>
Quaternion<T>::Quaternion ( Vector3<T> const & P )
    : m_r( 0 ), m_i( P[1] ), m_j( P[2] ), m_k( P[3] )
{}

// Copy Constructor
template <typename T>
Quaternion<T>::Quaternion ( Quaternion<T> const &Q )
    : m_r( Q.m_r ), m_i( Q.m_i ), m_j( Q.m_j ), m_k( Q.m_k )
{}

// Destructor
template <typename T>
Quaternion<T>::~Quaternion ()
{}

// String Info
template <typename T>
std::string Quaternion<T>::StrInfo () const
{
    std::ostringstream ss;
    ss << "Quaternion<" << typeid(T).name() << ">< ";
    ss << m_r << ", " << m_i << "i, " << m_j << "j, " << m_k << "k >";
    return ss.str();
}

// Conjugate Operator
template <typename T>
Quaternion<T> Quaternion<T>::operator! ()
{   
    return Quaternion( m_r, -m_i, -m_j, -m_k );
}

// Conjugated
template <typename T>
void Quaternion<T>::Conjugated ()
{
    m_i=-m_i;  m_j=-m_j;  m_k=-m_k;
}

// Get Conjugate
template <typename T>
Quaternion<T> Quaternion<T>::GetConjugate () const
{   
    return Quaternion<T>( m_r, -m_i, -m_j, -m_k );
}

// Assignment Operator
template <typename T>
Quaternion<T> & Quaternion<T>::operator= ( Quaternion<T> const &Q )
{   
    m_r=Q.m_r;  m_i=Q.m_i;  m_j=Q.m_j;  m_k=Q.m_k;
    return *this;
}

// Addition Operator
template <typename T>
Quaternion<T> Quaternion<T>::operator+ ( Quaternion<T> const &Q ) const
{   
    return Quaternion<T>( m_r+Q.m_r, m_i+Q.m_i, m_j+Q.m_j, m_k+Q.m_k );
}

// Addition and Assignment Operator
template <typename T>
Quaternion<T> & Quaternion<T>::operator+= ( Quaternion<T> const &Q )
{   
    m_r+=Q.m_r;  m_i+=Q.m_i;  m_j+=Q.m_j;  m_k+=Q.m_k;
    return *this;
}

// Subtraction Operator
template <typename T>
Quaternion<T> Quaternion<T>::operator- ( Quaternion<T> const &Q ) const
{   
    return Quaternion<T>( m_r-Q.m_r, m_i-Q.m_i, m_j-Q.m_j, m_k-Q.m_k );
}

// Subtraction and Assignment Operator
template <typename T>
Quaternion<T> & Quaternion<T>::operator-= ( Quaternion<T> const &Q )
{   
    m_r-=Q.m_r;  m_i-=Q.m_i;  m_j-=Q.m_j;  m_k-=Q.m_k;
    return *this;
}

// Multiplication Operator
template <typename T>
Quaternion<T> Quaternion<T>::operator* ( Quaternion<T> const &Q ) const
{   
    return Quaternion<T>( 
        m_r*Q.m_r - m_i*Q.m_i - m_j*Q.m_j - m_k*Q.m_k,
        m_r*Q.m_i + m_i*Q.m_r + m_j*Q.m_k - m_k*Q.m_j,
        m_r*Q.m_j + m_j*Q.m_r + m_k*Q.m_i - m_i*Q.m_k,
        m_r*Q.m_k + m_k*Q.m_r + m_i*Q.m_j - m_j*Q.m_i
    );
}

// Multiplication Operator
template <typename T>
Quaternion<T> Quaternion<T>::operator* ( Vector3<T> const &V ) const
{   
    return Quaternion<T>( 
        m_r*0     - m_i*V[0]  - m_j*V[1]  - m_k*V[2] ,
        m_r*V[0]  + m_i*0     + m_j*V[2]  - m_k*V[1] ,
        m_r*V[1]  + m_j*0     + m_k*V[0]  - m_i*V[2] ,
        m_r*V[2]  + m_k*0     + m_i*V[1]  - m_j*V[0] 
    );
}

template <typename T>
T   Quaternion<T>::InnerProduct ( Quaternion<T> const &Q ) const
{
    return m_r*Q.m_r + m_i*Q.m_i + m_j*Q.m_j + m_k*Q.m_k;
}


// Multiplication and Assignment Operator
template <typename T>
Quaternion<T> & Quaternion<T>::operator*= ( Quaternion<T> const &Q )
{   
    T r = m_r*Q.m_r - m_i*Q.m_i - m_j*Q.m_j - m_k*Q.m_k;
    T i = m_r*Q.m_i + m_i*Q.m_r + m_j*Q.m_k - m_k*Q.m_j;
    T j = m_r*Q.m_j + m_j*Q.m_r + m_k*Q.m_i - m_i*Q.m_k;
    T k = m_r*Q.m_k + m_k*Q.m_r + m_i*Q.m_j - m_j*Q.m_i;
    m_r=r;  m_i=i;  m_j=j;  m_k=k;
    return *this;
}

// Multiplication Operator
template <typename T>
Quaternion<T> Quaternion<T>::operator* ( T a ) const
{   
    return Quaternion<T>( m_r*a, m_i*a, m_j*a, m_k*a );
}

// Multiplication and Assignment Operator
template <typename T>
Quaternion<T> & Quaternion<T>::operator*= ( T a )
{   
    m_r*=a;  m_i*=a;  m_j*=a;  m_k*=a;
    return *this;
}

// Get the square of norm
template <typename T>
T Quaternion<T>::GetNormSquare () const
{
    return m_r*m_r + m_i*m_i + m_j*m_j + m_k*m_k;
}

// Get the norm
template <typename T>
T Quaternion<T>::GetNorm () const
{
    return sqrt( GetNormSquare() );
}

// Normalized
template <typename T>
Quaternion<T> & Quaternion<T>::Normalized ()
{
    T norm = GetNorm();
    m_r /= norm;  m_i /= norm;  m_j /= norm;  m_k /= norm;
    return *this;
}

// Get inverse
template <typename T>
Quaternion<T> Quaternion<T>::GetInverse () const
{
    T normSqrt = GetNormSquare();
    return Quaternion( m_r/normSqrt, -m_i/normSqrt, -m_j/normSqrt, -m_k/normSqrt );
}

// Inversed
template <typename T>
void Quaternion<T>::Inversed ()
{
    Conjugated();
    Normalized();
}

// Get the 3x3 rotation matrix from this quaternion
template <typename T>
Matrix3x3<T> Quaternion<T>::GetRotationMatrix3x3 () const
{
    T rr = m_r*m_r;
    T ii = m_i*m_i;
    T jj = m_j*m_j;
    T kk = m_k*m_k;
    T ri = m_r*m_i;
    T rj = m_r*m_j;
    T rk = m_r*m_k;
    T ij = m_i*m_j;
    T jk = m_j*m_k;
    T ik = m_i*m_k;

    // Ref: Schwab, Meijaard, "How to draw Euler angles and utilize 
    //      Euler parameters, ASME, 2006.
    // Remark: Works only for a unit quaternion, i.e. when r^2+i^2+j^2+k^2=1.
    return Matrix3x3<T>( 
            rr+ii-jj-kk,    2*(ij-rk),    2*(ik+rj),
              2*(ij+rk),  rr-ii+jj-kk,    2*(jk-ri),
              2*(ik-rj),    2*(jk+ri),  rr-ii-jj+kk
        );

    // Ref: Shoemake, "Animating rotation with quaternion curves", SIGGRAPH, 1985.
    // Remark: Works only for a unit quaternion, i.e. when r^2+i^2+j^2+k^2=1.
    //return Matrix3x3<T>( 
    //      1-2*jj-2*kk,      ij2-rk2,      ik2+rj2,
    //          ij2+rk2,  1-2*ii-2*kk,      jk2-ri2,
    //          ik2-rj2,      jk2+ri2,  1-2*ii-2*jj
    //  );
}

// Get the 4x4 rotation matrix from this quaternion
template <typename T>
Matrix4x4<T> Quaternion<T>::GetRotationMatrix4x4 () const
{
    T rr = m_r*m_r;
    T ii = m_i*m_i;
    T jj = m_j*m_j;
    T kk = m_k*m_k;
    T ri = m_r*m_i;
    T rj = m_r*m_j;
    T rk = m_r*m_k;
    T ij = m_i*m_j;
    T jk = m_j*m_k;
    T ik = m_i*m_k;

    // Ref: Schwab, Meijaard, "How to draw Euler angles and utilize 
    //      Euler parameters, ASME, 2006.
    // Remark: Works only for a unit quaternion, i.e. when r^2+i^2+j^2+k^2=1.
    return Matrix4x4<T>( 
            rr+ii-jj-kk,    2*(ij-rk),    2*(ik+rj),  0,
              2*(ij+rk),  rr-ii+jj-kk,    2*(jk-ri),  0,
              2*(ik-rj),    2*(jk+ri),  rr-ii-jj+kk,  0,
                 0,            0,            0,       1
        );
}

// Conversion from a 3x3 rotation matrix
template <typename T>
void Quaternion<T>::SetByRotationMatrix3x3 ( Matrix3x3<T> const & R3x3 )
{
    // Converse rotation matrix to Euler rotation axis and angle
    //  Ref: http://en.wikipedia.org/wiki/Rotation_representation_%28mathematics%29
    T rotationAngle = acos( (R3x3(0,0) + R3x3(1,1) + R3x3(2,2) - 1)*0.5 );
    T s2 = 2 * sin( rotationAngle );
    Vector3<T> rotationAxis(
        ( R3x3(2,1) - R3x3(1,2) ) / s2,
        ( R3x3(0,2) - R3x3(2,0) ) / s2,
        ( R3x3(1,0) - R3x3(0,1) ) / s2
    );

    // Converse Euler rotation axis and angle to quaternion
    Vector3<T> axis( rotationAxis.GetUnit() );
    T halfAngle = rotationAngle * T(0.5);
    m_r = cos( halfAngle );
    T s = sin( halfAngle );
    m_i = axis[0] * s;
    m_j = axis[1] * s;
    m_k = axis[2] * s;
}

/*
// Conversion from a 3x3 rotation matrix
template <typename T>
void Quaternion<T>::SetFromRotationMatrix3x3 ( Matrix3x3<T> const & R3x3 )
{
    //m_r = 0.5 * sqrt( 1 + R3x3(0,0) - R3x3(1,1) - R3x3(2,2) );
    //T r4 = 4*m_r;
    //m_i = ( R3x3(0,1) - R3x3(1,0) ) / r4;
    //m_j = ( R3x3(0,2) - R3x3(2,0) ) / r4;
    //m_k = ( R3x3(2,1) - R3x3(1,2) ) / r4;
    //Normalized();

    //return;

    // Does not work well for a lot of cases!

    // Ref: Shoemake, "Animating rotation with quaternion curves", SIGGRAPH, 1985.

    // Remark: 
    //   From testing conversions from Quaternion<double> to 
    //   Matrix3x3<double> and back to Quaternion<double>:
    //     Case 1 works very well, since the square of the real value is greater 
    //     than the round-off error.
    //     Case 2, 3, and 4 don't work at all!
    //       where Case 2 sets the m_r to zero.
    //       where Case 3 sets the m_r and m_i to zero.
    //       where Case 4 sets the m_r, m_i, and m_j to zero and m_k to one.

    T rr = 0.25 * ( 1 + R3x3(0,0) + R3x3(1,1) + R3x3(2,2) );
    // Case r^2 > \epsilon; where \epsilon is (the machine precision) of zero.
    if ( rr > Math<T>::ROUND_ERROR ) {
        //std::cout << "Case 1\n";

        //  Here we choose to get a quaternion with positive real part from this conversion.
        //  If we choose the real part to be negative, then the vector part of the quaternion 
        //  will be the conjugated of the quaternion with the positive real part.
        //  That means the vector, which is an axis of rotation, is reversed.
        m_r = sqrt( rr );
        T r4 = 4*m_r;
        m_i = ( R3x3(2,1) - R3x3(1,2) ) / r4;   // In the Shoemake's paper --> R3x3(1,2)-R3x3(2,1)
        m_j = ( R3x3(0,2) - R3x3(2,0) ) / r4;   // In the Shoemake's paper --> R3x3(2,0)-R3x3(0,2)
        m_k = ( R3x3(1,0) - R3x3(0,1) ) / r4;   // In the Shoemake's paper --> R3x3(0,1)-R3x3(1,0)
                                                // However, after the tests, these have to be swapped.
    }
    // Case r^2 <= \epsilon, i.e., r^2 is considered to be zero by the m/c precision.
    else {
        //std::cout << "Case 2\n";
        m_r = 0;
        T ii = -0.5 * ( R3x3(1,1) + R3x3(2,2) );
        if ( ii > Math<T>::ROUND_ERROR ) {
            m_i = sqrt( ii );
            T i2 = 2*m_i;
            m_j = R3x3(0,1) / i2;
            m_k = R3x3(0,2) / i2;
        }
        else {
            //std::cout << "Case 3\n";
            m_i = 0;
            T jj = 0.5 * ( 1 - R3x3(2,2) );
            if ( jj > Math<T>::ROUND_ERROR ) {
                m_j = sqrt( jj );
                m_k = R3x3(1,2) / (2*m_j);
            }
            else {
                //std::cout << "Case 4\n";
                m_j = 0;
                m_k = 1;
            }
        }
    }
}
//*/

// Get the quaternion matrix
template <typename T>
Matrix4x4<T> Quaternion<T>::GetQuaternionMatrix () const
{
    return Matrix4x4<T>(
        m_r, -m_i, -m_j, -m_k,
        m_i,  m_r, -m_k,  m_j,
        m_j,  m_k,  m_r, -m_i,
        m_k, -m_j,  m_i,  m_r
    );
}

// Get the quaternion conjugate matrix
template <typename T>
Matrix4x4<T> Quaternion<T>::GetQuaternionConjugateMatrix () const
{
    return Matrix4x4<T>(
        m_r, -m_i, -m_j, -m_k,
        m_i,  m_r,  m_k, -m_j,
        m_j, -m_k,  m_r,  m_i,
        m_k,  m_j, -m_i,  m_r
    );
}

template <typename T>
void Quaternion<T>::SetByRotationAxisAndAngle ( Vector3<T> const & rotAxis, T angle )
{
    Vector3<T> axis( rotAxis.GetUnit() );
    T halfAngle = angle * T(0.5) * Math<T>::DEG_TO_RAD;
    m_r = cos( halfAngle );
    T s = sin( halfAngle );
    m_i = axis[0] * s;
    m_j = axis[1] * s;
    m_k = axis[2] * s;
    //Normalized();
}

template <typename T>
void Quaternion<T>::SetByRotationAxisAndAngle ( T rotAxis_x, T rotAxis_y, T rotAxis_z, T angle )
{
    SetByRotationAxisAndAngle( Vector3<T>( rotAxis_x, rotAxis_y, rotAxis_z ), angle );
}

template <typename T>
void Quaternion<T>::GetRotationAxisAndAngle ( Vector3<T> & rotAxis, T & angle )
{
    angle = 2 * acos( m_r );
    if ( angle != 0 ) {
        T s = T(1) / sqrt(1 - m_r*m_r);
        rotAxis[0] = m_i * s;
        rotAxis[1] = m_j * s;
        rotAxis[2] = m_k * s;
    }
    angle *= Math<T>::RAD_TO_DEG;
}

template <typename T>
void Quaternion<T>::GetRotationAxisAndAngle ( T& rotAxis_x, T& rotAxis_y, T& rotAxis_z, T& angle )
{
    angle = 2 * acos( m_r );
    if ( angle != 0 ) {
        T s = T(1) / sqrt(1 - m_r*m_r);
        rotAxis_x = m_i * s;
        rotAxis_y = m_j * s;
        rotAxis_z = m_k * s;
    }
    angle *= Math<T>::RAD_TO_DEG;
}

template <typename T>
void Quaternion<T>::RotatePt ( Vector3<T> & P ) const
{
    Quaternion<T> R( (*this) * Quaternion<T>(P) * this->GetConjugate() );
    P[0] = R.m_i;
    P[1] = R.m_j;
    P[2] = R.m_k;
}

template <typename T>
void Quaternion<T>::RotatePt ( T & P_x, T & P_y, T & P_z ) const
{
    Quaternion<T> R( (*this) * Quaternion<T>(0,P_x,P_y,P_z) * this->GetConjugate() );
    P[0] = R.m_i;
    P[1] = R.m_j;
    P[2] = R.m_k;
}

template <typename T>
Vector3<T> Quaternion<T>::CalRotatedPtOfPt ( Vector3<T> const & P ) const
{
    Quaternion<T> R( (*this) * Quaternion<T>(P) * this->GetConjugate() );
    return Vector3<T>( R.m_i, R.m_j, R.m_k );
}

template <typename T>
Vector3<T> Quaternion<T>::CalRotatedPtOfPt ( T P_x, T P_y, T P_z ) const
{
    Quaternion<T> R( (*this) * Quaternion<T>(0,P_x,P_y,P_z) * this->GetConjugate() );
    return Vector3<T>( R.m_i, R.m_j, R.m_k );
}


//=============================================================================
#if defined(__gl_h_) || defined(__GL_H__)
//-----------------------------------------------------------------------------
template <typename T>
void Quaternion<T>::TransformByOpenGLForDrawing () const
{
    Matrix4x4<T> orientation = GetRotationMatrix4x4();
    GLfloat m[16] = {
        static_cast<GLfloat>(orientation[ 0]), static_cast<GLfloat>(orientation[ 4]), static_cast<GLfloat>(orientation[ 8]), static_cast<GLfloat>(orientation[ 12]), 
        static_cast<GLfloat>(orientation[ 1]), static_cast<GLfloat>(orientation[ 5]), static_cast<GLfloat>(orientation[ 9]), static_cast<GLfloat>(orientation[ 13]), 
        static_cast<GLfloat>(orientation[ 2]), static_cast<GLfloat>(orientation[ 6]), static_cast<GLfloat>(orientation[10]), static_cast<GLfloat>(orientation[ 14]), 
        static_cast<GLfloat>(orientation[ 3]), static_cast<GLfloat>(orientation[ 7]), static_cast<GLfloat>(orientation[11]), static_cast<GLfloat>(orientation[ 15])
    };
    glMultMatrixf( m );
}
//-----------------------------------------------------------------------------
#endif
//=============================================================================

//=============================================================================
END_NAMESPACE_TAPs
//34567890123456789012345678901234567890123456789012345678901234567890123456789
//--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+----