cQuaternion.h

Go to the documentation of this file.

//
//
 
#ifndef _INC_cQuaternion_H
#define _INC_cQuaternion_H
#ifndef NO_PRAGMA_ONCE
#pragma once
#endif
 
#include "cVector.h"
#include "GrayCore/include/cDebugAssert.h"
 
namespace GrayLib
{
        UNITTEST2_PREDEF(cQuaternion);
        class cMatrix4x4f;
 
        template<typename TYPE = DVALUEDEF_t>
        class GRAYLIB_LINK cQuaternionT : public cVecT4<TYPE>
        {
        public:
                // typedef TYPE DVALUE_t;
                typedef cVecT4<TYPE> SUPER_t;
 
                cQuaternionT() noexcept
                {}
                cQuaternionT(TYPE _x, TYPE _y, TYPE _z, TYPE _w) noexcept
                        : SUPER_t(_x, _y, _z, _w)
                {
                }
                cQuaternionT(const SUPER_t& q) noexcept
                        : SUPER_t(q)
                {
                }
 
                bool isIdentity() const noexcept
                {
                        if (this->x == 0 && this->y == 0 && this->z == 0 && this->w == 1)
                                return true;
                        return false;
                }
 
                bool IsNearQ(const SUPER_t& q, TYPE fDist = k_FLT_MIN2) const 
                {
                        ASSERT(fDist >= 0);
                        if (!CalcI::IsNear(CalcI::Abs(this->x), CalcI::Abs(q.x), fDist))
                                return false;
                        if (!CalcI::IsNear(CalcI::Abs(this->y), CalcI::Abs(q.y), fDist))
                                return false;
                        if (!CalcI::IsNear(CalcI::Abs(this->z), CalcI::Abs(q.z), fDist))
                                return false;
                        if (!CalcI::IsNear(CalcI::Abs(this->w), CalcI::Abs(q.w), fDist))
                                return false;
                        return true;
                }
 
        private:
                void SetZero()
                {
                }
                bool isZero() const
                {
                        return false;
                }
        };
 
        class GRAYLIB_LINK cQuaternionf : public cQuaternionT<float>
        {
 
        public:
                typedef cQuaternionT<float> SUPER_t;
                typedef cQuaternionf THIS_t;
                static const cQuaternionf k_Identity;
 
        public:
                cQuaternionf() noexcept
                {}
                cQuaternionf(DVALUE_t _x, DVALUE_t _y, DVALUE_t _z, DVALUE_t _w)
                        : SUPER_t(_x, _y, _z, _w)
                {
                }
                cQuaternionf(const SUPER_t& q)
                        : SUPER_t(q)
                {
                }
                cQuaternionf(const cMatrix4x4f& rm)
                {
                        InitRotationMatrix(rm);
                }
                cQuaternionf(const cVector3f& vAxis, RADIANf_t fAngle)
                {
                        InitRotationAxis(vAxis, fAngle);
                }
 
#if defined(USE_DXM) || defined(USE_DX) // ASSUME DVALUE_t = float !!!
                // Cast directly to the DirectX equivalent type. D3DXQUATERNION, FXMVECTOR
                cQuaternionf(const XMFLOAT4& v)
                        : SUPER_t(*(const SUPER_t*)&v)
                {}
                inline operator XMFLOAT4* ()
                {
                        return (XMFLOAT4*)this;
                }
                inline operator const XMFLOAT4* () const
                {
                        return (const XMFLOAT4*)this;
                }
#endif
 
#ifdef USE_DXM
                inline void Set(const XMVECTOR& v)
                {
                        ::XMStoreFloat4((XMFLOAT4*)this, v);
                }
                inline operator XMVECTOR () const
                {
                        return ::XMLoadFloat4((const XMFLOAT4*)this);
                }
#endif
 
                THIS_t GetMulQ(const THIS_t& q) const
                {
                        THIS_t qRet;
                        qRet.InitMulQ(*this, q);
                        return qRet;
                }
                THIS_t operator*(const THIS_t& q) const
                {
                        return GetMulQ(q);
                }
 
                RADIANf_t get_EulerY() const;
                cVector3f get_EulerV() const;
 
                // setters.
 
                void InitIdentity()
                {
                        *this = k_Identity;
                }
                void InitSlerp(const THIS_t& q1, const THIS_t& q2, DVALUE_t t)
                {
                        ASSERT(this != &q1);
                        ASSERT(this != &q2);
#ifdef USE_DXM
                        Set(::XMQuaternionSlerp((XMVECTOR)q1, (XMVECTOR)q2, t));
#else
                        DVALUE_t t2 = (1.0f - t);
                        DVALUE_t epsilon = 1.0f;
                        DVALUE_t dot = q1.GetDot(q2);
                        if (dot < 0.0f)
                                epsilon = -1.0f;
                        t *= epsilon;
                        this->x = t2 * q1.x + t * q2.x;
                        this->y = t2 * q1.y + t * q2.y;
                        this->z = t2 * q1.z + t * q2.z;
                        this->w = t2 * q1.w + t * q2.w;
#endif
                }
                void InitRotationAxis(const cVector3f& vNorm, RADIANf_t fAngle)
                {
#ifdef USE_DXM
                        Set(::XMQuaternionRotationAxis((XMVECTOR)vNorm, fAngle));
#else
                        float s, c;
                        Calc::SinCos(fAngle / 2.0f, s, c);
                        this->x = s * vNorm.x;
                        this->y = s * vNorm.y;
                        this->z = s * vNorm.z;
                        this->w = c;
#endif
                }
                void InitAngle(const cVector3f& v, RADIANf_t fAngle)
                {
                        InitRotationAxis(v.get_Normalized(), fAngle);
                }
 
                void InitMulQ(const THIS_t& q1, const THIS_t& q2)
                {
                        ASSERT(this != &q1);
                        ASSERT(this != &q2);
#ifdef USE_DXM
                        Set(::XMQuaternionMultiply((XMVECTOR)q1, (XMVECTOR)q2));
#else
                        this->x = q2.w * q1.x + q2.x * q1.w + q2.y * q1.z - q2.z * q1.y;
                        this->y = q2.w * q1.y - q2.x * q1.z + q2.y * q1.w + q2.z * q1.x;
                        this->z = q2.w * q1.z + q2.x * q1.y - q2.y * q1.x + q2.z * q1.w;
                        this->w = q2.w * q1.w - q2.x * q1.x - q2.y * q1.y - q2.z * q1.z;
#endif
                }
 
                void InitRotationMatrix(const cMatrix4x4f& rm);
                void InitEulerV(const cVector3f& v); // y=yaw=heading,x=pitch,z=roll=bank radians
 
                void AlignToNormal( /* const*/ cVector3f& vNorm);
                void SetToAngleBetween(const cVector3f& v1, const cVector3f& v2);
                void SetEulerY(RADIANf_t fAngleY, bool bMoveNormalized);
 
                UNITTEST_FRIEND(cQuaternion);
        };
 
 
#ifdef GRAY_DLL // force implementation/instantiate for DLL/SO.
        template class GRAYLIB_LINK cQuaternionT<float>;
        template class GRAYLIB_LINK cQuaternionT<double>;
#endif
 
} 
#endif // _INC_cQuaternion_H