tb_quaternion.hpp

#ifndef TurtleBrains_Quaternion_hpp
#define TurtleBrains_Quaternion_hpp

#include <turtle_brains/math/tb_math.hpp>
#include <turtle_brains/core/tb_configuration.hpp>
#include <turtle_brains/core/tb_error.hpp>
#include <turtle_brains/core/tb_defines.hpp> //For tb_unsused
#include <turtle_brains/math/tb_vector.hpp> //For SkipInitialization

namespace TurtleBrains
{
    namespace Math
    {
        class Matrix3;
        class Matrix4;

        namespace Unstable {

            class Quaternion;

            Vector3 MultiplyVector3Quaternion(const Vector3& left, const Quaternion& right);
            Quaternion* QuaternionMultiply(Quaternion* result, const Quaternion* left, const Quaternion* right);

            class Quaternion
            {
            public:
                static Quaternion Identity(void) { return Quaternion(0.0f, 0.0f, 0.0f, 1.0f); }

                union
                {
                    float mComponents[4];

#if defined(tb_visual_cpp)
#pragma warning(push)
#pragma warning(disable: 4201)
                    struct { float x, y, z, w; };
#pragma warning(pop)
#else
                    struct { float x, y, z, w; };
#endif
                };

                inline explicit Quaternion(const SkipInitialization& fastAndStupid)
                {
                    tb_unused(fastAndStupid);
                }

                inline Quaternion(void) :
                    x(0.0f),
                    y(0.0f),
                    z(0.0f),
                    w(1.0f)
                {
                }

                inline Quaternion(const float valueX, const float valueY, const float valueZ, const float valueW) :
                    x(valueX),
                    y(valueY),
                    z(valueZ),
                    w(valueW)
                {
                }

                inline explicit Quaternion(const Vector3& other, const float valueW) :
                    x(other.x),
                    y(other.y),
                    z(other.z),
                    w(valueW)
                {
                    tb_error("Probably useful to explicitly convert (not copy value) see HAXE: from_angle_axis function");
                }

                inline explicit Quaternion(const float* componentArray) :
                    x(componentArray[0]),
                    y(componentArray[1]),
                    z(componentArray[2]),
                    w(componentArray[3])
                {
                }

                inline Quaternion(const Quaternion& other) :
                    x(other.x),
                    y(other.y),
                    z(other.z),
                    w(other.w)
                {
                }

                ~Quaternion(void)
                {
                }

                inline Quaternion& operator=(const Quaternion& other)
                {
                    x = other.x;
                    y = other.y;
                    z = other.z;
                    w = other.w;
                    return *this;
                }

                inline bool operator==(const Quaternion& other) const
                {
                    return (true == IsEqual(x, other.x) && true == IsEqual(y, other.y) &&
                        true == IsEqual(z, other.z) && true == IsEqual(w, other.w)) ? true : false;
                }

                inline bool operator!=(const Quaternion& other) const
                {
                    return (true == operator==(other)) ? false : true;
                }

                inline operator const float*(void) const { return mComponents; }

                inline operator float*(void) { return mComponents; }

                inline const float& operator[](const int index) const { return mComponents[index]; }

                inline float& operator[](const int index) { return mComponents[index]; }

#if defined(tb_with_math_operators)

                inline Quaternion operator+(const Quaternion& rightSide) const { return Quaternion(x + rightSide.x, y + rightSide.y, z + rightSide.z, w + rightSide.w); }

                inline Quaternion& operator+=(const Quaternion& rightSide) { x += rightSide.x; y += rightSide.y; z += rightSide.z; w += rightSide.w; return *this; }

                inline Quaternion operator-(const Quaternion& rightSide) const { return Quaternion(x - rightSide.x, y - rightSide.y, z - rightSide.z, w - rightSide.w); }

                inline Quaternion& operator-=(const Quaternion& rightSide) { x -= rightSide.x; y -= rightSide.y; z -= rightSide.z; w -= rightSide.w; return *this; }

                inline Quaternion operator*(const Quaternion& rightSide) const
                {
                    Quaternion result(SkipInitialization::kSkipInitialization);
                    QuaternionMultiply(&result, this, &rightSide);
                    return result;
                }

                inline Quaternion operator*=(const Quaternion& rightSide)
                {
                    Quaternion result(SkipInitialization::kSkipInitialization);
                    QuaternionMultiply(&result, this, &rightSide);
                    *this = result;
                    return *this;
                }

                inline Quaternion operator*(float scalar) const { return Quaternion(x * scalar, y * scalar, z * scalar, w * scalar); }

                friend Quaternion operator*(float scalar, const Quaternion& rightSide) { return Quaternion(scalar * rightSide.x, scalar * rightSide.y, scalar * rightSide.z, scalar * rightSide.w); }

                inline Quaternion& operator*=(float scalar) { x *= scalar; y *= scalar; z *= scalar; w *= scalar; return *this; }

                inline Quaternion operator/(float scalar) const { return Quaternion(x / scalar, y / scalar, z / scalar, w / scalar); }

                inline Quaternion& operator/=(float scalar) { x /= scalar; y /= scalar; z /= scalar; w /= scalar; return *this; }

                inline Quaternion operator-(void) const { return Quaternion(-x, -y, -z, -w); }
#endif /* tb_math_with_operators */

                inline float Magnitude(void) const { return sqrt((x * x) + (y * y) + (z * z) + (w * w)); }

                inline float MagnitudeSquared(void) const { return (x * x) + (y * y) + (z * z) + (w * w); }

                inline Quaternion GetNormalized(void) const
                {
                    const float magnitude(Magnitude());
                    if (true == IsZero(magnitude)) { return Identity(); }
                    return Quaternion(x / magnitude, y / magnitude, z / magnitude, w / magnitude);
                }

                inline float Normalize(void)
                {
                    const float magnitude(Magnitude());
                    if (true == IsZero(magnitude))
                    {
                        x = 0.0f;
                        y = 0.0f;
                        z = 0.0f;
                        w = 1.0f;
                    }
                    else
                    {
                        x /= magnitude;
                        y /= magnitude;
                        z /= magnitude;
                        w /= magnitude;
                    }
                    return magnitude;
                }

                inline Vector3 ApplyForward(const Vector3& forward) const
                {
                    return MultiplyVector3Quaternion(forward, *this);
                }

                inline Vector3 ToEuler(void) const
                {
                    return Vector3(
                        atan2f(2 * mComponents[1] * mComponents[3] - 2 * mComponents[0] * mComponents[2], 1 - 2 * powf(mComponents[1], 2) - 2 * powf(mComponents[2], 2)), // heading
                        asinf(2 * mComponents[0] * mComponents[1] + 2 * mComponents[2] * mComponents[3]), // attitude
                        atan2f(2 * mComponents[0] * mComponents[3] - 2 * mComponents[1] * mComponents[2], 1 - 2 * powf(mComponents[0], 2) - 2 * powf(mComponents[2], 2)) // bank
                    );
                }

                inline void ToAngleAxis(Angle& angle, Vector3& axis) const
                {
                    const Quaternion normalizedSelf((w > 1.0f || w < -1.0f) ? GetNormalized() : *this);

                    const float s = sqrtf(1.0f - normalizedSelf.w * normalizedSelf.w); //var s = Math.sqrt(1 - a[3] * a[3]);
                    angle = Angle(2.0f * acosf(normalizedSelf.w), AngleUnit::Radians);
                    axis = (s < 0.995f) ? Vector3(normalizedSelf.x, normalizedSelf.y, normalizedSelf.z) :
                        Vector3(normalizedSelf.x / s, normalizedSelf.y / s, normalizedSelf.z / s);
                }

                inline static Quaternion FromAngleAxis(const Angle angle, const Vector3& axis)
                {
                    const float s = sinf(angle.AsRadians() * 0.5f);
                    const float c = cosf(angle.AsRadians() * 0.5f);
                    return Quaternion(axis[0] * s, axis[1] * s, axis[2] * s, c);
                }

                inline static Quaternion FromForward(const Vector3& forward, const Vector3* up = nullptr)
                {
                    const tbMath::Vector3 unitForward = forward.GetNormalized();
                    const tbMath::Vector3 unitUp = ((nullptr == up) ? Vector3(0.0f, 1.0f, 0.0f) : up->GetNormalized());

                    const float dot = Vector3DotProduct(&unitUp, &unitForward);
                    Vector3 axis(SkipInitialization::kSkipInitialization);
                    Vector3CrossProduct(&axis, &unitUp, &unitForward);
                    return Quaternion(axis, dot + 1.0f);
                }

                inline static Quaternion FromEuler(const Vector3& euler)
                {
                    const float halfHeading = euler.x * 0.5f;
                    const float halfPitch = euler.y * 0.5f;
                    const float halfRoll = euler.z * 0.5f;

                    const float c1 = cosf(halfHeading);
                    const float s1 = sinf(halfHeading);
                    const float c2 = cosf(halfPitch);
                    const float s2 = sinf(halfPitch);
                    const float c3 = cosf(halfRoll);
                    const float s3 = sinf(halfRoll);
                    const float c1c2 = c1*c2;
                    const float s1s2 = s1*s2;
                    return Quaternion(
                        c1c2*s3 + s1s2*c3,
                        s1*c2*c3 + c1*s2*s3,
                        c1*s2*c3 - s1*c2*s3,
                        c1c2*c3 - s1s2*s3
                    );
                }

                //These are defined in tb_matrix_quaternion.h
                static Quaternion FromMatrix(const Matrix3& matrix);
                static Quaternion FromMatrix(const Matrix4& matrix);

            };

    //-------------------------------------------------------------------------------------------------------------------//
    //-------------------------------------------------------------------------------------------------------------------//
    //-------------------------------------------------------------------------------------------------------------------//

            inline Quaternion* QuaternionAdd(Quaternion* result, const Quaternion* left, const Quaternion* right)
            {
                tb_error_if(nullptr == left, "tbExternalError: Invalid parameter for left, expected valid pointer.");
                tb_error_if(nullptr == right, "tbExternalError: Invalid parameter for right, expected valid pointer.");
                tb_error_if(nullptr == result, "tbExternalError: Invalid parameter for result, expected valid pointer.");

                result->x = left->x + right->x;
                result->y = left->y + right->y;
                result->z = left->z + right->z;
                result->w = left->w + right->w;
                return result;
            }

            inline Quaternion* QuaternionSubtract(Quaternion* result, const Quaternion* left, const Quaternion* right)
            {
                tb_error_if(nullptr == left, "tbExternalError: Invalid parameter for left, expected valid pointer.");
                tb_error_if(nullptr == right, "tbExternalError: Invalid parameter for right, expected valid pointer.");
                tb_error_if(nullptr == result, "tbExternalError: Invalid parameter for result, expected valid pointer.");

                result->x = left->x - right->x;
                result->y = left->y - right->y;
                result->z = left->z - right->z;
                result->w = left->w - right->w;
                return result;
            }

            inline Quaternion* QuaternionMultiply(Quaternion* result, const Quaternion* left, const Quaternion* right)
            {
                tb_error_if(nullptr == left, "tbExternalError: Invalid parameter for left, expected valid pointer.");
                tb_error_if(nullptr == right, "tbExternalError: Invalid parameter for right, expected valid pointer.");
                tb_error_if(nullptr == result, "tbExternalError: Invalid parameter for result, expected valid pointer.");
                tb_error_if(result == left || result == right, "tbExternalError: Invalid parameter for result, expected different location than left or right.")

                    const Quaternion& a(*left);
                const Quaternion& b(*right);
                //(*result)[0] = a[0] * b[3] + a[3] * b[0] + a[1] * b[2] - a[2] * b[1];
                //(*result)[1] = a[1] * b[3] + a[3] * b[1] + a[2] * b[0] - a[0] * b[2];
                //(*result)[2] = a[2] * b[3] + a[3] * b[2] + a[0] * b[1] - a[1] * b[0];
                //(*result)[3] = a[3] * b[3] - a[0] * b[0] - a[1] * b[1] - a[2] * b[2];

                result->x = a.x * b.w + a.w * b.x + a.y * b.z - a.z * b.y;
                result->y = a.y * b.w + a.w * b.y + a.z * b.x - a.x * b.z;
                result->z = a.z * b.w + a.w * b.z + a.x * b.y - a.y * b.x;
                result->w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z;
                return result;
            }

            inline Quaternion* QuaternionScale(Quaternion* result, const Quaternion* input, const float scalar)
            {
                tb_error_if(nullptr == result, "tbExternalError: Invalid parameter for result, expected valid pointer.");
                tb_error_if(nullptr == input, "tbExternalError: Invalid parameter for input, expected valid pointer.");

                result->x = input->x * scalar;
                result->y = input->y * scalar;
                result->z = input->z * scalar;
                result->w = input->w * scalar;
                return result;
            }

            inline Quaternion* QuaternionScaleDivide(Quaternion* result, const Quaternion* input, const float scalar)
            {
                tb_error_if(nullptr == result, "tbExternalError: Invalid parameter for result, expected valid pointer.");
                tb_error_if(nullptr == input, "tbExternalError: Invalid parameter for input, expected valid pointer.");

                result->x = input->x / scalar;
                result->y = input->y / scalar;
                result->z = input->z / scalar;
                result->w = input->w / scalar;
                return result;
            }

            inline Quaternion* QuaternionNegate(Quaternion* result, const Quaternion* input)
            {
                tb_error_if(nullptr == input, "tbExternalError: Invalid parameter for input, expected valid pointer.");
                tb_error_if(nullptr == result, "tbExternalError: Invalid parameter for result, expected valid pointer.");

                result->x = -input->x;
                result->y = -input->y;
                result->z = -input->z;
                result->w = -input->w;
                return result;
            }

            inline float QuaternionMagnitude(const Quaternion* input)
            {
                return sqrtf((input->x * input->x) + (input->y * input->y) + (input->z * input->z) + (input->w * input->w));
            }

            inline float QuaternionMagnitudeSquared(const Quaternion* input)
            {
                return (input->x * input->x) + (input->y * input->y) + (input->z * input->z) + (input->w * input->w);
            }

            inline Quaternion* QuaternionNormalize(Quaternion* result, const Quaternion* input)
            {
                tb_error_if(nullptr == result, "tbExternalError: Invalid parameter for result, expected valid pointer.");
                tb_error_if(nullptr == input, "tbExternalError: Invalid parameter for input, expected valid pointer.");

                const float magnitude = QuaternionMagnitude(input);
                if (true == IsZero(magnitude))
                {
                    result->x = 0.0f;
                    result->y = 0.0f;
                    result->z = 0.0f;
                    result->w = 1.0f;
                }
                else
                {
                    result->x = input->x / magnitude;
                    result->y = input->y / magnitude;
                    result->z = input->z / magnitude;
                    result->w = input->w / magnitude;
                }
                return result;
            }

            inline Quaternion* QuaternionNormalizeMagnitude(Quaternion* result, const Quaternion* input, float &magnitude)
            {
                tb_error_if(nullptr == result, "tbExternalError: Invalid parameter for result, expected valid pointer.");
                tb_error_if(nullptr == input, "tbExternalError: Invalid parameter for input, expected valid pointer.");

                magnitude = QuaternionMagnitude(input);
                if (true == IsZero(magnitude))
                {
                    result->x = 0.0f;
                    result->y = 0.0f;
                    result->z = 0.0f;
                    result->w = 1.0f;
                }
                else
                {
                    result->x = input->x / magnitude;
                    result->y = input->y / magnitude;
                    result->z = input->z / magnitude;
                    result->w = input->w / magnitude;
                }
                return result;
            }

            inline Quaternion* QuaternionInverse(Quaternion* result, const Quaternion* input)
            {
                tb_error_if(nullptr == input, "tbExternalError: Invalid parameter for input, expected valid pointer.");
                tb_error_if(nullptr == result, "tbExternalError: Invalid parameter for result, expected valid pointer.");

                result->x = -input->x;
                result->y = -input->y;
                result->z = -input->z;
                result->w = input->w;
                return result;
            }

            inline float QuaternionDotProduct(const Quaternion* leftSide, const Quaternion* rightSide)
            {
                tb_error_if(nullptr == leftSide, "tbExternalError: Invalid parameter for leftSide, expected valid pointer.");
                tb_error_if(nullptr == rightSide, "tbExternalError: Invalid parameter for rightSide, expected valid pointer.");
                return (leftSide->x * rightSide->x) + (leftSide->y * rightSide->y) + (leftSide->z * rightSide->z) + (leftSide->w * rightSide->w);
            }

            //Linear Interpolation
            inline Quaternion* QuaternionLerp(Quaternion* result, const Quaternion* start, const Quaternion* final, const float time)
            {
                tb_error_if(nullptr == start, "tbExternalError: Invalid parameter for start, expected valid pointer.");
                tb_error_if(nullptr == final, "tbExternalError: Invalid parameter for final, expected valid pointer.");
                tb_error_if(nullptr == result, "tbExternalError: Invalid parameter for result, expected valid pointer.");
                tb_error_if(result == start || result == final, "tbExternalError: Invalid parameter for result, expected different location than start or final.")

                //result = a + (b - a) * t;
                    QuaternionSubtract(result, final, start); // b - a
                QuaternionScale(result, result, time); //(b - a) * t
                QuaternionAdd(result, start, result); //result = a + (b - a) * t;
                result->Normalize();
                return result;
            }

            //Spherical Interpolation
            inline Quaternion QuaternionSlerp(const Quaternion& start, const Quaternion& final, const float time)
            {
                //a = start, b = final

                float dot = QuaternionDotProduct(&start, &final);

                Quaternion a(start);
                if (dot < 0.0f)
                {
                    QuaternionNegate(&a, &start);
                    dot = -dot;
                }

                if (dot > 0.995f)
                {   //Too close to bother with.
                    Quaternion result;
                    QuaternionLerp(&result, &start, &final, time);
                    return result;
                }

                //If inputs are already normalized, this winds up doing nothing.
                dot = tbMath::Clamp(dot, -1.0f, 1.0f);

                Quaternion temporary; //= b - a * d;
                QuaternionScale(&temporary, &a, dot); //a * dot
                QuaternionSubtract(&temporary, &final, &temporary); //b - (a * dot)
                temporary.Normalize();

                const float theta = acosf(dot) * time;
                QuaternionScale(&temporary, &temporary, sinf(theta));
                QuaternionScale(&a, &a, cosf(theta));

                //return a * Math.cos(theta) + c * Math.sin(theta);
                Quaternion result(SkipInitialization::kSkipInitialization);
                QuaternionAdd(&result, &a, &temporary);
                return result;
            }

            inline Vector3 MultiplyVector3Quaternion(const Vector3& left, const Quaternion& right)
            {
                Vector3 uv(SkipInitialization::kSkipInitialization);
                Vector3 uuv(SkipInitialization::kSkipInitialization);
                Vector3CrossProduct(&uv, reinterpret_cast<const Vector3*>(&right[0]), &left);
                Vector3CrossProduct(&uuv, reinterpret_cast<const Vector3*>(&right[0]), &uv);
                return left + ((uv * right.w) + uuv) * 2.0f;
            }

        }; /* namespace Unstable */
    }; /* namespace Math */
}; /* namespace TurtleBrains */

namespace tbMath = TurtleBrains::Math;

#endif /* TurtleBrains_Quaternion_hpp */