tb_matrix.h

#ifndef _TurtleBrains_Matrix_h_
#define _TurtleBrains_Matrix_h_

#include "tb_vector.h"
#include "../core/tb_error.h"
#include "../core/tb_defines.h"

#include <cmath>
#include <cstring> //memcpy / memset

//
// TODO: TIM: TurtleBrains: NOW:
//   Matrix identity constant should be removed and replaced with Identity() constexpr static member fucntion.
//   readup on constexpr
//

namespace TurtleBrains
{
    namespace Math
    {
        //Forward Declarations used in Matrix4 class.
        class Matrix4;
        Vector3* Vector3TransformCoord(Vector3* result, const Vector3* inputVector, const Matrix4* inputMatrix);
        Vector3* Vector3TransformNormal(Vector3* result, const Vector3* inputVector, const Matrix4* inputMatrix);

        class Matrix3
        {
        public:
            static Matrix3 Zero(void)
            {
                return Matrix3(
                    0.0f, 0.0f, 0.0f,
                    0.0f, 0.0f, 0.0f,
                    0.0f, 0.0f, 0.0f);
            }

            static Matrix3 Identity(void)
            {
                return Matrix3(
                    1.0f, 0.0f, 0.0f,
                    0.0f, 1.0f, 0.0f,
                    0.0f, 0.0f, 1.0f);
            }

            union
            {
                float mComponents[9];

#if defined(tb_visual_cpp)
#pragma warning(push)
#pragma warning(disable: 4201)
                struct {    //May be deprecating!?
                    float m_f11; float m_f21; float m_f31;
                    float m_f12; float m_f22; float m_f32;
                    float m_f13; float m_f23; float m_f33;
                };
#pragma warning(pop)
#else
                struct {    //May be deprecating!?
                    float m_f11; float m_f21; float m_f31;
                    float m_f12; float m_f22; float m_f32;
                    float m_f13; float m_f23; float m_f33;
                };
#endif
            };

            inline explicit Matrix3(    float f11 = 0.0f, float f21 = 0.0f, float f31 = 0.0f,
                                        float f12 = 0.0f, float f22 = 0.0f, float f32 = 0.0f,
                                        float f13 = 0.0f, float f23 = 0.0f, float f33 = 0.0f)
            {
                //m_f11 = f11; m_f21 = f21; m_f31 = f31;
                //m_f12 = f12; m_f22 = f22; m_f32 = f32;
                //m_f13 = f13; m_f23 = f23; m_f33 = f33;

                mComponents[0] = f11; mComponents[1] = f21; mComponents[2] = f31;
                mComponents[3] = f12; mComponents[4] = f22; mComponents[5] = f32;
                mComponents[6] = f13; mComponents[7] = f23; mComponents[8] = f33;
            }

            inline explicit Matrix3(const float * const componentArray)
            {
                memcpy(mComponents, componentArray, sizeof(float) * 9);
            }

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

            inline Matrix3(const Matrix3& other)
            {
                memcpy(mComponents, other.mComponents, sizeof(float) * 9);
            }

            inline Matrix3& operator=(const Matrix3& other)
            {
                if (this != &other)
                {
                    memcpy(mComponents, other.mComponents, sizeof(float) * 9);
                }
                return (*this);
            }

            inline bool operator==(const Matrix3& other) const
            {
                for (size_t index(0); index < 9; ++index)
                {
                    if (false == IsEqual(mComponents[index], other[index]))
                    {
                        return false;
                    }
                }

                return true;
            }

            inline bool operator!=(const Matrix3& other) const
            {
                for (size_t index(0); index < 9; ++index)
                {
                    if (false == IsEqual(mComponents[index], other[index]))
                    {
                        return true;
                    }
                }

                return false;
            }

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

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

            inline const float& operator()(const size_t& column, const size_t& row) const { return mComponents[column + (row * 3)]; }

            inline float& operator()(const size_t& column, const size_t& row) { return mComponents[column + (row * 3)]; }

            inline const float& operator()(int column, int row) const { return mComponents[column + (row * 3)]; }

            inline float& operator()(int column, int row) { return mComponents[column + (row * 3)]; }

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

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

            inline Vector3* GetBasis(const size_t& basisIndex) { return (Vector3*)&mComponents[basisIndex * 3]; }

            inline const Vector3* GetBasis(const size_t& basisIndex) const { return (const Vector3*)&mComponents[basisIndex * 3]; }

            inline void SetBasis(const size_t& basisIndex, const Vector3 &basis) { *((Vector3*)&mComponents[basisIndex * 3]) = basis; }

            inline void SetBasis(const size_t& basisIndex, float basisX, float basisY, float basisZ)
            {
                mComponents[(basisIndex * 3) + 0] = basisX;
                mComponents[(basisIndex * 3) + 1] = basisY;
                mComponents[(basisIndex * 3) + 2] = basisZ;
            }


#if defined(tb_with_math_operators)
            inline Matrix3 operator*(float scalar) const
            {
                return Matrix3(
                    (*this)(0, 0) * scalar, (*this)(1, 0) * scalar, (*this)(2, 0) * scalar,
                    (*this)(0, 1) * scalar, (*this)(1, 1) * scalar, (*this)(2, 1) * scalar, 
                    (*this)(0, 2) * scalar, (*this)(1, 2) * scalar, (*this)(2, 2) * scalar);
            }
#endif /* tb_with_math_operators */

        };


        class Matrix4;
        Matrix4* MatrixComputeInverse(Matrix4* result, const Matrix4* input);

        class Matrix4
        {
        public:

            static Matrix4 Zero(void)
            { 
                return Matrix4(
                    0.0f, 0.0f, 0.0f, 0.0f,
                    0.0f, 0.0f, 0.0f, 0.0f,
                    0.0f, 0.0f, 0.0f, 0.0f,
                    0.0f, 0.0f, 0.0f, 0.0f);
            }
            
            static Matrix4 Identity(void)
            {
                return Matrix4(
                    1.0f, 0.0f, 0.0f, 0.0f,
                    0.0f, 1.0f, 0.0f, 0.0f,
                    0.0f, 0.0f, 1.0f, 0.0f,
                    0.0f, 0.0f, 0.0f, 1.0f);
            }

            union
            {
                float mComponents[16];

#if defined(tb_visual_cpp)
#pragma warning(push)
#pragma warning(disable: 4201)
                struct {    //May be deprecating!?
                    float m_f11; float m_f21; float m_f31; float m_f41;
                    float m_f12; float m_f22; float m_f32; float m_f42;
                    float m_f13; float m_f23; float m_f33; float m_f43;
                    float m_f14; float m_f24; float m_f34; float m_f44;
                };
#pragma warning(pop)
#else
                struct {    //May be deprecating!?
                    float m_f11; float m_f21; float m_f31; float m_f41;
                    float m_f12; float m_f22; float m_f32; float m_f42;
                    float m_f13; float m_f23; float m_f33; float m_f43;
                    float m_f14; float m_f24; float m_f34; float m_f44;
                };
#endif
            };

            inline explicit Matrix4(    float f11 = 0.0f, float f21 = 0.0f, float f31 = 0.0f, float f41 = 0.0f,
                            float f12 = 0.0f, float f22 = 0.0f, float f32 = 0.0f, float f42 = 0.0f,
                            float f13 = 0.0f, float f23 = 0.0f, float f33 = 0.0f, float f43 = 0.0f,
                            float f14 = 0.0f, float f24 = 0.0f, float f34 = 0.0f, float f44 = 0.0f)
            {
                //m_f11 = f11; m_f21 = f21; m_f31 = f31; m_f41 = f41;
                //m_f12 = f12; m_f22 = f22; m_f32 = f32; m_f42 = f42;
                //m_f13 = f13; m_f23 = f23; m_f33 = f33; m_f43 = f43;
                //m_f14 = f14; m_f24 = f24; m_f34 = f34; m_f44 = f44;

                mComponents[0] = f11; mComponents[1] = f21; mComponents[2] = f31; mComponents[3] = f41;
                mComponents[4] = f12; mComponents[5] = f22; mComponents[6] = f32; mComponents[7] = f42;
                mComponents[8] = f13; mComponents[9] = f23; mComponents[10]= f33; mComponents[11]= f43;
                mComponents[12]= f14; mComponents[13]= f24; mComponents[14]= f34; mComponents[15]= f44;
            }

            inline explicit Matrix4(const float * const componentArray)
            {
                memcpy(mComponents, componentArray, sizeof(float) << 4);  //Same as (sizeof(float) * 16)
            }

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

            inline Matrix4(const Matrix4& other)
            {
                memcpy(mComponents, other.mComponents, sizeof(float) << 4);  //Same as (sizeof(float) * 16)
            }

            inline Matrix4& operator=(const Matrix4& other)
            {
                if (this != &other)
                {
                    memcpy(mComponents, other.mComponents, sizeof(float) << 4);  //Same as (sizeof(float) * 16)
                }
                return (*this);
            }

            inline bool operator==(const Matrix4& other) const
            {
                for (size_t index(0); index < 16; ++index)
                {
                    if (false == IsEqual(mComponents[index], other[index]))
                    {
                        return false;
                    }
                }

                return true;
            }

            inline bool operator!=(const Matrix4& other) const
            {
                for (size_t index(0); index < 16; ++index)
                {
                    if (false == IsEqual(mComponents[index], other[index]))
                    {
                        return true;
                    }
                }

                return false;
            }

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

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

            inline const float& operator()(const size_t& column, const size_t& row) const { return mComponents[column + (row << 2)]; }

            inline float& operator()(const size_t& column, const size_t& row) { return mComponents[column + (row << 2)]; }

            inline const float& operator()(int column, int row) const { return mComponents[column + (row << 2)]; }

            inline float& operator()(int column, int row) { return mComponents[column + (row << 2)]; }


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

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


            inline Vector3* GetBasis(const size_t& basisIndex) { return (Vector3*)&mComponents[basisIndex << 2]; }

            inline const Vector3* GetBasis(const size_t& basisIndex) const { return (const Vector3*)&mComponents[basisIndex<<2]; }

            inline void SetBasis(const size_t& basisIndex, const Vector3 &basis) { *((Vector3*)&mComponents[basisIndex<<2]) = basis; }

            inline void SetBasis(const size_t& basisIndex, float basisX, float basisY, float basisZ) 
            { 
                mComponents[(basisIndex<<2) + 0] = basisX;
                mComponents[(basisIndex<<2) + 1] = basisY;
                mComponents[(basisIndex<<2) + 2] = basisZ; 
            }

#if defined(tb_with_math_operators)
            inline Vector3 GetPosition(void) const
            {
                return Vector3((*this)(0, 3), (*this)(1, 3), (*this)(2, 3));
            }

            inline void SetPosition(const Vector3& position)
            {
                (*this)(0, 3) = position.x;
                (*this)(1, 3) = position.y;
                (*this)(2, 3) = position.z;
            }

            inline void SetPosition(float x, float y, float z)
            {
                (*this)(0, 3) = x;
                (*this)(1, 3) = y;
                (*this)(2, 3) = z;
            }

            inline Matrix4 GetTransposed(void) const
            {
                //result->m_f11 = input->m_f11; result->m_f12 = input->m_f21;   result->m_f13 = input->m_f31;   result->m_f14 = input->m_f41;
                //result->m_f21 = input->m_f12; result->m_f22 = input->m_f22;   result->m_f23 = input->m_f32;   result->m_f24 = input->m_f42;
                //result->m_f31 = input->m_f13; result->m_f32 = input->m_f23;   result->m_f33 = input->m_f33;   result->m_f34 = input->m_f43;
                //result->m_f41 = input->m_f14; result->m_f42 = input->m_f24;   result->m_f43 = input->m_f34;   result->m_f44 = input->m_f44;

                Matrix4 temp;
                const Matrix4& self(*this);
                temp(0, 0) = self(0, 0);    temp(0, 1) = self(1, 0);    temp(0, 2) = self(2, 0);    temp(0, 3) = self(3, 0);
                temp(1, 0) = self(0, 1);    temp(1, 1) = self(1, 1);    temp(1, 2) = self(2, 1);    temp(1, 3) = self(3, 1);
                temp(2, 0) = self(0, 2);    temp(2, 1) = self(1, 2);    temp(2, 2) = self(2, 2);    temp(2, 3) = self(3, 2);
                temp(3, 0) = self(0, 3);    temp(3, 1) = self(1, 3);    temp(3, 2) = self(2, 3);    temp(3, 3) = self(3, 3);
                return temp;
            }

            inline Matrix4 ComputeInverse(void) const
            {
                Matrix4 temp;
                MatrixComputeInverse(&temp, this);
                return temp;
            }

            inline Vector3 TransformCoordinate(const Vector3& inputVector) const
            {
                Vector3 result;
                Vector3TransformCoord(&result, &inputVector, this);
                return result;
            }

            inline Vector3 TransformNormal(const Vector3& inputVector) const
            {
                Vector3 result;
                Vector3TransformNormal(&result, &inputVector, this);
                return result;
            }

#endif /* tb_with_math_operators */
        };


        inline Matrix4* MatrixCreateIdentity(Matrix4* result)
        {
            *result = Matrix4::Identity();
            return result;
        }
        
        inline Matrix4* MatrixCreateFromForward(Matrix4* result, const Vector3* forward, const Vector3* up = nullptr)
        {
            tb_error("Not yet implemented / tested.");
            MatrixCreateIdentity(result);

            Vector3 realUp((nullptr == up) ? Vector3(0.0f, 1.0f, 0.0f) : *up);
            Vector3 right, finalUp;
            Vector3CrossProduct(&right, &realUp, forward);
            Vector3CrossProduct(&finalUp, &right, forward);
            //TODO: TIM: Implementation: This would be useful, implement it for future usage!
            return result;
        }

        inline Matrix4* MatrixCreateTranslation(Matrix4* result, const Vector3* translation)
        {   //Changed September 19, 2010
            MatrixCreateIdentity(result);
            result->m_f14 = translation->x;//matR->m_f41 = vT->x;
            result->m_f24 = translation->y;//matR->m_f42 = vT->y;
            result->m_f34 = translation->z;//matR->m_f43 = vT->z;
            return result;
        }

        inline Matrix4* MatrixCreateTranslation(Matrix4* result, const float translationX, const float translationY, const float translationZ)
        {   //Changed September 19, 2010
            MatrixCreateIdentity(result);
            result->m_f14 = translationX;//matR->m_f41 = fX;
            result->m_f24 = translationY;//matR->m_f42 = fY;
            result->m_f34 = translationZ;//matR->m_f43 = fZ;
            return result;
        }

        inline Matrix4* MatrixCreateScale(Matrix4* result, const Vector3* scale)
        {
            memset(result->mComponents, 0, sizeof(float) << 4);
            result->m_f11 = scale->x;
            result->m_f22 = scale->y;
            result->m_f33 = scale->z;
            result->m_f44 = 1.0f;
            return result;
        }

        inline Matrix4* MatrixCreateScale(Matrix4* result, const float scaleX, const float scaleY, const float scaleZ)
        {
            memset(result->mComponents, 0, sizeof(float) << 4);
            result->m_f11 = scaleX;
            result->m_f22 = scaleY;
            result->m_f33 = scaleZ;
            result->m_f44 = 1.0f;
            return result;
        }
        
        inline Matrix4* MatrixCreateRotationX(Matrix4* result, const float rotationInDegrees)
        {
            const float rotationInRadians(tbMath::Convert::DegreesToRadians(rotationInDegrees));

            MatrixCreateIdentity(result);
            result->m_f22 = cos(rotationInRadians);
            result->m_f32 = -sin(rotationInRadians);
            result->m_f23 = sin(rotationInRadians);
            result->m_f33 = cos(rotationInRadians);
            return result;
        }

        inline Matrix4* MatrixCreateRotationY(Matrix4* result, const float rotationInDegrees)
        {
            const float rotationInRadians(tbMath::Convert::DegreesToRadians(rotationInDegrees));

            MatrixCreateIdentity(result);
            result->m_f11 = cos(rotationInRadians);
            result->m_f31 = sin(rotationInRadians);
            result->m_f13 = -sin(rotationInRadians);
            result->m_f33 = cos(rotationInRadians);
            return result;
        }

        inline Matrix4* MatrixCreateRotationZ(Matrix4* result, const float rotationInDegrees)
        {
            const float rotationInRadians(tbMath::Convert::DegreesToRadians(rotationInDegrees));

            MatrixCreateIdentity(result);
            result->m_f11 = cos(rotationInRadians);
            result->m_f21 = -sin(rotationInRadians);
            result->m_f12 = sin(rotationInRadians);
            result->m_f22 = cos(rotationInRadians);
            return result;
        }

        inline Matrix4* MatrixCreateRotationA(Matrix4* result, const Vector3* rotationAxis, const float rotationInDegrees)
        {
            const float rotationInRadians(tbMath::Convert::DegreesToRadians(rotationInDegrees));

            // Rotations.
            const float s = sinf(rotationInRadians);
            const float c = cosf(rotationInRadians);
            const float t = 1.0f - c;

            float x = rotationAxis->x;
            float y = rotationAxis->y;
            float z = rotationAxis->z;

            const float tx = t * rotationAxis->x;
            const float ty = t * rotationAxis->y;
            const float tz = t * rotationAxis->z;
            const float sx = s * rotationAxis->x;
            const float sy = s * rotationAxis->y;
            const float sz = s * rotationAxis->z;

            result->mComponents[0] = tx * x + c;
            result->mComponents[1] = tx * y + sz;
            result->mComponents[2] = tx * z - sy;
            result->mComponents[3] = 0.0f;

            result->mComponents[4]  = tx * y - sz;
            result->mComponents[5]  = ty * y + c;
            result->mComponents[6]  = ty * z + sx;
            result->mComponents[7]  = 0.0f;

            result->mComponents[8]  = tx * z + sy;
            result->mComponents[9]  = ty * z - sx;
            result->mComponents[10] = tz * z + c;
            result->mComponents[11] = 0.0f;

            result->mComponents[12] = 0.0f;
            result->mComponents[13] = 0.0f; 
            result->mComponents[14] = 0.0f;
            result->mComponents[15] = 1.0f; 

            return result;
        }

        inline Matrix4* MatrixCreatePerspectiveRH(Matrix4* result, const float fieldOfView, const float aspectRatio, 
            const float nearPlane, const float farPlane)
        {
            memset(result->mComponents, 0, sizeof(float) << 4);

            const float f = 1.0f / tanf(fieldOfView / 2.0f);
            result->m_f11 = f / aspectRatio;
            result->m_f22 = f;
            result->m_f33 = (farPlane) / (nearPlane - farPlane);
            result->m_f43 = -1.0f;
            result->m_f34 = (nearPlane * farPlane) / (nearPlane - farPlane);

            return result;
        }
        
        inline Matrix4* MatrixCreatePerspectiveLH(Matrix4* result, const float fieldOfView, const float aspectRatio, 
            const float nearPlane, const float farPlane)
        {
            memset(result->mComponents, 0, sizeof(float) << 4);

            const float f = 1.0f / tanf(fieldOfView / 2.0f);
            result->m_f11 = f / aspectRatio;
            result->m_f22 = f;
            result->m_f33 = (farPlane) / (farPlane - nearPlane);
            result->m_f43 = 1.0f;
            result->m_f34 = -(nearPlane * farPlane) / (farPlane - nearPlane);

            return result;
        }

        inline Matrix4* MatrixCreateOrthoRH(Matrix4* result, const float width, const float height, 
            const float nearPlane, const float farPlane)
        {
            memset(result->mComponents, 0, sizeof(float) << 4);

            result->m_f11 = 2.0f / width;
            result->m_f22 = 2.0f / height;
            result->m_f33 = 1.0f / (nearPlane - farPlane);
            result->m_f34 = nearPlane / (nearPlane - farPlane);
            result->m_f44 = 1.0f;

            return result;
        }

        inline Matrix4* MatrixCreateOrthoLH(Matrix4* result, const float width, const float height,
            const float nearPlane, const float farPlane)
        {
            memset(result->mComponents, 0, sizeof(float) << 4);

            //matR->m_f11 = 2.0f / fWidth;
            //matR->m_f22 = 2.0f / fHeight;
            //matR->m_f33 = 1 / (fFar - fNear);
            //matR->m_f34 = fNear / (fNear - fFar);
            //matR->m_f44 = 1;

            result->m_f11 = 2.0f / width;
            result->m_f22 = 2.0f / height;
            result->m_f33 = 1.0f / (farPlane - nearPlane);
            result->m_f34 = nearPlane / (nearPlane - farPlane);
            result->m_f44 = 1.0f;

            return result;
        }

        inline Matrix4* MatrixAdd(Matrix4 *result, const Matrix4 *leftSide, const Matrix4 *rightSide)
        {
            float *r = result->mComponents;
            const float *m1 = leftSide->mComponents;
            const float *m2 = rightSide->mComponents;

            r[0] = m1[0] + m2[0];           r[1] = m1[1] + m2[1];           r[2] = m1[2] + m2[2];           r[3] = m1[3] + m2[3];
            r[4] = m1[4] + m2[4];           r[5] = m1[5] + m2[5];           r[6] = m1[6] + m2[6];           r[7] = m1[7] + m2[7];
            r[8] = m1[8] + m2[8];           r[9] = m1[9] + m2[9];           r[10] = m1[10] + m2[10];        r[11] = m1[11] + m2[11];
            r[12] = m1[12] + m2[12];        r[13] = m1[13] + m2[13];        r[14] = m1[14] + m2[14];        r[15] = m1[15] + m2[15];
            return result;
        }

        inline Matrix4* MatrixSubtract(Matrix4* result, const Matrix4* leftSide, const Matrix4* rightSide)
        {
            float *r = result->mComponents;
            const float *m1 = leftSide->mComponents;
            const float *m2 = rightSide->mComponents;

            r[0] = m1[0] - m2[0];           r[1] = m1[1] - m2[1];           r[2] = m1[2] - m2[2];           r[3] = m1[3] - m2[3];
            r[4] = m1[4] - m2[4];           r[5] = m1[5] - m2[5];           r[6] = m1[6] - m2[6];           r[7] = m1[7] - m2[7];
            r[8] = m1[8] - m2[8];           r[9] = m1[9] - m2[9];           r[10] = m1[10] - m2[10];        r[11] = m1[11] - m2[11];
            r[12] = m1[12] - m2[12];        r[13] = m1[13] - m2[13];        r[14] = m1[14] - m2[14];        r[15] = m1[15] - m2[15];
            return result;
        }
        
        inline Matrix4* MatrixTranspose(Matrix4* result, const Matrix4* input)
        {
            tb_error_if(result == input, "tbExternalError: Invalid parameter; expected result to be different than input");
            result->m_f11 = input->m_f11;   result->m_f12 = input->m_f21;   result->m_f13 = input->m_f31;   result->m_f14 = input->m_f41;
            result->m_f21 = input->m_f12;   result->m_f22 = input->m_f22;   result->m_f23 = input->m_f32;   result->m_f24 = input->m_f42;
            result->m_f31 = input->m_f13;   result->m_f32 = input->m_f23;   result->m_f33 = input->m_f33;   result->m_f34 = input->m_f43;
            result->m_f41 = input->m_f14;   result->m_f42 = input->m_f24;   result->m_f43 = input->m_f34;   result->m_f44 = input->m_f44;
            return result;
        }
        
        inline Matrix4* MatrixMultiply(Matrix4* result, const Matrix4* leftSide, const Matrix4* rightSide)
        {
            tb_error_if(result == leftSide || result == rightSide, "tbExternalError: Invalid parameter; expected result to be different than leftSide and rightSide");
            result->m_f11 = (leftSide->m_f11 * rightSide->m_f11) + (leftSide->m_f21 * rightSide->m_f12) + (leftSide->m_f31 * rightSide->m_f13) + (leftSide->m_f41 * rightSide->m_f14);
            result->m_f21 = (leftSide->m_f11 * rightSide->m_f21) + (leftSide->m_f21 * rightSide->m_f22) + (leftSide->m_f31 * rightSide->m_f23) + (leftSide->m_f41 * rightSide->m_f24);
            result->m_f31 = (leftSide->m_f11 * rightSide->m_f31) + (leftSide->m_f21 * rightSide->m_f32) + (leftSide->m_f31 * rightSide->m_f33) + (leftSide->m_f41 * rightSide->m_f34);
            result->m_f41 = (leftSide->m_f11 * rightSide->m_f41) + (leftSide->m_f21 * rightSide->m_f42) + (leftSide->m_f31 * rightSide->m_f43) + (leftSide->m_f41 * rightSide->m_f44);
            result->m_f12 = (leftSide->m_f12 * rightSide->m_f11) + (leftSide->m_f22 * rightSide->m_f12) + (leftSide->m_f32 * rightSide->m_f13) + (leftSide->m_f42 * rightSide->m_f14);
            result->m_f22 = (leftSide->m_f12 * rightSide->m_f21) + (leftSide->m_f22 * rightSide->m_f22) + (leftSide->m_f32 * rightSide->m_f23) + (leftSide->m_f42 * rightSide->m_f24);
            result->m_f32 = (leftSide->m_f12 * rightSide->m_f31) + (leftSide->m_f22 * rightSide->m_f32) + (leftSide->m_f32 * rightSide->m_f33) + (leftSide->m_f42 * rightSide->m_f34);
            result->m_f42 = (leftSide->m_f12 * rightSide->m_f41) + (leftSide->m_f22 * rightSide->m_f42) + (leftSide->m_f32 * rightSide->m_f43) + (leftSide->m_f42 * rightSide->m_f44);
            result->m_f13 = (leftSide->m_f13 * rightSide->m_f11) + (leftSide->m_f23 * rightSide->m_f12) + (leftSide->m_f33 * rightSide->m_f13) + (leftSide->m_f43 * rightSide->m_f14);
            result->m_f23 = (leftSide->m_f13 * rightSide->m_f21) + (leftSide->m_f23 * rightSide->m_f22) + (leftSide->m_f33 * rightSide->m_f23) + (leftSide->m_f43 * rightSide->m_f24);
            result->m_f33 = (leftSide->m_f13 * rightSide->m_f31) + (leftSide->m_f23 * rightSide->m_f32) + (leftSide->m_f33 * rightSide->m_f33) + (leftSide->m_f43 * rightSide->m_f34);
            result->m_f43 = (leftSide->m_f13 * rightSide->m_f41) + (leftSide->m_f23 * rightSide->m_f42) + (leftSide->m_f33 * rightSide->m_f43) + (leftSide->m_f43 * rightSide->m_f44);
            result->m_f14 = (leftSide->m_f14 * rightSide->m_f11) + (leftSide->m_f24 * rightSide->m_f12) + (leftSide->m_f34 * rightSide->m_f13) + (leftSide->m_f44 * rightSide->m_f14);
            result->m_f24 = (leftSide->m_f14 * rightSide->m_f21) + (leftSide->m_f24 * rightSide->m_f22) + (leftSide->m_f34 * rightSide->m_f23) + (leftSide->m_f44 * rightSide->m_f24);
            result->m_f34 = (leftSide->m_f14 * rightSide->m_f31) + (leftSide->m_f24 * rightSide->m_f32) + (leftSide->m_f34 * rightSide->m_f33) + (leftSide->m_f44 * rightSide->m_f34);
            result->m_f44 = (leftSide->m_f14 * rightSide->m_f41) + (leftSide->m_f24 * rightSide->m_f42) + (leftSide->m_f34 * rightSide->m_f43) + (leftSide->m_f44 * rightSide->m_f44);
            return result;
        }

        inline Vector3* Vector3MatrixMultiply(Vector3* result, const Vector3* inputVector, const Matrix3* inputMatrix)
        {
            tb_error_if(result == inputVector, "tbExternalError: Invalid parameter; expected result to be different than inputVector.");
            result->x = (inputVector->x * inputMatrix->m_f11) + (inputVector->y * inputMatrix->m_f12) + (inputVector->z * inputMatrix->m_f13);
            result->y = (inputVector->x * inputMatrix->m_f21) + (inputVector->y * inputMatrix->m_f22) + (inputVector->z * inputMatrix->m_f23);
            result->z = (inputVector->x * inputMatrix->m_f31) + (inputVector->y * inputMatrix->m_f32) + (inputVector->z * inputMatrix->m_f33);
            return result;
        }

        inline Vector4* Vector4MatrixMultiply(Vector4* result, const Vector4* inputVector, const Matrix4* inputMatrix)
        {
            tb_error_if(result == inputVector , "tbExternalError: Invalid parameter; expected result to be different than inputVector.");
            result->x = (inputVector->x * inputMatrix->m_f11) + (inputVector->y * inputMatrix->m_f12) + (inputVector->z * inputMatrix->m_f13) + (inputVector->w * inputMatrix->m_f14);
            result->y = (inputVector->x * inputMatrix->m_f21) + (inputVector->y * inputMatrix->m_f22) + (inputVector->z * inputMatrix->m_f23) + (inputVector->w * inputMatrix->m_f24);
            result->z = (inputVector->x * inputMatrix->m_f31) + (inputVector->y * inputMatrix->m_f32) + (inputVector->z * inputMatrix->m_f33) + (inputVector->w * inputMatrix->m_f34);
            result->w = (inputVector->x * inputMatrix->m_f41) + (inputVector->y * inputMatrix->m_f42) + (inputVector->z * inputMatrix->m_f43) + (inputVector->w * inputMatrix->m_f44);
            return result;
        }

        inline Vector3* Vector3TransformCoord(Vector3* result, const Vector3* inputVector, const Matrix4* inputMatrix)
        {
            tb_error_if(result == inputVector, "tbExternalError: Invalid parameter; expected result to be different than inputVector");
            result->x = (inputVector->x * inputMatrix->m_f11) + (inputVector->y * inputMatrix->m_f12) + (inputVector->z * inputMatrix->m_f13) + inputMatrix->m_f14;
            result->y = (inputVector->x * inputMatrix->m_f21) + (inputVector->y * inputMatrix->m_f22) + (inputVector->z * inputMatrix->m_f23) + inputMatrix->m_f24;
            result->z = (inputVector->x * inputMatrix->m_f31) + (inputVector->y * inputMatrix->m_f32) + (inputVector->z * inputMatrix->m_f33) + inputMatrix->m_f34;
            return result;
        }

        inline Vector3* Vector3TransformNormal(Vector3* result, const Vector3* inputVector, const Matrix4* inputMatrix)
        {
            tb_error_if(result == inputVector, "tbExternalError: Invalid parameter; expected result to be different than inputVector");
            result->x = (inputVector->x * inputMatrix->m_f11) + (inputVector->y * inputMatrix->m_f12) + (inputVector->z * inputMatrix->m_f13);
            result->y = (inputVector->x * inputMatrix->m_f21) + (inputVector->y * inputMatrix->m_f22) + (inputVector->z * inputMatrix->m_f23);
            result->z = (inputVector->x * inputMatrix->m_f31) + (inputVector->y * inputMatrix->m_f32) + (inputVector->z * inputMatrix->m_f33);
            return result;
        }

        inline float Matrix3x3Determinant(const float f11, const float f12, const float f13, const float f21, const float f22, const float f23, const float f31, const float f32, const float f33)
        {
            return f11 * (f22 * f33 - f32 * f23) - f12 * (f21 * f33 - f31 * f23) + f13 * (f21 * f32 - f31 * f22);
        }

        inline float MatrixDeterminant(const Matrix4* input)
        {
            return  input->m_f11 * Matrix3x3Determinant(input->m_f22, input->m_f32, input->m_f42, input->m_f23, input->m_f33, input->m_f43, input->m_f24, input->m_f34, input->m_f44) -
                    input->m_f21 * Matrix3x3Determinant(input->m_f12, input->m_f32, input->m_f42, input->m_f13, input->m_f33, input->m_f43, input->m_f14, input->m_f34, input->m_f44) +
                    input->m_f31 * Matrix3x3Determinant(input->m_f12, input->m_f22, input->m_f42, input->m_f13, input->m_f23, input->m_f43, input->m_f14, input->m_f24, input->m_f44) -
                    input->m_f41 * Matrix3x3Determinant(input->m_f12, input->m_f22, input->m_f32, input->m_f13, input->m_f23, input->m_f33, input->m_f14, input->m_f24, input->m_f34);
        }
        
        inline Matrix4* MatrixComputeInverse(Matrix4* result, const Matrix4* input)
        {
            tb_error_if(result == input, "tbExternalError: Invalid parameter, expected result to be different than input.");
            const float determinant = MatrixDeterminant(input);
            if (true == IsZero(determinant))
            {
                *result = *input;
                return result;
            }

            const float inverseDeterminant = 1.0f / determinant;

            result->m_f11 = (Matrix3x3Determinant (input->m_f22, input->m_f32, input->m_f42, input->m_f23, input->m_f33, input->m_f43, input->m_f24, input->m_f34, input->m_f44)) * inverseDeterminant;
            result->m_f21 = (-Matrix3x3Determinant(input->m_f21, input->m_f31, input->m_f41, input->m_f23, input->m_f33, input->m_f43, input->m_f24, input->m_f34, input->m_f44)) * inverseDeterminant;
            result->m_f31 = (Matrix3x3Determinant (input->m_f21, input->m_f31, input->m_f41, input->m_f22, input->m_f32, input->m_f42, input->m_f24, input->m_f34, input->m_f44)) * inverseDeterminant;
            result->m_f41 = (-Matrix3x3Determinant(input->m_f21, input->m_f31, input->m_f41, input->m_f22, input->m_f32, input->m_f42, input->m_f23, input->m_f33, input->m_f43)) * inverseDeterminant;
            result->m_f12 = (-Matrix3x3Determinant(input->m_f12, input->m_f32, input->m_f42, input->m_f13, input->m_f33, input->m_f43, input->m_f14, input->m_f34, input->m_f44)) * inverseDeterminant;
            result->m_f22 = (Matrix3x3Determinant (input->m_f11, input->m_f31, input->m_f41, input->m_f13, input->m_f33, input->m_f43, input->m_f14, input->m_f34, input->m_f44)) * inverseDeterminant;
            result->m_f32 = (-Matrix3x3Determinant(input->m_f11, input->m_f31, input->m_f41, input->m_f12, input->m_f32, input->m_f42, input->m_f14, input->m_f34, input->m_f44)) * inverseDeterminant;
            result->m_f42 = (Matrix3x3Determinant (input->m_f11, input->m_f31, input->m_f41, input->m_f12, input->m_f32, input->m_f42, input->m_f13, input->m_f33, input->m_f43)) * inverseDeterminant;
            result->m_f13 = (Matrix3x3Determinant (input->m_f12, input->m_f22, input->m_f42, input->m_f13, input->m_f23, input->m_f43, input->m_f14, input->m_f24, input->m_f44)) * inverseDeterminant;
            result->m_f23 = (-Matrix3x3Determinant(input->m_f11, input->m_f21, input->m_f41, input->m_f13, input->m_f23, input->m_f43, input->m_f14, input->m_f24, input->m_f44)) * inverseDeterminant;
            result->m_f33 = (Matrix3x3Determinant (input->m_f11, input->m_f21, input->m_f41, input->m_f12, input->m_f22, input->m_f42, input->m_f14, input->m_f24, input->m_f44)) * inverseDeterminant;
            result->m_f43 = (-Matrix3x3Determinant(input->m_f11, input->m_f21, input->m_f41, input->m_f12, input->m_f22, input->m_f42, input->m_f13, input->m_f23, input->m_f43)) * inverseDeterminant;
            result->m_f14 = (-Matrix3x3Determinant(input->m_f12, input->m_f22, input->m_f32, input->m_f13, input->m_f23, input->m_f33, input->m_f14, input->m_f24, input->m_f34)) * inverseDeterminant;
            result->m_f24 = (Matrix3x3Determinant (input->m_f11, input->m_f21, input->m_f31, input->m_f13, input->m_f23, input->m_f33, input->m_f14, input->m_f24, input->m_f34)) * inverseDeterminant;
            result->m_f34 = (-Matrix3x3Determinant(input->m_f11, input->m_f21, input->m_f31, input->m_f12, input->m_f22, input->m_f32, input->m_f14, input->m_f24, input->m_f34)) * inverseDeterminant;
            result->m_f44 = (Matrix3x3Determinant (input->m_f11, input->m_f21, input->m_f31, input->m_f12, input->m_f22, input->m_f32, input->m_f13, input->m_f24, input->m_f33)) * inverseDeterminant;

            return result;
        }

        namespace Unstable {
            inline Matrix4* FastInverse(Matrix4* result, const Matrix4* input)
            {
                (*result)(0, 0) = (*input)(0, 0);   //Diagonal is equal
                (*result)(1, 1) = (*input)(1, 1);
                (*result)(2, 2) = (*input)(2, 2);
                (*result)(3, 3) = (*input)(3, 3);

                (*result)(1, 0) = (*input)(0, 1);   //Rotation is transposed
                (*result)(2, 0) = (*input)(0, 2);
                (*result)(0, 1) = (*input)(1, 0);
                (*result)(2, 1) = (*input)(1, 2);
                (*result)(0, 2) = (*input)(2, 0);
                (*result)(1, 2) = (*input)(2, 1);

                (*result)(3, 0) = -(*input)(3, 0);  //Translation is negated
                (*result)(3, 1) = -(*input)(3, 1);
                (*result)(3, 2) = -(*input)(3, 2);

                (*result)(0, 3) = 0.0f;
                (*result)(1, 3) = 0.0f;
                (*result)(2, 3) = 0.0f;

                return result;
            }
        };

        namespace Unstable
        {
            inline Matrix4 Translate(const Matrix4& originalMatrix, const Vector3& translation)
            {
                Matrix4 outputMatrix(kSkipInitialization);
                Matrix4 translationMatrix(kSkipInitialization);

                MatrixCreateTranslation(&translationMatrix, &translation);
                const Matrix4 translationTransposed(translationMatrix.GetTransposed());
                MatrixMultiply(&outputMatrix, &originalMatrix, &translationTransposed);

                return outputMatrix;
            }

            inline Matrix4 Translate(const Matrix4& originalMatrix, float x, float y, float z)
            {
                return Translate(originalMatrix, tbMath::Vector3(x, y, z));
            }

            inline Matrix4 TranslateLocal(const Matrix4& originalMatrix, const Vector3& translation)
            {
                Matrix4 outputMatrix(kSkipInitialization);
                Matrix4 translationMatrix(kSkipInitialization);

                MatrixCreateTranslation(&translationMatrix, &translation);
                MatrixMultiply(&outputMatrix, &originalMatrix, &translationMatrix);

                return outputMatrix;
            }

            inline Matrix4 TranslateLocal(const Matrix4& originalMatrix, float x, float y, float z)
            {
                return TranslateLocal(originalMatrix, tbMath::Vector3(x, y, z));
            }


            inline Matrix4 Rotate(const Matrix4& originalMatrix, const Vector3& aroundAxis, const float rotationInDegrees)
            {
                Matrix4 outputMatrix(kSkipInitialization);
                Matrix4 rotationMatrix(kSkipInitialization);

                //Negating here for positive rotationInDegrees to spin clockwise around the axis.
                MatrixCreateRotationA(&rotationMatrix, &aroundAxis, -rotationInDegrees);
                //const Matrix4 rotationTransposed(rotationMatrix.GetTransposed());
                //MatrixMultiply(&outputMatrix, &originalMatrix, &rotationTransposed);
                MatrixMultiply(&outputMatrix, &originalMatrix, &rotationMatrix);

                return outputMatrix;
            }

            inline Matrix4 Rotate(const size_t& axisIndex, const Matrix4& originalMatrix, const float rotationInDegrees)
            {
                return Rotate(originalMatrix, *originalMatrix.GetBasis(axisIndex), rotationInDegrees);
            }

            inline Matrix4 RotateWorldSpace(const size_t& axisIndex, const Matrix4& originalMatrix, const float rotationInDegrees)
            {
                Matrix4 outputMatrix(kSkipInitialization);
                Matrix4 rotationMatrix(kSkipInitialization);

                //Negating here for positive rotationInDegrees to spin clockwise around the axis.
                MatrixCreateRotationA(&rotationMatrix, Matrix4::Identity().GetBasis(axisIndex), rotationInDegrees);
                const Matrix4 rotationTransposed(rotationMatrix.GetTransposed());
                MatrixMultiply(&outputMatrix, &originalMatrix, &rotationTransposed);

                return outputMatrix;
            }

            inline Matrix4 Scale(const Matrix4& originalMatrix, const Vector3& scale)
            {
                Matrix4 outputMatrix(kSkipInitialization);
                Matrix4 scalingMatrix(kSkipInitialization);

                MatrixCreateScale(&scalingMatrix, &scale);
                MatrixMultiply(&outputMatrix, &originalMatrix, &scalingMatrix);

                return outputMatrix;
            }

            inline Matrix4 Scale(const Matrix4& originalMatrix, float x, float y, float z)
            {
                return Scale(originalMatrix, Vector3(x, y, z));
            }

        }; /* namespace Unstable */

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

namespace tbMath = TurtleBrains::Math;

#endif /* _TurtleBrains_Matrix_h_ */