vec.h

//
// Quaternion Class
//
#ifndef CQuat_h
#define CQuat_h

#include "vec3.h"

const float TO_HALF_RAD = 3.14159265f / 360.0f;
const float MINVAL = 0.005f;

class CQuat
{
public:
    float x,y,z,w;

    CQuat( ) : x(0), y(0), z(0), w(1)
    {
    }

    CQuat( float fx, float fy, float fz, float fw ) : x(fx), y(fy), z(fz), w(fw)
    {
    }

    // This just took four floats initially to avoid dependence on the vector class
    // but I decided avoiding confusion with the value setting constructor was more important
    CQuat( float Angle, const CVec3& Axis )
    {
        SetAxis( Angle, Axis.x, Axis.y, Axis.z );
    }

    // No rotation
    void Reset( )
    {
        x = 0;
        y = 0;
        z = 0;
        w = 1;
    }

    // Set Quat from axis-angle
    void SetAxis( float degrees, float fX, float fY, float fZ )
    {
        float HalfAngle = degrees * TO_HALF_RAD; // Get half angle in radians from angle in degrees
        float sinA = (float)sin( HalfAngle ) ;
        w = (float)cos( HalfAngle );
        x = fX * sinA;
        y = fY * sinA;
        z = fZ * sinA;
    }

    CQuat Invert( ) const
    {
        return CQuat( -x, -y, -z, w );
    }

    // Note that order matters with concatenating Quaternion rotations
    inline CQuat operator* (const CQuat &b) const
    {
        CQuat r;

        r.w = w*b.w - x*b.x  -  y*b.y  -  z*b.z;
        r.x = w*b.x + x*b.w  +  y*b.z  -  z*b.y;
        r.y = w*b.y + y*b.w  +  z*b.x  -  x*b.z;
        r.z = w*b.z + z*b.w  +  x*b.y  -  y*b.x;

        return r;
    }

    // You could add an epsilon to this equality test if needed
    inline bool operator== ( const CQuat &b ) const
    {
        return (x == b.x && y == b.y && z == b.z && w == b.w);
    }

    int IsIdentity( ) const
    {
        return (x == 0.0f && y == 0.0f && z == 0.0f && w==1.0f);
    }

    // Can be used the determine Quaternion neighbourhood
    float Dot( const CQuat& a ) const
    {
        return x * a.x + y * a.y + z * a.z + w * a.w;
    }

    // Scalar multiplication
    CQuat operator*( float s ) const
    {
        return CQuat(x * s, y * s, z * s, w * s );
    }

    // Addition
    CQuat operator+ ( const CQuat& b ) const
    {
        return CQuat( x + b.x, y + b.y, z + b.z, w + b.w );
    }

    // ------------------------------------
    // Simple Euler Angle to Quaternion conversion, this could be made faster
    // ------------------------------------
    void FromEuler( float rx, float ry, float rz )
    {
        CQuat qx(-rx, CVec3( 1, 0, 0 ) );
        CQuat qy(-ry, CVec3( 0, 1, 0 ) );
        CQuat qz(-rz, CVec3( 0, 0, 1 ) );
        qz = qy * qz;
        *this = qx * qz;
    }

    // ------------------------------------
    // Quaternions store scale as well as rotation, but usually we just want rotation, so we can normalize.
    // ------------------------------------
    int Normalize( )
    {
        float lengthSq = x * x + y * y + z * z + w * w;

        if (lengthSq == 0.0 ) return -1;
        if (lengthSq != 1.0 )
            {
            float scale = ( 1.0f / sqrtf( lengthSq ) );
            x *= scale;
            y *= scale;
            z *= scale;
            w *= scale;
            return 1;
            }
        return 0;
    }

    // ------------------------------------
    // Creates a value for this Quaternion from spherical linear interpolation
    // t is the interpolation value from 0 to 1
    // if bReduceTo360 is true, the interpolation will take the shortest path for a 360 deg angle range (max delta rotation = 180 degrees)
    // if bReduceTo360 is false, the interpolation will take the shortest path for a 720 deg angle range (max delta rotation = 360 degrees)
    // ------------------------------------
    void Slerp(const CQuat& a, const CQuat& b, float t, const bool bReduceTo360 )
    {
      float w1, w2;
      int bFlip = 0;

      float cosTheta = a.Dot(b);
      if ( bReduceTo360 && cosTheta < 0.0f ) { // We need to flip a quaternion for shortest path interpolation
          cosTheta = -cosTheta;
          bFlip = 1;
      }
      float theta    = acos(cosTheta);
      float sinTheta = sin(theta);

      if( sinTheta > MINVAL )
      {
        w1 = sin( (1.0f-t)*theta ) / sinTheta;
        w2 = sin( t*theta) / sinTheta;
      } else {
        // They're almost the same quaternion
        w1 = 1.0f - t;
        w2 = t;
      }

      if ( bFlip )
          w2 = -w2;

      *this = a*w1 + b*w2;
    }

    #define EPSILON 0.00001f

    void QuaternionSlerp(const CQuat& quat0, const CQuat& quat1, float t)
    {
        float cosineom, sineom, scalar_x, scalar_y, scalar_z, scalar_w;
        float omega, scale0, scale1;

        CQuat newquat;

        if ((fabs(quat0.x - quat1.x) < EPSILON)&&(fabs(quat0.y - quat1.y) < EPSILON)&&
           (fabs(quat0.z - quat1.z) < EPSILON)&&(fabs(quat0.w - quat1.w) < EPSILON))
        {
            newquat.x = quat1.x;
            newquat.y = quat1.y;
            newquat.z = quat1.z;
            newquat.w = quat1.w;
            *this = newquat;
        }

        cosineom = quat0.x*quat1.x + quat0.y*quat1.y + quat0.z*quat1.z + quat0.w*quat1.w;

        if (cosineom<=0)
        {
            cosineom = -cosineom;
            scalar_x = -quat1.x;
            scalar_y = -quat1.y;
            scalar_z = -quat1.z;
            scalar_w = -quat1.w;
        }
        else
        {
            scalar_x = quat1.x;
            scalar_y = quat1.y;
            scalar_z = quat1.z;
            scalar_w = quat1.w;
        }

        if ((1.0f - cosineom) > EPSILON)
        {   // do this whole SLerp thing
            omega = acosf(cosineom);
            sineom = sinf(omega);
            scale0 = sinf((1.0f - t)*omega)/sineom;
            scale1 = sinf(t*omega)/sineom;
        }
        else
        {   // close enough to do a straight up linear interpolation
            scale0 = 1.0f - t;
            scale1 = t;
        }

        newquat.x=scale0*quat0.x + scale1*scalar_x;
        newquat.y=scale0*quat0.y + scale1*scalar_y;
        newquat.z=scale0*quat0.z + scale1*scalar_z;
        newquat.w=scale0*quat0.w + scale1*scalar_w;

        *this = newquat;
    }
    // ------------------------------------
    // linearly interpolate each component, then normalize the Quaternion
    // Unlike spherical interpolation, this does not rotate at a constant velocity,
    // although that's not necessarily a bad thing
    // ------------------------------------
    void Nlerp(const CQuat &a, const CQuat &b, float t, const bool bReduceTo360 )
    {
        float t1 = 1.0f - t;

        if ( bReduceTo360 && a.Dot(b) < 0.0f )
            *this = a * t1 + b * -t;
        else
            *this = a * t1 + b * t;

        Normalize();
    }

    // ------------------------------------
    // Set a 4x4 matrix with the rotation of this Quaternion
    // ------------------------------------
    void inline ToMatrix( float mf[16] ) const
    {
        float x2 = 2.0f * x,  y2 = 2.0f * y,  z2 = 2.0f * z;

        float xy = x2 * y,  xz = x2 * z;
        float yy = y2 * y,  yw = y2 * w;
        float zw = z2 * w,  zz = z2 * z;

        mf[ 0] = 1.0f - ( yy + zz );
        mf[ 1] = ( xy - zw );
        mf[ 2] = ( xz + yw );
        mf[ 3] = 0.0f;

        float xx = x2 * x,  xw = x2 * w,  yz = y2 * z;

        mf[ 4] = ( xy +  zw );
        mf[ 5] = 1.0f - ( xx + zz );
        mf[ 6] = ( yz - xw );
        mf[ 7] = 0.0f;

        mf[ 8] = ( xz - yw );
        mf[ 9] = ( yz + xw );
        mf[10] = 1.0f - ( xx + yy );
        mf[11] = 0.0f;

        mf[12] = 0.0f;
        mf[13] = 0.0f;
        mf[14] = 0.0f;
        mf[15] = 1.0f;
    }

    // ------------------------------------
    // Set this Quat to aim the Z-Axis along the vector from P1 to P2
    // ------------------------------------
    void AimZAxis( const CVec3& P1, const CVec3& P2 )
    {
        CVec3 vAim = P2 - P1;
        vAim.Normalize();

        x = vAim.y;
        y = -vAim.x;
        z = 0.0f;
        w = 1.0f + vAim.z;

        if ( x == 0.0f && y == 0.0f && z == 0.0f && w == 0.0f ) {
            *this = CQuat( 0, 1, 0, 0 ); // If we can't normalize it, just set it
        } else {
            Normalize();
        }
    }
};

#endif