Quaternion.h

#ifndef Quat_H
#define Quat_H

//#include "vector.h"
#include <math.h>


//-----------------------------------------------------------------------------
// Quat
//-----------------------------------------------------------------------------

class RadEuler;
class QAngle;
class Vector;
struct matrix3x4_t;

class Quat              // same data-layout as engine's vec4_t,
{                               //      which is a vec_t[4]
public:
    inline Quat(void)   {

    // Initialize to NAN to catch errors
#ifdef _DEBUG
#ifdef VECTOR_PARANOIA
        x = y = z = w = VEC_T_NAN;
#endif
#endif
    }
    inline Quat(vec_t ix, vec_t iy, vec_t iz, vec_t iw) : x(ix), y(iy), z(iz), w(iw) { }
    inline Quat(RadEuler const &angle); // evil auto type promotion!!!

    inline void Init(vec_t ix=0.0f, vec_t iy=0.0f, vec_t iz=0.0f, vec_t iw=0.0f)    { x = ix; y = iy; z = iz; w = iw; }

    bool IsValid() const;

    bool operator==( const Quat &src ) const;
    bool operator!=( const Quat &src ) const;

    Quat    operator*(const Quat& q) const;
    Quat  operator/(const Quat& q) const;
    Quat  operator*(const float c) const;

    Vector  operator*(const Vector& vec) const;

    FORCEINLINE Quat&   operator+=(const Quat &b);
    FORCEINLINE Quat&   operator+(const Quat &b);

    FORCEINLINE Quat&   operator-=(const Quat &b);

    FORCEINLINE Quat&   operator*=(const float &f);
    FORCEINLINE Quat&   operator*(const float &f);
    //Quat operator*(const float& f) const;
    //Quat operator-(const Quat& a, const Quat& b);
    //friend inline Quat operator*(const Quat& a, const Quat& b);
    //Quat operator+(const Quat& b) const;

    // array access...
    vec_t operator[](int i) const;
    vec_t& operator[](int i);

    vec_t x, y, z, w;

    //MKS - Quat extras
    float  NormalizeInPlace();
    void GetAngles( QAngle &angles );
    //void GetAngles( RadEuler &angles );
    //QuatAngles( const Quat &q, RadEuler &angles );
    matrix3x4_t GetMatrix();

    inline Quat Conj() { return Quat( -x, -y, -z, w ); };
    void Slerp( Quat &from, Quat &to, float t );
    void SlerpNoAlign( const Quat &p, const Quat &q, float t );
    void Align( const Quat &p, const Quat &q );
    void AxisAngle( const Vector &axis, float angle );
    void ToAxisAngle( Vector &axis, float &angle );

    void TransformVector( Vector &vec );
    void FromRotationMatrix (const matrix3x4_t& kRot);
    void ToRotationMatrix (matrix3x4_t& kRot) const;

    //Gets a rotation by using 2 UP directions.
    void GetRotationTo(const Vector& start, const Vector& dest);

    //Gets a rotation by using cross product and given angle.
    void GetRotationTo(const Vector& cross, float angle);

    void ToAxes(Vector* akAxis) const;


    Quat i( void ) const;
    Quat expon( void ) const;
    Quat power(const float t) const;
    Quat Logr( void )const;
    float dot_prod(const Quat & q) const;

    void CreateFromYawPitchRoll(Vector up, float yaw, Vector right, float pitch, Vector forward, float roll);
};

inline bool Quat::IsValid() const
{
    return IsFinite(x) && IsFinite(y) && IsFinite(z) && IsFinite(w);
}

inline Quat& Quat::operator+= (const Quat& b)
//FORCEINLINE Quat operator+ (const Quat& b) const
{
    x += b.x;
    y += b.y;
    z += b.z;
    w += b.w;
    return *this;
}

inline  Quat& Quat::operator+ (const Quat& b)
//FORCEINLINE Quat operator+ (const Quat& b) const
{
    x += b.x;
    y += b.y;
    z += b.z;
    w += b.w;
    return *this;
}
/*
inline Quat operator* (const Quat& a, const Quat& b)
{
    return Quat( a.w*b.x + a.x*b.w + a.y*b.z - a.z*b.y,
                    a.w*b.y - a.x*b.z + a.y*b.w + a.z*b.x,
                    a.w*b.z + a.x*b.y - a.y*b.x + a.z*b.w,
                    a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z );
}
*/
inline  Quat& Quat::operator*= (const float& f)
//Quat Quat::operator* (const float& f) const
{
    x*=f;
    y*=f;
    z*=f;
    w*=f;
    return *this;
}

inline  Quat& Quat::operator* (const float& f)
//Quat Quat::operator* (const float& f) const
{
    x*=f;
    y*=f;
    z*=f;
    w*=f;
    return *this;
}
/*
FORCEINLINE_VECTOR  Quat& Quat::operator- (const Quat& b)
//Quat Quat::operator- (const Quat& b) const
{
    x -= b.x;
    y -= b.y;
    z -= b.z;
    w -= b.w;
    return *this;
}
*/
//-----------------------------------------------------------------------------
// Radian Euler QAngle aligned to axis (NOT ROLL/PITCH/YAW)
//-----------------------------------------------------------------------------
class RadEuler
{
public:
    inline RadEuler(void)                           { }
    inline RadEuler(vec_t X, vec_t Y, vec_t Z)      { x = X; y = Y; z = Z; }
    //inline RadEuler(Quat const &q);   // evil auto type promotion!!!

    // Initialization
    inline void Init(vec_t ix=0.0f, vec_t iy=0.0f, vec_t iz=0.0f)   { x = ix; y = iy; z = iz; }

    bool IsValid() const;

    // array access...
    vec_t operator[](int i) const;
    vec_t& operator[](int i);

    vec_t x, y, z;
};

inline bool RadEuler::IsValid() const
{
    return IsFinite(x) && IsFinite(y) && IsFinite(z);
}

/*
inline RadEuler::RadEuler(Quat const &q)
{
    RadEuler ang;
    q.GetAngles( ang );
    x = ang.x;
    y = ang.y;
    z = ang.z;
    //QuatAngles( q, *this );
}
*/
inline void VectorCopy( RadEuler const& src, RadEuler &dst )
{
    //CHECK_VALID(src);
    dst.x = src.x;
    dst.y = src.y;
    dst.z = src.z;
}

inline void VectorScale( RadEuler const& src, float b, RadEuler &dst )
{
    //CHECK_VALID(src);
//  Assert( IsFinite(b) );
    dst.x = src.x * b;
    dst.y = src.y * b;
    dst.z = src.z * b;
}

Quat Slerp2(const Quat & q0, const Quat & q1, const float t);
Quat Slerp_Prime(const Quat & q0, const Quat & q1, const float t);

#endif