ParaQuaternion.h

#pragma once
#include "ParaMath.h"

namespace ParaEngine {

    class Quaternion
    {
    public:
        // special values
        static const Quaternion ZERO;
        static const Quaternion IDENTITY;

        float x, y, z, w;
    public:
        inline Quaternion(float fX = 0.0, float fY = 0.0, float fZ = 0.0, float fW = 1.0)
        {
            x = fX;
            y = fY;
            z = fZ;
            w = fW;
        }
        inline Quaternion(const Matrix3& rot)
        {
            this->FromRotationMatrix(rot);
        }

        inline Quaternion(const Matrix4& rot)
        {
            this->FromRotationMatrix(rot);
        }

        Quaternion(const Vector3& a, const Vector3& b);

        inline Quaternion(const Radian& rfAngle, const Vector3& rkAxis)
        {
            this->FromAngleAxis(rfAngle, rkAxis);
        }

        inline Quaternion(const Vector3& rkAxis, float fAngle)
        {
            this->FromAngleAxis(Radian(fAngle), rkAxis);
        }


        inline Quaternion(const Vector3& xaxis, const Vector3& yaxis, const Vector3& zaxis)
        {
            this->FromAxes(xaxis, yaxis, zaxis);
        }
        inline Quaternion(const Vector3* akAxis)
        {
            this->FromAxes(akAxis);
        }
        inline Quaternion(float* valptr)
        {
            memcpy(&x, valptr, sizeof(float)*4);
        }

        inline float operator [] ( const size_t i ) const
        {
            assert( i < 4 );

            return *(&x+i);
        }

        inline float& operator [] ( const size_t i )
        {
            assert( i < 4 );

            return *(&x+i);
        }

        inline float* ptr()
        {
            return &x;
        }

        inline const float* ptr() const
        {
            return &x;
        }

        void FromRotationMatrix (const Matrix3& kRot);
        void FromRotationMatrix(const Matrix4& kRot);

        void ToRotationMatrix (Matrix3& kRot) const;
        void ToRotationMatrix(Matrix4& kRot, const Vector3& Origin) const;
        void FromAngleAxis (const Radian& rfAngle, const Vector3& rkAxis);
        void ToAngleAxis (Radian& rfAngle, Vector3& rkAxis) const;
        inline void ToAngleAxis (Degree& dAngle, Vector3& rkAxis) const {
            Radian rAngle;
            ToAngleAxis ( rAngle, rkAxis );
            dAngle = rAngle;
        }
        void FromAxes (const Vector3* akAxis);
        void FromAxes (const Vector3& xAxis, const Vector3& yAxis, const Vector3& zAxis);
        void ToAxes (Vector3* akAxis) const;
        void ToAxes (Vector3& xAxis, Vector3& yAxis, Vector3& zAxis) const;
        Vector3 xAxis(void) const;
        Vector3 yAxis(void) const;
        Vector3 zAxis(void) const;

        inline Quaternion& operator= (const Quaternion& rkQ)
        {
            x = rkQ.x;
            y = rkQ.y;
            z = rkQ.z;
            w = rkQ.w;
            return *this;
        }
        Quaternion operator+ (const Quaternion& rkQ) const;
        Quaternion operator- (const Quaternion& rkQ) const;
        Quaternion operator* (const Quaternion& rkQ) const;
        Quaternion operator* (float fScalar) const;
        friend Quaternion operator* (float fScalar,
            const Quaternion& rkQ);
        Quaternion operator- () const;
        inline bool operator== (const Quaternion& rhs) const
        {
            return (rhs.x == x) && (rhs.y == y) && (rhs.z == z) && (rhs.w == w);
        }
        inline bool operator!= (const Quaternion& rhs) const
        {
            return !operator==(rhs);
        }
        // functions of a quaternion
        float Dot (const Quaternion& rkQ) const;  // dot product
        float Norm () const;  // squared-length
        float normalise(void); 
        Quaternion Inverse () const;  // apply to non-zero quaternion
        Quaternion UnitInverse () const;  // apply to unit-length quaternion
        Quaternion Exp () const;
        Quaternion Log () const;

        inline Quaternion& invertWinding() {
            x = -x; y = -y; z = -z;
            return *this;
        }

        // rotation of a vector by a quaternion
        Vector3 operator* (const Vector3& rkVector) const;

        Radian getRoll(bool reprojectAxis = true) const;
        Radian getPitch(bool reprojectAxis = true) const;
        Radian getYaw(bool reprojectAxis = true) const; 

        void    ToRadians(float *outX, float *outY, float *outZ) const;

        bool equals(const Quaternion& rhs, const Radian& tolerance) const;
        
        // spherical linear interpolation
        static Quaternion Slerp (float fT, const Quaternion& rkP, const Quaternion& rkQ, bool shortestPath = false);

        static Quaternion SlerpExtraSpins (float fT, const Quaternion& rkP, const Quaternion& rkQ,
            int iExtraSpins);

        // setup for spherical quadratic interpolation
        static void Intermediate (const Quaternion& rkQ0, const Quaternion& rkQ1, const Quaternion& rkQ2, Quaternion& rka, Quaternion& rkB);

        // spherical quadratic interpolation
        static Quaternion Squad (float fT, const Quaternion& rkP, const Quaternion& rkA, const Quaternion& rkB, const Quaternion& rkQ, bool shortestPath = false);

        // normalised linear interpolation - faster but less accurate (non-constant rotation velocity)
        static Quaternion nlerp(float fT, const Quaternion& rkP, const Quaternion& rkQ, bool shortestPath = false);

        // cutoff for sine near zero
        static const float ms_fEpsilon;

        inline friend std::ostream& operator <<
            ( std::ostream& o, const Quaternion& q )
        {
            o << "Quaternion(" << q.x << ", " << q.y << ", " << q.z << ", " << q.w << ")";
            return o;
        }

    };

}