tb_quaternion.hpp

 
#ifndef TurtleBrains_Quaternion_hpp
#define TurtleBrains_Quaternion_hpp
 
#include <turtle_brains/math/tb_math.hpp>
#include <turtle_brains/math/tb_interpolation.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
#include <turtle_brains/math/tb_angle.hpp>
 
namespace TurtleBrains
{
    namespace Math
    {
        template<typename Type> class TypedMatrix3;
        template<typename Type> class TypedMatrix4;
        template<typename Type> class TypedQuaternion;
 
        typedef TypedQuaternion<float> Quaternion;
 
        template<typename Type> TypedVector3<Type> MultiplyVector3Quaternion(const TypedVector3<Type>& left, const TypedQuaternion<Type>& right);
        template<typename Type> TypedQuaternion<Type>* QuaternionMultiply(TypedQuaternion<Type>* result,
            const TypedQuaternion<Type>* left, const TypedQuaternion<Type>* right);
 
        template<typename Type> class TypedQuaternion
        {
        public:
            static TypedQuaternion Identity(void) { return TypedQuaternion(Type(0), Type(0), Type(0), Type(1)); }
 
            union
            {
                Type mComponents[4];
 
#if defined(tb_visual_cpp)
#pragma warning(push)
#pragma warning(disable: 4201)
                struct { Type x, y, z, w; };
#pragma warning(pop)
#else
                struct { Type x, y, z, w; };
#endif
            };
 
            inline explicit TypedQuaternion(const SkipInitialization& fastAndStupid)
            {
                tb_unused(fastAndStupid);
            }
 
            inline TypedQuaternion(void) :
                x(Type(0)),
                y(Type(0)),
                z(Type(0)),
                w(Type(1))
            {
            }
 
            inline TypedQuaternion(const Type valueX, const Type valueY, const Type valueZ, const Type valueW) :
                x(valueX),
                y(valueY),
                z(valueZ),
                w(valueW)
            {
            }
 
            inline explicit TypedQuaternion(const TypedVector3<Type>& other, const Type 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 TypedQuaternion(const Type* componentArray) :
                x(componentArray[0]),
                y(componentArray[1]),
                z(componentArray[2]),
                w(componentArray[3])
            {
            }
 
            inline TypedQuaternion(const TypedQuaternion& other) :
                x(other.x),
                y(other.y),
                z(other.z),
                w(other.w)
            {
            }
 
            ~TypedQuaternion(void)
            {
            }
 
            inline TypedQuaternion& operator=(const TypedQuaternion& other)
            {
                x = other.x;
                y = other.y;
                z = other.z;
                w = other.w;
                return *this;
            }
 
            inline bool operator==(const TypedQuaternion& 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 TypedQuaternion& other) const
            {
                return (true == operator==(other)) ? false : true;
            }
 
            inline operator const Type*(void) const
            {
                return mComponents;
            }
 
            inline operator Type*(void)
            {
                return mComponents;
            }
 
            template<typename ToType> explicit operator TypedQuaternion<ToType>(void) const
            {
                return TypedQuaternion<ToType>(static_cast<ToType>(x), static_cast<ToType>(y),
                    static_cast<ToType>(z), static_cast<ToType>(w));
            }
 
            inline const Type& operator[](const int index) const
            {
                return mComponents[index];
            }
 
            inline Type& operator[](const int index)
            {
                return mComponents[index];
            }
 
#if defined(tb_with_math_operators)
 
            inline TypedQuaternion operator+(const TypedQuaternion& rightSide) const
            {
                return TypedQuaternion(x + rightSide.x, y + rightSide.y, z + rightSide.z, w + rightSide.w);
            }
 
            inline TypedQuaternion& operator+=(const TypedQuaternion& rightSide)
            {
                x += rightSide.x; y += rightSide.y; z += rightSide.z; w += rightSide.w;
                return *this;
            }
 
            inline TypedQuaternion operator-(const TypedQuaternion& rightSide) const
            {
                return TypedQuaternion(x - rightSide.x, y - rightSide.y, z - rightSide.z, w - rightSide.w);
            }
 
            inline TypedQuaternion& operator-=(const TypedQuaternion& rightSide)
            {
                x -= rightSide.x; y -= rightSide.y; z -= rightSide.z; w -= rightSide.w;
                return *this;
            }
 
            inline TypedQuaternion operator*(const TypedQuaternion& rightSide) const
            {
                TypedQuaternion result(SkipInitialization::kSkipInitialization);
                QuaternionMultiply(&result, this, &rightSide);
                return result;
            }
 
            inline TypedQuaternion operator*=(const TypedQuaternion& rightSide)
            {
                TypedQuaternion result(SkipInitialization::kSkipInitialization);
                QuaternionMultiply(&result, this, &rightSide);
                *this = result;
                return *this;
            }
 
            inline TypedQuaternion operator*(Type scalar) const
            {
                return TypedQuaternion(x * scalar, y * scalar, z * scalar, w * scalar);
            }
 
            friend TypedQuaternion operator*(Type scalar, const TypedQuaternion& rightSide)
            {
                return TypedQuaternion(scalar * rightSide.x, scalar * rightSide.y, scalar * rightSide.z, scalar * rightSide.w);
            }
 
            inline TypedQuaternion& operator*=(Type scalar)
            {
                x *= scalar; y *= scalar; z *= scalar; w *= scalar;
                return *this;
            }
 
            inline TypedQuaternion operator/(Type scalar) const
            {
                return TypedQuaternion(x / scalar, y / scalar, z / scalar, w / scalar);
            }
 
            inline TypedQuaternion& operator/=(Type scalar)
            {
                x /= scalar;
                y /= scalar;
                z /= scalar;
                w /= scalar;
                return *this;
            }
 
            inline TypedQuaternion operator-(void) const
            {
                return TypedQuaternion(-x, -y, -z, -w);
            }
 
#endif /* tb_math_with_operators */
 
            inline Type Magnitude(void) const
            {
                return sqrt((x * x) + (y * y) + (z * z) + (w * w));
            }
 
            inline Type MagnitudeSquared(void) const
            {
                return (x * x) + (y * y) + (z * z) + (w * w);
            }
 
            inline TypedQuaternion GetNormalized(void) const
            {
                const Type magnitude(Magnitude());
                if (true == IsZero(magnitude)) { return Identity(); }
                return TypedQuaternion(x / magnitude, y / magnitude, z / magnitude, w / magnitude);
            }
 
            inline Type Normalize(void)
            {
                const Type magnitude(Magnitude());
                if (true == IsZero(magnitude))
                {
                    *this = Identity();
                }
                else
                {
                    x /= magnitude;
                    y /= magnitude;
                    z /= magnitude;
                    w /= magnitude;
                }
                return magnitude;
            }
 
            inline TypedVector3<Type> ApplyForward(const TypedVector3<Type>& forward) const
            {
                return MultiplyVector3Quaternion(forward, *this);
            }
 
            inline TypedVector3<Type> ToEuler(const AngleUnit& angleUnits) const
            {
                //return TypedVector3<Type>(
                //  TypedAngle<Type>(std::atan2(Type(2) * mComponents[1] * mComponents[3] - Type(2) * mComponents[0] * mComponents[2],
                //      Type(1) - Type(2) * std::pow(mComponents[1], Type(2)) - Type(2) * std::pow(mComponents[2], Type(2))),
                //      AngleUnit::Radians).As(angleUnits), // heading
 
                //  TypedAngle<Type>(std::asin(Type(2) * mComponents[0] * mComponents[1] + 2 * mComponents[2] * mComponents[3]),
                //      AngleUnit::Radians).As(angleUnits), // attitude
 
                //  TypedAngle<Type>(std::atan2(Type(2) * mComponents[0] * mComponents[3] - Type(2) * mComponents[1] * mComponents[2],
                //      Type(1) - Type(2) * std::pow(mComponents[0], Type(2)) - Type(2) * std::pow(mComponents[2], Type(2))),
                //      AngleUnit::Radians).As(angleUnits) // bank
                //);
 
                //return TypedVector3<Type>(
                //  TypedAngle<Type>(std::asin(Type(2) * mComponents[0] * mComponents[1] + 2 * mComponents[2] * mComponents[3]),
                //      AngleUnit::Radians).As(angleUnits), // attitude
 
                //  TypedAngle<Type>(std::atan2(Type(2) * mComponents[1] * mComponents[3] - Type(2) * mComponents[0] * mComponents[2],
                //      Type(1) - Type(2) * std::pow(mComponents[1], Type(2)) - Type(2) * std::pow(mComponents[2], Type(2))),
                //      AngleUnit::Radians).As(angleUnits), // heading
 
                //  TypedAngle<Type>(std::atan2(Type(2) * mComponents[0] * mComponents[3] - Type(2) * mComponents[1] * mComponents[2],
                //      Type(1) - Type(2) * std::pow(mComponents[0], Type(2)) - Type(2) * std::pow(mComponents[2], Type(2))),
                //      AngleUnit::Radians).As(angleUnits) // bank
                //);
 
                return TypedVector3<Type>(
                    TypedAngle<Type>(std::atan2(Type(2) * mComponents[0] * mComponents[3] - Type(2) * mComponents[1] * mComponents[2],
                        Type(1) - Type(2) * std::pow(mComponents[0], Type(2)) - Type(2) * std::pow(mComponents[2], Type(2))),
                        AngleUnit::Radians).As(angleUnits), // bank
 
                    TypedAngle<Type>(std::atan2(Type(2) * mComponents[1] * mComponents[3] - Type(2) * mComponents[0] * mComponents[2],
                        Type(1) - Type(2) * std::pow(mComponents[1], Type(2)) - Type(2) * std::pow(mComponents[2], Type(2))),
                        AngleUnit::Radians).As(angleUnits), // heading
 
                    TypedAngle<Type>(std::asin(Type(2) * mComponents[0] * mComponents[1] + 2 * mComponents[2] * mComponents[3]),
                        AngleUnit::Radians).As(angleUnits) // attitude
                );
            }
 
            inline void ToAngleAxis(TypedAngle<Type>& angle, TypedVector3<Type>& axis) const
            {
                const TypedQuaternion normalizedSelf((w > Type(1.0) || w < Type(-1.0)) ? GetNormalized() : *this);
 
                const Type s = std::sqrt(Type(1.0) - normalizedSelf.w * normalizedSelf.w); //var s = Math.sqrt(1 - a[3] * a[3]);
                angle = TypedAngle<Type>(Type(2.0) * std::acos(normalizedSelf.w), AngleUnit::Radians);
                axis = (s < Type(0.995)) ? Vector3(normalizedSelf.x, normalizedSelf.y, normalizedSelf.z) :
                    Vector3(normalizedSelf.x / s, normalizedSelf.y / s, normalizedSelf.z / s);
            }
 
            inline static TypedQuaternion FromAngleAxis(const TypedAngle<Type> angle, const TypedVector3<Type>& axis)
            {
                const Type s = std::sin(angle.AsRadians() * Type(0.5));
                const Type c = std::cos(angle.AsRadians() * Type(0.5));
                return TypedQuaternion(axis[0] * s, axis[1] * s, axis[2] * s, c);
            }
 
            inline static TypedQuaternion FromForward(const TypedVector3<Type>& forward, const TypedVector3<Type>* up = nullptr)
            {
                const TypedVector3<Type> unitForward = forward.GetNormalized();
                const TypedVector3<Type> unitUp = ((nullptr == up) ? TypedVector3<Type>(Type(0.0), Type(1.0), Type(0.0)) : up->GetNormalized());
 
                const Type dot = Vector3DotProduct(&unitUp, &unitForward);
                Vector3 axis(SkipInitialization::kSkipInitialization);
                Vector3CrossProduct(&axis, &unitUp, &unitForward);
                return TypedQuaternion(axis, dot + Type(1.0));
            }
 
            inline static TypedQuaternion FromEuler(const TypedVector3<Type>& euler, const tbMath::AngleUnit& angleUnits)
            {
                const Type halfHeading = TypedAngle<Type>(euler.x * Type(0.5), angleUnits).AsRadians();
                const Type halfPitch = TypedAngle<Type>(euler.y * Type(0.5), angleUnits).AsRadians();
                const Type halfRoll = TypedAngle<Type>(euler.z * Type(0.5), angleUnits).AsRadians();
 
                const Type c1 = std::cos(halfHeading);
                const Type s1 = std::sin(halfHeading);
                const Type c2 = std::cos(halfPitch);
                const Type s2 = std::sin(halfPitch);
                const Type c3 = std::cos(halfRoll);
                const Type s3 = std::sin(halfRoll);
                const Type c1c2 = c1*c2;
                const Type s1s2 = s1*s2;
                return TypedQuaternion(
                    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.hpp
            static TypedQuaternion FromMatrix(const TypedMatrix3<Type>& matrix);
            static TypedQuaternion FromMatrix(const TypedMatrix4<Type>& matrix);
 
        };
 
//--------------------------------------------------------------------------------------------------------------------//
//--------------------------------------------------------------------------------------------------------------------//
//--------------------------------------------------------------------------------------------------------------------//
 
        template<typename Type> TypedQuaternion<Type>* QuaternionAdd(TypedQuaternion<Type>* result,
            const TypedQuaternion<Type>* left, const TypedQuaternion<Type>* 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;
        }
 
        template<typename Type> TypedQuaternion<Type>* QuaternionSubtract(TypedQuaternion<Type>* result,
            const TypedQuaternion<Type>* left, const TypedQuaternion<Type>* 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;
        }
 
        template<typename Type> TypedQuaternion<Type>* QuaternionMultiply(TypedQuaternion<Type>* result,
            const TypedQuaternion<Type>* left, const TypedQuaternion<Type>* 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 TypedQuaternion<Type>& a(*left);
            const TypedQuaternion<Type>& 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;
        }
 
        template<typename Type> TypedQuaternion<Type>* QuaternionScale(TypedQuaternion<Type>* result,
            const TypedQuaternion<Type>* input, const Type 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;
        }
 
        template<typename Type> TypedQuaternion<Type>* QuaternionScaleDivide(TypedQuaternion<Type>* result,
            const TypedQuaternion<Type>* input, const Type 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;
        }
 
        template<typename Type> TypedQuaternion<Type>* QuaternionNegate(TypedQuaternion<Type>* result, const TypedQuaternion<Type>* 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;
        }
 
        template<typename Type> Type QuaternionMagnitude(const TypedQuaternion<Type>* input)
        {
            return std::sqrt((input->x * input->x) + (input->y * input->y) + (input->z * input->z) + (input->w * input->w));
        }
 
        template<typename Type> Type QuaternionMagnitudeSquared(const TypedQuaternion<Type>* input)
        {
            return (input->x * input->x) + (input->y * input->y) + (input->z * input->z) + (input->w * input->w);
        }
 
        template<typename Type> TypedQuaternion<Type>* QuaternionNormalize(TypedQuaternion<Type>* result, const TypedQuaternion<Type>* 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 Type magnitude = QuaternionMagnitude(input);
            if (true == IsZero(magnitude))
            {
                result->x = Type(0.0);
                result->y = Type(0.0);
                result->z = Type(0.0);
                result->w = Type(1.0);
            }
            else
            {
                result->x = input->x / magnitude;
                result->y = input->y / magnitude;
                result->z = input->z / magnitude;
                result->w = input->w / magnitude;
            }
            return result;
        }
 
        template<typename Type> TypedQuaternion<Type>* QuaternionNormalizeMagnitude(TypedQuaternion<Type>* result,
            const TypedQuaternion<Type>* input, Type& 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;
        }
 
        template<typename Type> TypedQuaternion<Type>* QuaternionInverse(TypedQuaternion<Type>* result, const TypedQuaternion<Type>* 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;
        }
 
        template<typename Type> Type QuaternionDotProduct(const TypedQuaternion<Type>* leftSide, const TypedQuaternion<Type>* 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
        template<typename Type> TypedQuaternion<Type>* QuaternionLerp(TypedQuaternion<Type>* result,
            const TypedQuaternion<Type>* start, const TypedQuaternion<Type>* final, const Type 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.")
 
            
            //QuaternionSubtract(result, final, start); // b - a
            //QuaternionScale(result, result, time); //(b - a) * t
            //QuaternionAdd(result, start, result); //result = a + (b - a) * t;
            //return QuaternionNormalize(result, result);
 
            //result = a * (1 - t) + b * t; (this fixes an f32 numerical bug shakesoda fought)
            TypedQuaternion<Type> resultB;
            QuaternionScale(result, start, Type(1.0) - time); //a * (1 - t)
            QuaternionScale(&resultB, final, time); //b * t
            QuaternionAdd(result, result, &resultB); // +
            return QuaternionNormalize(result, result);
        }
 
        //Spherical Interpolation
        template<typename Type> TypedQuaternion<Type> QuaternionSlerp(const TypedQuaternion<Type>& start,
            const TypedQuaternion<Type>& final, const Type time)
        {
 
            //TypedQuaternion<Type> z = final;
 
            //Type cosTheta = QuaternionDotProduct(&start, &final);
 
            //if (cosTheta < Type(0.0))
            //{
            //  z = -final;
            //  cosTheta = -cosTheta;
            //}
 
            //if (cosTheta > Type(1.0) - Type(0.00001))
            //{
            //  // Linear interpolation
            //  return TypedQuaternion<Type>(
            //      tbMath::Interpolation::Linear(start.x, z.x, time),
            //      tbMath::Interpolation::Linear(start.y, z.y, time),
            //      tbMath::Interpolation::Linear(start.z, z.z, time),
            //      tbMath::Interpolation::Linear(start.w, z.w, time));
            //}
            //else
            //{
            //  // Essential Mathematics, page 467
            //  Type angle = acos(cosTheta);
            //  return (sin((static_cast<Type>(1.0) - time) * angle) * start + sin(time * angle) * z) / sin(angle);
            //}
 
 
 
 
 
            //a = start, b = final
            Type dot = QuaternionDotProduct(&start, &final);
 
            TypedQuaternion<Type> a(start);
            if (dot < Type(0.0))
            {
                QuaternionNegate(&a, &start);
                dot = -dot;
            }
 
            if (dot > Type(0.995))
            {   //Too close to bother with.
                TypedQuaternion<Type> result;
                QuaternionLerp(&result, &start, &final, time);
                return result;
            }
 
            //If inputs are already normalized, this winds up doing nothing.
            dot = tbMath::Clamp(dot, Type(-1.0), Type(1.0));
 
            TypedQuaternion<Type> temporary; //= b - a * d;
            QuaternionScale(&temporary, &a, dot); //a * dot
            QuaternionSubtract(&temporary, &final, &temporary); //b - (a * dot)
            temporary.Normalize();
 
            const Type theta = std::acos(dot) * time;
            QuaternionScale(&temporary, &temporary, sinf(theta));
            QuaternionScale(&a, &a, std::cos(theta));
 
            //return a * Math.cos(theta) + c * Math.sin(theta);
            TypedQuaternion<Type> result(SkipInitialization::kSkipInitialization);
            QuaternionAdd(&result, &a, &temporary);
            return result;
        }
 
        template<typename Type> TypedVector3<Type> MultiplyVector3Quaternion(const TypedVector3<Type>& left,
            const TypedQuaternion<Type>& right)
        {
            TypedVector3<Type> uv(SkipInitialization::kSkipInitialization);
            TypedVector3<Type> uuv(SkipInitialization::kSkipInitialization);
            TypedVector3<Type> quat3(right[0], right[1], right[2]);
 
            Vector3CrossProduct(&uv, &quat3, &left);
            Vector3CrossProduct(&uuv, &quat3, &uv);
            return left + ((uv * right.w) + uuv) * Type(2.0);
        }
 
        template<typename Type> std::ostream& operator<<(std::ostream& output, const tbMath::TypedQuaternion<Type>& data)
        {
            output << "{ " << data.x << ", " << data.y << ", " << data.z << ", " << data.w << " }";
            return output;
        }
 
    }; /* namespace Math */
}; /* namespace TurtleBrains */
 
namespace tbMath = TurtleBrains::Math;
 
#endif /* TurtleBrains_Quaternion_hpp */