F:/My Projects/LSEngine/Modules/LSMathLib/Src/Quaternion/LSMQuaternion.h

#ifndef __LSM_QUATERNION_H__
#define __LSM_QUATERNION_H__

#include "../LSMMathLib.h"
#include "../Matrix/LSMMatrix4x4.h"
#include "../Vector/LSMVector3.h"

#pragma warning( push )

// warning C4201: nonstandard extension used : nameless struct/union
#pragma warning( disable : 4201 )


namespace lsm {

        class CQuaternion {
        public :
                // == Various constructors.
                LSE_INLINE LSE_CALLCTOR                                 CQuaternion();
                LSE_INLINE LSE_CALLCTOR                                 CQuaternion( LSREAL _fX, LSREAL _fY, LSREAL _fZ, LSREAL _fW );
                LSE_INLINE LSE_CALLCTOR                                 CQuaternion( const CQuaternion &_qQuat );


                // == Operators.
                LSE_INLINE CQuaternion & LSE_FCALL              operator += ( const CQuaternion &_qQuat );

                LSE_INLINE CQuaternion & LSE_FCALL              operator -= ( const CQuaternion &_qQuat );

                LSE_INLINE CQuaternion & LSE_FCALL              operator *= ( const CQuaternion &_qQuat );

                LSE_INLINE CQuaternion & LSE_FCALL              operator *= ( LSREAL _fVal );

                LSE_INLINE CQuaternion & LSE_FCALL              operator /= ( LSREAL _fVal );

                LSE_INLINE CQuaternion  LSE_FCALL               operator + () const;

                LSE_INLINE CQuaternion  LSE_FCALL               operator - () const;

                LSE_INLINE CQuaternion  LSE_FCALL               operator + ( const CQuaternion &_qQuat ) const;
                
                LSE_INLINE CQuaternion  LSE_FCALL               operator - ( const CQuaternion &_qQuat ) const;

                LSE_INLINE CQuaternion  LSE_FCALL               operator * ( const CQuaternion &_qQuat ) const;

                LSE_INLINE CQuaternion  LSE_FCALL               operator * ( LSREAL _fVal ) const;

                LSE_INLINE CQuaternion  LSE_FCALL               operator / ( LSREAL _fVal ) const;

                LSE_INLINE bool LSE_FCALL                               operator == ( const CQuaternion &_qQuat ) const;

                LSE_INLINE bool LSE_FCALL                               operator != ( const CQuaternion &_qQuat ) const;

                LSE_INLINE LSREAL & LSE_FCALL                   operator [] ( LSUINT32 _ui32Index );

                LSE_INLINE LSREAL LSE_FCALL                             operator [] ( LSUINT32 _ui32Index ) const;


                // == Functions.
                LSE_INLINE CQuaternion & LSE_FCALL              Identity();
                
                LSE_INLINE CQuaternion LSE_FCALL                Conjugate() const;
                
                LSE_INLINE LSREAL LSE_FCALL                             Dot( const CQuaternion &_qQuat ) const;
                
                CQuaternion LSE_FCALL                                   Exp() const;
                
                CQuaternion LSE_FCALL                                   Log() const;
                
                LSE_INLINE LSREAL LSE_FCALL                             LenSq() const;
                
                LSE_INLINE LSREAL LSE_FCALL                             Len() const;
                
                LSE_INLINE LSVOID LSE_FCALL                             Normalize();
                
                LSE_INLINE CQuaternion LSE_FCALL                Inverse();
                
                LSE_INLINE CQuaternion & LSE_FCALL              Lerp( const CQuaternion &_qLeft, const CQuaternion &_qRight, LSREAL _fT );
                
                CQuaternion & LSE_FCALL                                 Slerp( const CQuaternion &_qLeft, const CQuaternion &_qRight, LSREAL _fT );
                
                CQuaternion & LSE_FCALL                                 Squad( const CQuaternion &_qQ, const CQuaternion &_qA, const CQuaternion &_qB, const CQuaternion &_qC, LSREAL _fT );

                CQuaternion & LSE_FCALL                                 FromRotationMatrix( const CMatrix4x4 &_mMat );
                
                LSVOID LSE_FCALL                                                ToRotationMatrix( CMatrix4x4 &_mMat ) const;

                CQuaternion & LSE_FCALL                                 FromRotationVectors( const CVector3 _vVectors[3] );

                CQuaternion & LSE_FCALL                                 AddScaledVector( const CVector3 &_vVec, LSREAL _fTime );
                
                LSE_INLINE LSREAL & LSE_FCALL                   X();
                
                LSE_INLINE LSREAL & LSE_FCALL                   Y();
                
                LSE_INLINE LSREAL & LSE_FCALL                   Z();
                
                LSE_INLINE LSREAL & LSE_FCALL                   W();

                // == Members.
#pragma pack( push, 1 )
                struct {
                        LSREAL                                                          x,
                                                                                                y,
                                                                                                z,
                                                                                                w;
                } u;
#pragma pack( pop )

        protected :
                static LSUINT32                                                 m_ui32Next[3];
        };


        // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
        // DEFINITIONS
        // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
        // == Various constructors.
        LSE_INLINE LSE_CALLCTOR CQuaternion::CQuaternion() {}
        LSE_INLINE LSE_CALLCTOR CQuaternion::CQuaternion( LSREAL _fX, LSREAL _fY, LSREAL _fZ, LSREAL _fW ) {
                u.x = _fX;
                u.y = _fY;
                u.z = _fZ;
                u.w = _fW;
        }
        LSE_INLINE LSE_CALLCTOR CQuaternion::CQuaternion( const CQuaternion &_qQuat ) {
                u.x = _qQuat[0];
                u.y = _qQuat[1];
                u.z = _qQuat[2];
                u.w = _qQuat[3];
        }


        // == Operators.
        LSE_INLINE CQuaternion & LSE_FCALL CQuaternion::operator += ( const CQuaternion &_qQuat ) {
                u.x += _qQuat[0];
                u.y += _qQuat[1];
                u.z += _qQuat[2];
                u.w += _qQuat[3];
                return (*this);
        }

        LSE_INLINE CQuaternion & LSE_FCALL CQuaternion::operator -= ( const CQuaternion &_qQuat ) {
                u.x -= _qQuat[0];
                u.y -= _qQuat[1];
                u.z -= _qQuat[2];
                u.w -= _qQuat[3];
                return (*this);
        }

        LSE_INLINE CQuaternion & LSE_FCALL CQuaternion::operator *= ( const CQuaternion &_qQuat ) {
                CQuaternion qProd;

                qProd[3] =
                        u.w * _qQuat[3] -
                        u.x * _qQuat[0] -
                        u.y * _qQuat[1] -
                        u.z * _qQuat[2];

                qProd[0] =
                        u.w * _qQuat[0] +
                        u.x * _qQuat[3] +
                        u.y * _qQuat[2] -
                        u.z * _qQuat[1];

                qProd[1] =
                        u.w * _qQuat[1] +
                        u.y * _qQuat[3] +
                        u.z * _qQuat[0] -
                        u.x * _qQuat[2];

                qProd[2] =
                        u.w * _qQuat[2] +
                        u.z * _qQuat[3] +
                        u.x * _qQuat[1] -
                        u.y * _qQuat[0];

                (*this) = qProd;
                return (*this);
        }

        LSE_INLINE CQuaternion & LSE_FCALL CQuaternion::operator *= ( LSREAL _fVal ) {
                u.x *= _fVal;
                u.y *= _fVal;
                u.z *= _fVal;
                u.w *= _fVal;
                return (*this);
        }

        LSE_INLINE CQuaternion & LSE_FCALL CQuaternion::operator /= ( LSREAL _fVal ) {
                _fVal = LSM_ONE / _fVal;
                u.x *= _fVal;
                u.y *= _fVal;
                u.z *= _fVal;
                u.w *= _fVal;
                return (*this);
        }


        LSE_INLINE CQuaternion LSE_FCALL CQuaternion::operator + () const {
                return CQuaternion( +u.x, +u.y, +u.z, +u.w );
        }

        LSE_INLINE CQuaternion LSE_FCALL CQuaternion::operator - () const {
                return CQuaternion( -u.x, -u.y, -u.z, -u.w );
        }


        LSE_INLINE CQuaternion LSE_FCALL CQuaternion::operator + ( const CQuaternion &_qQuat ) const {
                return CQuaternion( u.x + _qQuat.u.x, u.y + _qQuat.u.y, u.z + _qQuat.u.z, u.w + _qQuat.u.w );
        }

        LSE_INLINE CQuaternion LSE_FCALL CQuaternion::operator - ( const CQuaternion &_qQuat ) const {
                return CQuaternion( u.x - _qQuat.u.x, u.y - _qQuat.u.y, u.z - _qQuat.u.z, u.w - _qQuat.u.w );
        }

        LSE_INLINE CQuaternion LSE_FCALL CQuaternion::operator * ( const CQuaternion &_qQuat ) const {
                /*return CQuaternion( u.w * _qQuat.u.x +
                        u.x * _qQuat.u.w +
                        u.y * _qQuat.u.z -
                        u.z * _qQuat.u.y,
                        u.w * _qQuat.u.y +
                        u.y * _qQuat.u.w +
                        u.z * _qQuat.u.x -
                        u.x * _qQuat.u.z,
                        u.w * _qQuat.u.z +
                        u.z * _qQuat.u.w +
                        u.x * _qQuat.u.y -
                        u.y * _qQuat.u.x,
                        u.w * _qQuat.u.w -
                        u.x * _qQuat.u.x -
                        u.y * _qQuat.u.y -
                        u.z * _qQuat.u.z );*/
                return CQuaternion( u.w * _qQuat.u.x +
                        u.x * _qQuat.u.w -
                        u.y * _qQuat.u.z -
                        u.z * _qQuat.u.y,
                        u.w * _qQuat.u.y -
                        u.x * _qQuat.u.z +
                        u.y * _qQuat.u.w -
                        u.z * _qQuat.u.x,
                        u.w * _qQuat.u.z +
                        u.x * _qQuat.u.y -
                        u.y * _qQuat.u.x +
                        u.z * _qQuat.u.w,
                        u.w * _qQuat.u.w -
                        u.x * _qQuat.u.x -
                        u.y * _qQuat.u.y -
                        u.z * _qQuat.u.z );
        }

        LSE_INLINE CQuaternion LSE_FCALL CQuaternion::operator * ( LSREAL _fVal ) const {
                return CQuaternion( u.x * _fVal, u.y * _fVal, u.z * _fVal, u.w * _fVal );
        }

        LSE_INLINE CQuaternion LSE_FCALL CQuaternion::operator / ( LSREAL _fVal ) const {
                register LSREAL fVal = LSM_ONE / _fVal;
                return CQuaternion( u.x * fVal, u.y * fVal, u.z * fVal, u.w * fVal );
        }

        LSE_INLINE bool LSE_FCALL CQuaternion::operator == ( const CQuaternion &_qQuat ) const {
                return u.x == _qQuat[0] && u.y == _qQuat[1] && u.z == _qQuat[2] && u.w == _qQuat[3];
        }

        LSE_INLINE bool LSE_FCALL CQuaternion::operator != ( const CQuaternion &_qQuat ) const {
                return u.x != _qQuat[0] || u.y != _qQuat[1] || u.z != _qQuat[2] || u.w != _qQuat[3];
        }

        LSE_INLINE LSREAL & LSE_FCALL CQuaternion::operator [] ( LSUINT32 _ui32Index ) {
                return reinterpret_cast<LSREAL *>(this)[_ui32Index];
        }

        LSE_INLINE LSREAL LSE_FCALL CQuaternion::operator [] ( LSUINT32 _ui32Index ) const {
                return reinterpret_cast<const LSREAL *>(this)[_ui32Index];
        }


        LSE_INLINE CQuaternion & LSE_FCALL CQuaternion::Identity() {
                u.x = u.y = u.z = LSM_ZERO;
                u.w = LSM_ONE;
                return (*this);
        }

        LSE_INLINE CQuaternion LSE_FCALL CQuaternion::Conjugate() const {
                return CQuaternion( -u.x, -u.y, -u.z, u.w );
        }

        LSE_INLINE LSREAL LSE_FCALL CQuaternion::Dot( const CQuaternion &_qQuat ) const {
                return (u.x * _qQuat[0]) +
                        (u.y * _qQuat[1]) +
                        (u.z * _qQuat[2]) +
                        (u.w * _qQuat[3]);
        }

        LSE_INLINE LSREAL LSE_FCALL CQuaternion::LenSq() const {
                return u.x * u.x +
                        u.y * u.y +
                        u.z * u.z +
                        u.w * u.w;
        }

        LSE_INLINE LSREAL LSE_FCALL CQuaternion::Len() const {
                return CMathLib::Sqrt( u.x * u.x +
                        u.y * u.y +
                        u.z * u.z +
                        u.w * u.w );
        }

        LSE_INLINE LSVOID LSE_FCALL CQuaternion::Normalize() {
                LSREAL fInvLen = CMathLib::InvSqrt( u.x * u.x +
                        u.y * u.y +
                        u.z * u.z +
                        u.w * u.w );
                u.x *= fInvLen;
                u.y *= fInvLen;
                u.z *= fInvLen;
                u.w *= fInvLen;
        }

        LSE_INLINE CQuaternion LSE_FCALL CQuaternion::Inverse() {
                CQuaternion qRet;

                LSREAL fInvLen = -CMathLib::InvSqrt( u.x * u.x +
                        u.y * u.y +
                        u.z * u.z +
                        u.w * u.w );

                qRet[3] = u.w * -fInvLen;
                qRet[0] = u.x * fInvLen;
                qRet[1] = u.y * fInvLen;
                qRet[2] = u.z * fInvLen;

                return qRet;
        }

        LSE_INLINE CQuaternion & LSE_FCALL CQuaternion::Lerp( const CQuaternion &_qLeft, const CQuaternion &_qRight, LSREAL _fT ) {
                (*this) = _qLeft + (_qRight - _qLeft) * _fT;
                Normalize();
                return (*this);
        }

        LSE_INLINE LSVOID LSE_FCALL CQuaternion::ToRotationMatrix( CMatrix4x4 &_mMat ) const {
                LSREAL fTx  = LSM_TWO * u.x;
                LSREAL fTy  = LSM_TWO * u.y;
                LSREAL fTz  = LSM_TWO * u.z;
                LSREAL fTwx = fTx * u.w;
                LSREAL fTwy = fTy * u.w;
                LSREAL fTwz = fTz * u.w;
                LSREAL fTxx = fTx * u.x;
                LSREAL fTxy = fTy * u.x;
                LSREAL fTxz = fTz * u.x;
                LSREAL fTyy = fTy * u.y;
                LSREAL fTyz = fTz * u.y;
                LSREAL fTzz = fTz * u.z;

                _mMat( 0, 0 ) = LSM_ONE - (fTyy + fTzz);
                _mMat( 1, 0 ) = fTxy - fTwz;
                _mMat( 2, 0 ) = fTxz + fTwy;
                _mMat( 0, 1 ) = fTxy + fTwz;
                _mMat( 1, 1 ) = LSM_ONE - (fTxx + fTzz);
                _mMat( 2, 1 ) = fTyz - fTwx;
                _mMat( 0, 2 ) = fTxz - fTwy;
                _mMat( 1, 2 ) = fTyz + fTwx;
                _mMat( 2, 2 ) = LSM_ONE - (fTxx + fTyy);
        }
        
        LSE_INLINE LSREAL & LSE_FCALL CQuaternion::X() {
                return u.x;
        }
        
        LSE_INLINE LSREAL & LSE_FCALL CQuaternion::Y() {
                return u.y;
        }
        
        LSE_INLINE LSREAL & LSE_FCALL CQuaternion::Z() {
                return u.z;
        }
        
        LSE_INLINE LSREAL & LSE_FCALL CQuaternion::W() {
                return u.w;
        }

}       // namespace lsm

#pragma warning( pop )

#endif  // __LSM_QUATERNION_H__