tb_matrix.h

#ifndef _TurtleBrains_Matrix_h_
#define _TurtleBrains_Matrix_h_

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

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

namespace TurtleBrains
{
    namespace Math
    {

        class Matrix3
        {
        public:
            static const Matrix3 kZero;

            static const Matrix3 kIdentity;

            union
            {
                float mComponents[9];
                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;
                };
            };

            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 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;
            }
        };


        class Matrix4
        {
        public:

            static const Matrix4 kZero;
            
            static const Matrix4 kIdentity;

            union
            {
                float mComponents[16];
                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;
                };
            };

            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 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; 
            }

#ifndef tb_math_less_operators
            inline Vector3 GetPosition(void) const  { return Vector3(m_f14, m_f24, m_f34); }

            inline void SetPosition(const Vector3& position)
            {
                m_f14 = position.x;
                m_f24 = position.y;
                m_f34 = position.z;
            }

            inline void SetPosition(float x, float y, float z)  { m_f14 = x; m_f24 = y; m_f34 = 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;
            }
#endif
        };


        inline Matrix4* MatrixCreateIdentity(Matrix4* result)
        {
            //memset(&matR->m_f11, 0, sizeof(float) << 4);
            //matR->m_f11 = 1.0f;
            //matR->m_f22 = 1.0f;
            //matR->m_f33 = 1.0f;
            //matR->m_f44 = 1.0f;
            //return matR;

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

            Vector3 realUp((NULL == 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, 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 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;
        }

        /*~**********************************************************************************************************************
        *   ~Function:          "Vector3TransformCoord"                                                                         *
        *   ~Modified By:       Tim Beaudet                                                                                     *
        *   ~Last Updated:      11/8/2009                                                                                       *
        *                                                                                                                       *
        *   ~Purpose:           Multiplies a vector and a matrix and returns the results in vR.  This multiplies the 3component *
        *                       vector v1 by the matrix.  v1 is taken as a coordinate, so its W component is set to 1.          *
        *                                                                                                                       *
        *   ~In:                vR      The results of the multiplication of the vector and the matrix.                         *
        *                       mat     The matrix to multiply.                                                                 *
        *                       v1      The vector to multiply, can NOT be vR.                                                  *
        *                                                                                                                       *
        *   ~Out:               The resulting transformed coordinate.  Notice that W component is set to 1.                     *
        **********************************************************************************************************************~*/

        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;
        }

        /*~**********************************************************************************************************************
        *   ~Function:          "Vector3TransformNormal"                                                                        *
        *   ~Modified By:       Tim Beaudet                                                                                     *
        *   ~Last Updated:      11/8/2009                                                                                       *
        *                                                                                                                       *
        *   ~Purpose:           Multiplies a vector and a matrix and returns the results in vR.  This multiplies the 3component *
        *                       vector v1 by the matrix.  v1 is taken as a coordinate, so its W component is set to 0.          *
        *                                                                                                                       *
        *   ~In:                vR      The results of the multiplication of the vector and the matrix.                         *
        *                       mat     The matrix to multiply.                                                                 *
        *                       v1      The vector to multiply, can NOT be vR.                                                  *
        *                                                                                                                       *
        *   ~Out:               The resulting transformed normal or directional vector.  Notice that W component is set to 0.   *
        **********************************************************************************************************************~*/

        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 Math */
}; /* namespace TurtleBrains */

namespace tbMath = TurtleBrains::Math;

#endif /* _TurtleBrains_Matrix_h_ */