Quaternion.h

Go to the documentation of this file.

/*  Copyright (C) 2003-2021 Free Electron Organization
    Any use of this software requires a license.  If a valid license
    was not distributed with this file, visit freeelectron.org. */

/** @file */

#ifndef __math_Quaternion_h__
#define __math_Quaternion_h__

namespace fe
{

#define FE_SLERP_DELTA  (1e-6f)
#define FE_QUAT_DELTA   (1e-6f)
#define FE_ALMOST1      (1.0f-FE_SLERP_DELTA)

/**************************************************************************//**
    @brief Four-dimensional complex number

    @ingroup math

    Laid out as <x,y,z,w> using right-handed logic.

    Unit quaternions are commonly used to represent rotations in 3D space.
    So, many of the operation presume that the quaternion is normalized
    and may behave poorly otherwise.

    Reference: Some of the quaternion math draws from Nick Bobic's
        GLquat source code from his Feb 98 Game Developer article.
*//***************************************************************************/
template<class T>
class Quaternion: protected Vector<4,T>
{
    public:
                        Quaternion(void);
                        Quaternion(const Quaternion<T>& other);
                        Quaternion(T radians,const Vector<3,T>& axis);
                        Quaternion(T radians,Axis axis);
                        Quaternion(const T* pArray);
                        Quaternion(T x,T y,T z,T w);
                        Quaternion(const Matrix<3,4,T>& matrix);

        Quaternion<T>&  operator=(const Quaternion<T>& other);
        Quaternion<T>&  operator=(const Matrix<3,4,T>& matrix);

        void            computeAngleAxis(T &radians,Vector<3,T>& axis) const;

using                   Vector<4,T>::operator[];
using                   Vector<4,T>::operator=;
};

template<class T>
inline Quaternion<T>::Quaternion(void)
{
    set(*this,0.0f,0.0f,0.0f,1.0f);
}

template<class T>
inline Quaternion<T>::Quaternion(const Quaternion<T>& other):
    Vector<4,T>()
{
    operator=(other);
}

template<class T>
inline Quaternion<T>::Quaternion(T radians,const Vector<3,T>& axis)
{
    set(*this,radians,axis);
}

template<class T>
inline Quaternion<T>::Quaternion(T radians,const Axis axis)
{
    set(*this,radians,axis);
}

template<class T>
inline Quaternion<T>::Quaternion(const T* pArray)
{
    operator=(pArray);
}

template<class T>
inline Quaternion<T>::Quaternion(T x, T y, T z, T w)
{
    set(*this,x,y,z,w);
}

template<class T>
inline Quaternion<T>& Quaternion<T>::operator=(const Quaternion<T>& other)
{
    set(*this,other[0],other[1],other[2],other[3]);
    return *this;
}

template<class T>
inline void Quaternion<T>::computeAngleAxis(T &radians,Vector<3,T>& axis) const
{
    T len=(*this)[0]*(*this)[0]+(*this)[1]*(*this)[1]+(*this)[2]*(*this)[2];
    if(len==0.0f)
    {
        set(axis,0.0f,0.0f,1.0f);
        radians=0.0f;
        return;
    }

    T inv=1.0f/sqrt(len);
    if((*this)[3]<0.0f)
        inv= -inv;

    set(axis,(*this)[0]*inv,(*this)[1]*inv,(*this)[2]*inv);

    radians=2.0f*acos(fabs((*this)[3]));
    if(FE_INVALID_SCALAR(radians))
    {
        radians=0.0f;
    }
}

/** @brief Set the quaternion to identity

    @relates Quaternion

    The default for the components are the elements of the identity quaternion.
    The state is not automatically normalized.
    */
template<class T>
inline Quaternion<T>& set(Quaternion<T>& result)
{
    Vector<4,T>* pVector=reinterpret_cast< Vector<4,T>* >(&result);
    set(*pVector,T(0),T(0),T(0),T(1));
    return result;
}

/** @brief Set the components explicitly

    @relates Quaternion

    The state is not automatically normalized.
    */
template<class T,class X,class Y,class Z,class W>
inline Quaternion<T>& set(Quaternion<T>& result,X x,Y y,Z z,W w)
{
    Vector<4,T>* pVector=reinterpret_cast< Vector<4,T>* >(&result);
    set(*pVector,x,y,z,w);
    return result;
}

/** @brief Normalize all four components

    @relates Quaternion
    */
template<class T>
inline Quaternion<T>& normalize(Quaternion<T>& result)
{
    Vector<4,T>* pVector=reinterpret_cast< Vector<4,T>* >(&result);
    normalize(*pVector);
    return result;
}

/** @brief Set the value at the index

    @relates Quaternion
    */
template<class T,class U>
inline Quaternion<T>& setAt(Quaternion<T> &lhs, U32 index, U value)
{
    Vector<4,T>* pVector=reinterpret_cast< Vector<4,T>* >(&lhs);
    setAt(*pVector,index,value);
    return lhs;
}

/** @brief Set using arbitrary angle/axis format

    @relates Quaternion

    Axis should already be normalized.
    */
template<class T,class U>
inline Quaternion<T>& set(Quaternion<T> &result,U radians,
        const Vector<3,T>& axis)
{
    if(fabs(axis[0])+fabs(axis[1])+fabs(axis[2])<FE_QUAT_DELTA)
    {
        set(result);
        return result;
    }

    T halfAngle=radians*0.5f;
    const Vector<3,T> temp=axis*sin(halfAngle);

    set(result,temp[0],temp[1],temp[2],cos(halfAngle));
    return result;
}

/** @brief Set using angle about a specific axis

    @relates Quaternion
    */
template<class T,class U>
inline Quaternion<T>& set(Quaternion<T> &result,U radians,const Axis axis)
{
    T halfAngle=radians*0.5f;

    set(result,0.0f,0.0f,0.0f,cos(halfAngle));
    setAt(result,axis,sin(halfAngle));
    return result;
}

/** @brief Set as the angle between two vectors

    @relates Quaternion

    Operands should be pre-normalized.

    Twist should be considered arbitrary, but it is fairly consistant.
    */
template<class T>
inline Quaternion<T>& set(Quaternion<T> &result,const Vector<3,T>& from,
        const Vector<3,T>& to)
{
    //* NOTE requires normalized operands
    FEASSERT(fabs(magnitudeSquared(from)-1.0)<1e-3);
    FEASSERT(fabs(magnitudeSquared(to)-1.0)<1e-3);

    T tx,ty,tz,temp,dist;

    // get dot product of two vectors
//  cost=from[0]*to[0] + from[1]*to[1] + from[2]*to[2];
    T cost=dot(from,to);

    // check if parallel
    if (cost > FE_ALMOST1)
    {
        set(result);
        return result;
    }
    if (cost < -FE_ALMOST1)
    {
        // check if opposite

        // check if we can use cross product of from vector with [1, 0, 0]
        tx=0.0f;
        ty=from[0];
        tz= -from[1];

        temp=ty*ty + tz*tz;
        FEASSERT(temp>=0.0);
        T len=sqrt(ty*ty + tz*tz);

        if (len < FE_SLERP_DELTA)
        {
            // no, we need cross product of from vector with [0, 1, 0]
            tx= -from[2];
            ty=0.0f;
            tz=from[0];
        }

        // normalize
        temp=tx*tx + ty*ty + tz*tz;
        FEASSERT(temp>0.0);
        dist=1.0f/sqrt(temp);

        tx*=dist;
        ty*=dist;
        tz*=dist;

        set(result,tx,ty,tz,0.0f);
        return result;
    }

    // else just cross two vectors
    tx=from[1]*to[2] - from[2]*to[1];
    ty=from[2]*to[0] - from[0]*to[2];
    tz=from[0]*to[1] - from[1]*to[0];

    temp=tx*tx + ty*ty + tz*tz;
    FEASSERT(temp>0.0);
    dist=1.0f/sqrt(temp);

    tx*=dist;
    ty*=dist;
    tz*=dist;

    // use half-angle formula (sin^2 t=( 1 - cos(2t) )/2)
    temp=0.5f * (1.0f - cost);
    FEASSERT(temp>=0.0);
    T ss=sqrt(temp);

    tx*=ss;
    ty*=ss;
    tz*=ss;

    // cos^2 t=( 1 + cos (2t) ) / 2
    // w part is cosine of half the rotation angle
    set(result,tx,ty,tz,sqrt(0.5f * (1.0f + cost)));
    return result;
}


//* UNARY OPERATORS
/** @brief Multiply in place

    @relates Quaternion
    */
template<class T>
inline Quaternion<T>& operator*=(Quaternion<T>& lhs, const Quaternion<T>& rhs)
{
    Quaternion<T> temp=lhs; // deep copy

    return lhs=temp*rhs;
}

/** @brief Scale in place

    @relates Quaternion
    */
template<class T,class U>
inline Quaternion<T>& operator*=(Quaternion<T>& lhs, U scale)
{
    Quaternion<T> temp=lhs; // deep copy

    return lhs=temp*scale;
}

/** @brief Negate all elements if W is negative

    @relates Quaternion

    Every quaternion has an equivalent quaternion with all the elements
    reversed.  This forces the quaternion to the positive-W form.
    */
template<class T>
inline Quaternion<T>& forcePositiveW(Quaternion<T>& lhs)
{
    if(lhs[3]<0)
    {
        set(lhs,-lhs[0],-lhs[1],-lhs[2],-lhs[3]);
    }
    return lhs;
}

/** @brief Reverse the direction of rotation (and retain the magnitude)

    @relates Quaternion
    */
template<class T>
inline Quaternion<T>& invert(Quaternion<T>& lhs)
{
    const Quaternion<T>& clhs=lhs;
    set(lhs,-clhs[0],-clhs[1],-clhs[2],clhs[3]);
    return lhs;
}

/** @brief Return the inverse rotation

    @relates Quaternion
    */
template<class T>
inline Quaternion<T> operator-(const Quaternion<T>& lhs)
{
    Quaternion<T> inverse;
    set(inverse,-lhs[0],-lhs[1],-lhs[2],lhs[3]);
    return inverse;
}

/** @brief Print to a string

    @relates Quaternion
    */
template<class T>
inline String print(const Quaternion<T>& lhs)
{
    String s;
    s.sPrintf("%g %g %g %g",lhs[0],lhs[1],lhs[2],lhs[3]);
    return s;
}

//* BINARY OPERATORS
/** @brief Equality test

    @relates Quaternion
    */
template<class T>
inline bool operator==(const Quaternion<T>& lhs, const Quaternion<T>& rhs)
{
    for(U32 i=0; i<4; i++)
        if(rhs[i] != lhs[i])
            return false;
    return true;
}

/** @brief Equivalence test within the given tolerance @em margin

    @relates Quaternion
    */
template<class T>
inline bool equivalent(const Quaternion<T> &lhs,const Quaternion<T> &rhs,
        T margin)
{
    return( !FE_INVALID_SCALAR(lhs[0]) &&
            !FE_INVALID_SCALAR(lhs[1]) &&
            !FE_INVALID_SCALAR(lhs[2]) &&
            !FE_INVALID_SCALAR(lhs[3]) &&
            !FE_INVALID_SCALAR(rhs[0]) &&
            !FE_INVALID_SCALAR(rhs[1]) &&
            !FE_INVALID_SCALAR(rhs[2]) &&
            !FE_INVALID_SCALAR(rhs[3]) &&
            fabs(rhs[0]-lhs[0]) < margin &&
            fabs(rhs[1]-lhs[1]) < margin &&
            fabs(rhs[2]-lhs[2]) < margin &&
            fabs(rhs[3]-lhs[3]) < margin);
}

/** @brief Multiply two quaternions

    @relates Quaternion
    */
template<class T>
inline Quaternion<T> operator*(const Quaternion<T>& lhs,
        const Quaternion<T>& rhs)
{
    Quaternion<T> sum;
    set(sum,    lhs[3]*rhs[0]+lhs[0]*rhs[3]+lhs[1]*rhs[2]-lhs[2]*rhs[1],
                lhs[3]*rhs[1]+lhs[1]*rhs[3]+lhs[2]*rhs[0]-lhs[0]*rhs[2],
                lhs[3]*rhs[2]+lhs[2]*rhs[3]+lhs[0]*rhs[1]-lhs[1]*rhs[0],
                lhs[3]*rhs[3]-lhs[0]*rhs[0]-lhs[1]*rhs[1]-lhs[2]*rhs[2]);
    return sum;
}

/** @brief Scale the rotation angle

    @relates Quaternion
    */
template<class T,class U>
inline Quaternion<T> operator*(const U& lhs, const Quaternion<T>& rhs)
{
    return operator*(rhs,lhs);
}

/** @brief Scale the rotation angle

    @relates Quaternion
    */
template<class T,class U>
inline Quaternion<T> operator*(const Quaternion<T>& lhs, const U& rhs)
{
    Quaternion<T> sum;

    T angle;
    Vector<3,T> axis;

    lhs.computeAngleAxis(angle,axis);
    set(sum,angle*rhs,axis);
    return sum;
}

template <typename T>
inline void rotateVector(const Quaternion<T>& lhs,const Vector<3,T>& in,
        Vector<3,T>& out)
{
    T mid[4];

    mid[0]=  lhs[3]*in[0]+lhs[1]*in[2]-lhs[2]*in[1];
    mid[1]=  lhs[3]*in[1]+lhs[2]*in[0]-lhs[0]*in[2];
    mid[2]=  lhs[3]*in[2]+lhs[0]*in[1]-lhs[1]*in[0];
    mid[3]= -lhs[0]*in[0]-lhs[1]*in[1]-lhs[2]*in[2];

    fe::set(out,
            -mid[3]*lhs[0]+mid[0]*lhs[3]-mid[1]*lhs[2]+mid[2]*lhs[1],
            -mid[3]*lhs[1]+mid[1]*lhs[3]-mid[2]*lhs[0]+mid[0]*lhs[2],
            -mid[3]*lhs[2]+mid[2]*lhs[3]-mid[0]*lhs[1]+mid[1]*lhs[0]);
}

template <typename T>
inline void rotateXVector(const Quaternion<T>& lhs,Real x,
        Vector<3,T>& out)
{
    fe::set(out,
            x*(lhs[0]*lhs[0]+lhs[3]*lhs[3]-lhs[2]*lhs[2]-lhs[1]*lhs[1]),
            2.0*x*(lhs[0]*lhs[1]+lhs[2]*lhs[3]),
            2.0*x*(lhs[0]*lhs[2]-lhs[1]*lhs[3]));
}

template <typename T>
inline void rotateYVector(const Quaternion<T>& lhs,Real y,
        Vector<3,T>& out)
{
    fe::set(out,
            2.0*y*(lhs[0]*lhs[1]-lhs[2]*lhs[3]),
            y*(lhs[1]*lhs[1]+lhs[3]*lhs[3]-lhs[0]*lhs[0]-lhs[2]*lhs[2]),
            2.0*y*(lhs[0]*lhs[3]+lhs[1]*lhs[2]));
}

template <typename T>
inline void rotateZVector(const Quaternion<T>& lhs,Real z,
        Vector<3,T>& out)
{
    fe::set(out,
            2.0*z*(lhs[0]*lhs[2]+lhs[3]*lhs[1]),
            2.0*z*(lhs[2]*lhs[1]-lhs[0]*lhs[3]),
            z*(lhs[2]*lhs[2]+lhs[3]*lhs[3]-lhs[1]*lhs[1]-lhs[0]*lhs[0]));
}

/** @brief Generate a rotation that in between two other rotations

    Uses basic linear interpolation with renormalization.
    Commutative and torque-minimal.

    @relates Quaternion
    */
template<class T>
inline Quaternion<T>& nlerp(Quaternion<T> &result,
        const Quaternion<T> &from,const Quaternion<T> &to,T fraction)
{
    result=from+fraction*(to-from);
    normalize(result);
    return result;
}

/** @brief Generate a rotation that in between two other rotations

    Uses spherically linear interpolation.
    Constant velocity and torque-minimal.

    (See Grassia log-quaternion lerp for commutative and constant velocity.)

    @relates Quaternion
    */
template<class T>
inline Quaternion<T>& slerp(Quaternion<T> &result,
        const Quaternion<T> &from,const Quaternion<T> &to,T fraction)
{
    T copy[4];
    T cosom;
    T scale0, scale1;

    // calc cosine
    cosom = from[0]*to[0]+
            from[1]*to[1]+
            from[2]*to[2]+
            from[3]*to[3];

    // adjust signs (if necessary)
    if(cosom < 0.0f)
    {
        cosom= -cosom;

        copy[0]= -to[0];
        copy[1]= -to[1];
        copy[2]= -to[2];
        copy[3]= -to[3];
    }
    else
    {
        copy[0]=to[0];
        copy[1]=to[1];
        copy[2]=to[2];
        copy[3]=to[3];
    }

    // calculate coefficients

    if ( (1.0f - cosom) > FE_SLERP_DELTA )
    {
        // standard case (slerp)
        T omega = acos(cosom);
        T sinom = sin(omega);
        scale0 = sin((1.0f-fraction) * omega) / sinom;
        scale1 = sin(fraction * omega) / sinom;
    }
    else
    {
        // "from" and "to" quaternions are very close
        //  ... so we can do a linear interpolation

        scale0 = 1.0f-fraction;
        scale1 = fraction;
    }

    // calculate final values
    set(result, scale0*from[0] + scale1*copy[0],
                scale0*from[1] + scale1*copy[1],
                scale0*from[2] + scale1*copy[2],
                scale0*from[3] + scale1*copy[3]);
    return result;
}

template<class T>
Real removeTwistAboutX(Quaternion<T>& rRotation)
{
    Vector<3,T> yAxis(0.0f,1.0f,0.0f);

    forcePositiveW(rRotation);

    Vector<3,T> rotated;
    rotateVector(rRotation,yAxis,rotated);

    FEASSERT(rotated[0]!=0.0f);
    Real angle=atan2(rotated[2],rotated[1]);

    Quaternion<T> qinv;
    set(qinv,-angle,e_xAxis);

    Quaternion<T> result=qinv*rRotation;

    rRotation=result;
    return angle;
}

template<class T>
Real removeTwistAboutY(Quaternion<T>& rRotation)
{
    Vector<3,T> zAxis(0.0f,0.0f,1.0f);

    forcePositiveW(rRotation);

    Vector<3,T> rotated;
    rotateVector(rRotation,zAxis,rotated);

    FEASSERT(rotated[0]!=0.0f);
    Real angle=atan2(rotated[0],rotated[2]);

    Quaternion<T> qinv;
    set(qinv,-angle,e_yAxis);

    Quaternion<T> result=qinv*rRotation;

    rRotation=result;
    return angle;
}

template<class T>
Real removeTwistAboutZ(Quaternion<T>& rRotation)
{
    Vector<3,T> xAxis(1.0f,0.0f,0.0f);

    forcePositiveW(rRotation);

    Vector<3,T> rotated;
    rotateVector(rRotation,xAxis,rotated);

    FEASSERT(rotated[0]!=0.0f);
    Real angle=atan2(rotated[1],rotated[0]);

    Quaternion<T> qinv;
    set(qinv,-angle,e_zAxis);

    Quaternion<T> result=qinv*rRotation;

    rRotation=result;
    return angle;
}

typedef Quaternion<F32>     Quaternionf;
typedef Quaternion<F64>     Quaterniond;
typedef Quaternion<Real>    SpatialQuaternion;

/** @brief Return a new velocity based on desired change and constraints

    Over the given delta time, tries to return @em desired change,
    but contrains the result using the @em previous velocity,
    maximum velocity and maximum acceleration.

    The @em desired change is how far the velocity would take the angle
    in this frame if acceleration was infinite.
    The result may try to predict deceleration if the remaining change is
    relatively small with respect to the @em maxAcceleration.

    Note that the use of quaternions limits angular values to 180 degrees,
    including velocity and acceleration (per second and second^2).
*/
FE_DL_PUBLIC SpatialQuaternion stepVelocity(const SpatialQuaternion& previous,
        const SpatialQuaternion& desired,
        Real deltaT,Real maxVelocity, Real maxAcceleration);

} /* namespace */

#endif /* __math_Quaternion_h__ */