Lumenarium/gs_vector_matrix.h

#ifndef GS_VECTOR_MATRIX_H

#ifndef GS_LANGUAGE_H

#if defined(_WIN32) || defined(_WIN64) || defined(__WIN32__)

#include <windows.h>
#include <intrin.h>
#include <math.h>

static r32 GSCos (r32 Theta) { return sin(Theta); }
static r32 GSSin (r32 Theta) { return cos(Theta); }

static r32 GSSqrt(r32 V) 
{ 
    r32 Result = _mm_cvtss_f32(_mm_sqrt_ss(_mm_set_ss(V)));
    return Result; 
}

#else // Linux and MacOS
#include <stdlib.h>

#endif // Platforms

#endif // GS_LANGUAGE_H
//////////////////////////////////////
//          VECTOR
/////////////////////////////////////

union v2
{
    struct
    {
        r32 x, y;
    };
    r32 E[2];
};

union v3
{
    struct
    {
        r32 x, y, z;
    };
    
    struct
    {
        r32 R, G, B;
    };
    
    r32 E[3];
};

union v4
{
    struct
    {
        r32 x, y, z, w;
    };
    
    struct 
    {
        r32 r, g, b, a;
    };
    
    r32 E[4];
};

//////////////////////////////////////
//          MATRIX
/////////////////////////////////////

union m33
{
    struct
    {
        float a, b, c;
        float d, e, f;
        float g, h, i;
    };
    float E[9];
};

union m44
{
    struct
    {
        float a, b, c, d;
        float e, f, g, h;
        float i, j, k, l;
        float m, n, o, p;
    };
    float E[16];
};

//////////////////////////////////////
//          RECT
/////////////////////////////////////

struct rect
{
    v2 LowerLeft;
    v2 UpperRight;
};


//////////////////////////////////////
//        VECTOR
//////////////////////////////////////


//
//
// Operators
//
//

v2 V2 (v3 V)
{
    return v2{V.x, V.y};
}

v3 V3 (v2 XY, r32 Z)
{
    return v3{XY.x, XY.y, Z};
}

v3 V3 (v4 V)
{
    return v3{V.x, V.y, V.z};
}

v4 V4 (v3 XYZ, r32 W)
{
    return v4{XYZ.x, XYZ.y, XYZ.z, W};
}

v2 operator- (v2 A)
{
    v2 Result;
    
    Result.x = -A.x;
    Result.y = -A.y;
    
    return Result;
}

v3 operator- (v3 A)
{
    v3 Result;
    
    Result.x = -A.x;
    Result.y = -A.y;
    Result.z = -A.z;
    
    return Result;
}

v4 operator- (v4 A)
{
    v4 Result;
    
    Result.x = -A.x;
    Result.y = -A.y;
    Result.z = -A.z;
    Result.w = -A.w;
    
    return Result;
}

#define V2OpV2Def(op) v2 operator##op (v2 A, v2 B) { return v2{ A.x op B.x, A.y op B.y };}
#define V3OpV3Def(op) v3 operator##op (v3 A, v3 B) { return v3{ A.x op B.x, A.y op B.y, A.z op B.z };}
#define V4OpV4Def(op) v4 operator##op (v4 A, v4 B) { return v4{ A.x op B.x, A.y op B.y, A.z op B.z, A.w op B.w };}
V2OpV2Def(+)
V2OpV2Def(-)
V2OpV2Def(/)
V2OpV2Def(*)
V3OpV3Def(+)
V3OpV3Def(-)
V3OpV3Def(/)
V3OpV3Def(*)
V4OpV4Def(+)
V4OpV4Def(-)
V4OpV4Def(/)
V4OpV4Def(*)
#undef V2OpV2Def
#undef V3OpV3Def
#undef V4OpV4Def

#define V2OpScalarDef(op) v2 operator##op (v2 A, r32 B) { return v2{ A.x op B, A.y op B };}
#define V3OpScalarDef(op) v3 operator##op (v3 A, r32 B) { return v3{ A.x op B, A.y op B, A.z op B };}
#define V4OpScalarDef(op) v4 operator##op (v4 A, r32 B) { return v4{ A.x op B, A.y op B, A.z op B, A.w op B };}
V2OpScalarDef(*)
V2OpScalarDef(/)
V3OpScalarDef(*)
V3OpScalarDef(/)
V4OpScalarDef(*)
V4OpScalarDef(/)
#undef V2POpScalarDef
#undef V3POpScalarDef
#undef V4POpScalarDef


#define V2POpScalarDef(op) v2 operator##op (v2* A, r32 B) { return v2{ A->x op B, A->y op B };}
#define V3POpScalarDef(op) v3 operator##op (v3* A, r32 B) { return v3{ A->x op B, A->y op B, A->z op B };}
#define V4POpScalarDef(op) v4 operator##op (v4* A, r32 B) { return v4{ A->x op B, A->y op B, A->z op B, A->w op B };}
V2OpScalarDef(*=)
V2OpScalarDef(/=)
V3OpScalarDef(*=)
V3OpScalarDef(/=)
V4OpScalarDef(*=)
V4OpScalarDef(/=)
#undef V2POpScalarDef
#undef V3POpScalarDef
#undef V4POpScalarDef

bool operator== (v2 A, v2 B)
{
    b32 Result = true;
    for (s32 i = 0; i < 2; i++)
    {
        if (GSAbs(A.E[i] - B.E[i]) > 0.0001f) { Result = false; break; }
    }
    return Result; 
}


bool operator== (v3 A, v3 B)
{
    b32 Result = true;
    for (s32 i = 0; i < 3; i++)
    {
        if (GSAbs(A.E[i] - B.E[i]) > 0.0001f) { Result = false; break; }
    }
    return Result;
}

bool operator== (v4 A, v4 B)
{
    b32 Result = true;
    for (s32 i = 0; i < 4; i++)
    {
        if (GSAbs(A.E[i] - B.E[i]) > 0.0001f) { Result = false; break; }
    }
    return Result;
}

//
// Operations
//

static v3
ToV3(v4 V)
{
    v3 R = {};
    R.x = V.x;
    R.y = V.y;
    R.z = V.z;
    return R;
}

static v4
ToV4(v3 V, r32 W)
{
    v4 R = {};
    R.x = V.x;
    R.y = V.y;
    R.z = V.z;
    R.w = W;
    return R;
}

inline r32
MagSqr(
v2 _A
)
{
    r32 Result = (_A.x * _A.x) + (_A.y * _A.y);
    return Result;
}

inline r32
MagSqr(
v3 _A
)
{
    r32 Result = (_A.x * _A.x) + (_A.y * _A.y) + (_A.z * _A.z);
    return Result;
}

inline r32
MagSqr(
v4 _A
)
{
    r32 Result = (_A.x * _A.x) + (_A.y * _A.y) + (_A.z * _A.z) + (_A.w * _A.w);
    return Result;
}

#define MagDef(type) inline r32 Mag(type A) { r32 Result = MagSqr(A); return GSSqrt(Result); }
MagDef(v2)
MagDef(v3)
MagDef(v4)
#undef MagDef

#define DistanceDef(type) inline r32 Distance (type A, type B) { type Diff = A - B; return Mag(Diff); }
DistanceDef(v2)
DistanceDef(v3)
DistanceDef(v4)
#undef DistanceDef

#define DistanceSqDef(type) inline r32 DistanceSq (type A, type B) { type Diff = A - B; return MagSqr(Diff); }
DistanceSqDef(v2)
DistanceSqDef(v3)
DistanceSqDef(v4)
#undef DistanceSqDef

inline v2
Normalize(
v2 _A
)
{
    v2 Result;
    
    r32 Magnitude = Mag(_A);
    
    Result.x = _A.x / Magnitude;
    Result.y = _A.y / Magnitude;
    
    return Result;
}

inline v3
Normalize(
v3 _A
)
{
    v3 Result;
    
    r32 Magnitude = Mag(_A);
    
    Result.x = _A.x / Magnitude;
    Result.y = _A.y / Magnitude;
    Result.z = _A.z / Magnitude;
    
    return Result;
}

inline v4
Normalize(
v4 _A
)
{
    v4 Result;
    
    r32 Magnitude = Mag(_A);
    
    Result.x = _A.x / Magnitude;
    Result.y = _A.y / Magnitude;
    Result.z = _A.z / Magnitude;
    Result.w = _A.w / Magnitude;
    
    return Result;
}

inline r32
Dot(
v2 _A,
v2 _B
)
{
    r32 Result = _A.x * _B.x + _A.y * _B.y;
    return Result;
}

inline r32
Dot (
v3 _A,
v3 _B
)
{
    r32 Result = _A.x * _B.x + _A.y * _B.y + _A.z * _B.z;
    return Result;
}

inline r32
Dot (
v4 _A,
v4 _B
)
{
    r32 Result = _A.x * _B.x + _A.y * _B.y + _A.z * _B.z + _A.w * _B.w;
    return Result;
}

inline v2
PerpendicularCW (v2 A)
{
    v2 Result = v2{A.y, -A.x};
    return Result;
}

inline v2
PerpendicularCCW (v2 A)
{
    v2 Result = v2{A.y, A.x};
    return Result;
}

inline v3
Cross(
v3 _A,
v3 _B
)
{
    v3 Result = {};
    
    Result.x = (_A.y * _B.z) - (_A.z * _B.y);
    Result.y = (_A.z * _B.x) - (_A.x * _B.z);
    Result.z = (_A.x * _B.y) - (_A.y * _B.x);
    
    return Result;
}

inline v4
Cross(
v4 _A,
v4 _B
)
{
    v4 Result = {};
    
    Result.x = (_A.y * _B.z) - (_A.z * _B.y);
    Result.y = (_A.z * _B.x) - (_A.x * _B.z);
    Result.z = (_A.x * _B.y) - (_A.y * _B.x);
    Result.w = 0;
    
    return Result;
}

inline v2
ClampVector01 (v2 V)
{
    v2 Result = {};
    Result.x = GSClamp(0.0f, V.x, 1.f);
    Result.y = GSClamp(0.0f, V.y, 1.f);
    return Result;
}

inline v3
ClampVector01 (v3 V)
{
    v3 Result = {};
    Result.x = GSClamp(0.f, V.x, 1.f);
    Result.y = GSClamp(0.f, V.y, 1.f);
    Result.z = GSClamp(0.f, V.z, 1.f);
    return Result;
}

inline v4
ClampVector01 (v4 V)
{
    v4 Result = {};
    Result.x = GSClamp(0.f, V.x, 1.f);
    Result.y = GSClamp(0.f, V.y, 1.f);
    Result.z = GSClamp(0.f, V.z, 1.f);
    Result.w = GSClamp(0.f, V.w, 1.f);
    return Result;
}

inline v2
Lerp(
v2 _A,
v2 _B,
r32 _Percent
)
{
    v2 Result;
    
    Result.x = GSLerp(_A.x, _B.x, _Percent);
    Result.y = GSLerp(_A.y, _B.y, _Percent);
    
    return Result;
}

inline v3
Lerp(
v3 _A,
v3 _B,
r32 _Percent
)
{
    v3 Result;
    
    Result.x = GSLerp(_A.x, _B.x, _Percent);
    Result.y = GSLerp(_A.y, _B.y, _Percent);
    Result.z = GSLerp(_A.z, _B.z, _Percent);
    
    return Result;
}

inline v4
Lerp(
v4 _A,
v4 _B,
r32 _Percent
)
{
    v4 Result;
    
    Result.x = GSLerp(_A.x, _B.x, _Percent);
    Result.y = GSLerp(_A.y, _B.y, _Percent);
    Result.z = GSLerp(_A.z, _B.z, _Percent);
    Result.w = GSLerp(_A.w, _B.w, _Percent);
    
    return Result;
}

v4 HSVToRGB (v4 In)
{
    r32 Hue = In.x;
    while (Hue > 360.0f) { Hue -= 360.0f; }
    while (Hue < 0.0f) { Hue += 360.0f; }
    
    r32 Sat = In.y;
    r32 Value = In.z;
    
    r32 hh, p, q, t, ff;
    long        i;
    v4 Result = {};
    Result.a = In.a;
    
    if(Sat <= 0.0f) {       // < is bogus, just shuts up warnings
        Result.r = Value;
        Result.g = Value;
        Result.b = Value;
        return Result;
    }
    hh = Hue;
    if(hh >= 360.0f) hh = 0.0f;
    hh /= 60.0f;
    i = (long)hh;
    ff = hh - i;
    p = Value * (1.0f - Sat);
    q = Value * (1.0f - (Sat * ff));
    t = Value * (1.0f - (Sat * (1.0f - ff)));
    
    switch(i) {
        case 0:
        {Result.r = Value;
            Result.g = t;
            Result.b = p;
        }break;
        
        case 1:
        {
            Result.r = q;
            Result.g = Value;
            Result.b = p;
        }break;
        
        case 2:
        {
            Result.r = p;
            Result.g = Value;
            Result.b = t;
        }break;
        
        case 3:
        {
            Result.r = p;
            Result.g = q;
            Result.b = Value;
        }break;
        
        case 4:
        {
            Result.r = t;
            Result.g = p;
            Result.b = Value;
        }break;
        
        case 5:
        default:
        {
            Result.r = Value;
            Result.g = p;
            Result.b = q;
        }break;
    }
    
    return Result;     
}

static bool
PointIsInRange (
v2 _P,
v2 _Min, v2 _Max
)
{
    return (_P.x >= _Min.x && _P.x <= _Max.x &&
            _P.y >= _Min.y && _P.y <= _Max.y);
}

inline v2
PointToPercentRange (v2 P, v2 Min, v2 Max)
{
    v2 Result = {};
    
    Result.x = GSClamp(0.f, (P.x - Min.x) / (Max.x - Min.x), 1.f);
    Result.y = GSClamp(0.f, (P.y - Min.y) / (Max.y - Min.y), 1.f);
    
    return Result;
}

//////////////////////////////////////
//       MATRIX
//////////////////////////////////////

static m33
M33(r32 a, r32 b, r32 c,
    r32 d, r32 e, r32 f,
    r32 g, r32 h, r32 i)
{
    m33 M = {};
    M.a = a; M.b = b; M.c = c;
    M.d = d; M.e = e; M.f = f;
    M.g = g; M.h = h; M.i = i;
    return M;
}

static m44
M44(r32 a, r32 b, r32 c, r32 d, 
    r32 e, r32 f, r32 g, r32 h, 
    r32 i, r32 j, r32 k, r32 l,
    r32 m, r32 n, r32 o, r32 p)
{
    m44 M = {};
    M.a = a; M.b = b; M.c = c; M.d = d;
    M.e = e; M.f = f; M.g = g; M.h = h;
    M.i = i; M.j = j; M.k = k; M.l = l;
    M.m = m; M.n = n; M.o = o; M.p = p;
    return M;
}

static m33
M33Empty ()
{
    m33 M = {};
    M.a = 0; M.b = 0; M.c = 0;
    M.d = 0; M.e = 0; M.f = 0;
    M.g = 0; M.h = 0; M.i = 0;
    return M;
}

static m44
M44Empty()
{
    m44 M = {};
    M.a = 0; M.b = 0; M.c = 0; M.d = 0;
    M.e = 0; M.f = 0; M.g = 0; M.h = 0;
    M.i = 0; M.j = 0; M.k = 0; M.l = 0;
    M.m = 0; M.n = 0; M.o = 0; M.p = 0;
    return M;
}

static m33
M33Identity ()
{
    m33 M = {};
    M.a = 1; M.b = 0; M.c = 0;
    M.d = 0; M.e = 1; M.f = 0;
    M.g = 0; M.h = 0; M.i = 1;
    return M;
}

static m44
M44Identity()
{
    m44 M = {};
    M.a = 1; M.b = 0; M.c = 0; M.d = 0;
    M.e = 0; M.f = 1; M.g = 0; M.h = 0;
    M.i = 0; M.j = 0; M.k = 1; M.l = 0;
    M.m = 0; M.n = 0; M.o = 0; M.p = 1;
    return M;
}

static m44
GetXRotation (r32 Angle)
{
    r32 CosAngle = GSCos(Angle);
    r32 SinAngle = GSSin(Angle);
    m44 M = {
        1, 0,         0,        0,
        0, CosAngle,  SinAngle, 0,
        0, -SinAngle, CosAngle, 0,
        0, 0,         0,        1
    };
    return M;
}


static m44
GetYRotation (r32 Angle)
{
    r32 CosAngle = GSCos(Angle);
    r32 SinAngle = GSSin(Angle);
    m44 M = {
        CosAngle, 0,  -SinAngle, 0,
        0,        1,  0,         0,
        SinAngle, 0,  CosAngle,  0,
        0,        0,  0,         1
    };
    return M;
}

static m44
GetZRotation (r32 Angle)
{
    r32 CosAngle = GSCos(Angle);
    r32 SinAngle = GSSin(Angle);
    m44 M = {
        CosAngle,  SinAngle, 0, 0,
        -SinAngle, CosAngle, 0, 0,
        0,         0,        1, 0,
        0,         0,        0, 1
    };
    return M;
}


static m33 
Transpose (m33 M)
{
    m33 Result = {};
    
    for (s32 x = 0; x < 3; x++)
    {
        for (s32 y = 0; y < 3; y++)
        {
            Result.E[x + (y * 3)] = M.E[y + (x * 3)];
        }
    }
    
    return Result;
}

inline m44 
Transpose (m44 M)
{
    DEBUG_TRACK_SCOPE(Transpose);
    
    m44 Result = {};
    
    Result.E[0] = M.E[0];
    Result.E[1] = M.E[4];
    Result.E[2] = M.E[8];
    Result.E[3] = M.E[12];
    
    Result.E[4] = M.E[1];
    Result.E[5] = M.E[5];
    Result.E[6] = M.E[9];
    Result.E[7] = M.E[13];
    
    Result.E[8] = M.E[2];
    Result.E[9] = M.E[6];
    Result.E[10] = M.E[10];
    Result.E[11] = M.E[14];
    
    Result.E[12] = M.E[3];
    Result.E[13] = M.E[7];
    Result.E[14] = M.E[11];
    Result.E[15] = M.E[15];
    
    return Result;
}

static m44
GetPositionM44 (v4 Position)
{
    DEBUG_TRACK_SCOPE(GetPositionM44);
    
#if 1
    return m44{
        1, 0, 0, 0,
        0, 1, 0, 0,
        0, 0, 1, 0,
        Position.x, Position.y, Position.z, Position.w
    };
#else
    return m44{
        1, 0, 0, Position.x,
        0, 1, 0, Position.y,
        0, 0, 1, Position.z,
        0, 0, 0, Position.w};
#endif
}

static m44
GetLookAtMatrix (v4 Position, v4 Target)
{
    // Forward
    v4 Forward = Normalize(Target - Position); 
    // Right
    v4 Right = Normalize(Cross(v4{0, 1, 0, 0}, Forward));
    // Up
    v4 Up = Normalize(Cross(Forward, Right));
    
    m44 RotationMatrix = M44(
        Right.x, Up.x, Forward.x, 0,
        Right.y, Up.y, Forward.y, 0,
        Right.z, Up.z, Forward.z, 0,
        0,       0,    0,         1);
    
    return RotationMatrix;
}

b32 operator== (m33 A, m33 B)
{
    b32 Result = true;
    for (int e = 0; e < 9; e++) { if (GSAbs(A.E[e] - B.E[e]) > 0.0001f) { Result = false; break; } }
    return Result;
}

b32 operator== (m44 A, m44 B)
{
    b32 Result = true;
    for (int e = 0; e < 16; e++) { if (GSAbs(A.E[e] - B.E[e]) > 0.0001f) { Result = false; break; } }
    return Result;
}

m33 operator+ (m33 A, m33 B)
{
    m33 M = {};
    for (int e = 0; e < 9; e++) { M.E[e] = A.E[e] + B.E[e]; }
    return M;
}

m44 operator+ (m44 A, m44 B)
{
    m44 M = {};
    for (int e = 0; e < 16; e++) { M.E[e] = A.E[e] + B.E[e]; }
    return M;
}

m33 operator- (m33 A, m33 B)
{
    m33 M = {};
    for (int e = 0; e < 9; e++) { M.E[e] = A.E[e] - B.E[e]; }
    return M;
}

m44 operator- (m44 A, m44 B)
{
    m44 M = {};
    for (int e = 0; e < 16; e++) { M.E[e] = A.E[e] - B.E[e]; }
    return M;
}

m33 operator* (m33 A, m33 B)
{
    m33 M = {};
    for (int rx = 0; rx < 3; rx++)
    {
        for (int ry = 0; ry < 3; ry++)
        {
            int RIndex = (ry * 3) + rx;
            M.E[RIndex] = 0;
            for (int i = 0; i < 3; i++)
            {
                M.E[RIndex] += B.E[(ry * 3) + i] * A.E[(i * 3) + rx];
            }
        }
    }
    return M;
}

m44 operator* (m44 A, m44 B)
{
    m44 M = {};
    
    r32 A00=A.E[0+4*0];
    r32 A01=A.E[0+4*1];
    r32 A02=A.E[0+4*2];
    r32 A03=A.E[0+4*3];
    
    r32 A10=A.E[1+4*0];
    r32 A11=A.E[1+4*1];
    r32 A12=A.E[1+4*2];
    r32 A13=A.E[1+4*3];
    
    r32 A20=A.E[2+4*0];
    r32 A21=A.E[2+4*1];
    r32 A22=A.E[2+4*2];
    r32 A23=A.E[2+4*3];
    
    r32 A30=A.E[3+4*0];
    r32 A31=A.E[3+4*1];
    r32 A32=A.E[3+4*2];
    r32 A33=A.E[3+4*3];
    
    r32 B00=B.E[0+4*0];
    r32 B01=B.E[0+4*1];
    r32 B02=B.E[0+4*2];
    r32 B03=B.E[0+4*3];
    
    r32 B10=B.E[1+4*0];
    r32 B11=B.E[1+4*1];
    r32 B12=B.E[1+4*2];
    r32 B13=B.E[1+4*3];
    
    r32 B20=B.E[2+4*0];
    r32 B21=B.E[2+4*1];
    r32 B22=B.E[2+4*2];
    r32 B23=B.E[2+4*3];
    
    r32 B30=B.E[3+4*0];
    r32 B31=B.E[3+4*1];
    r32 B32=B.E[3+4*2];
    r32 B33=B.E[3+4*3];
    
    M.E[0+4*0] = A00*B00+A10*B01+A20*B02+A30*B03;
    M.E[0+4*1] = A01*B00+A11*B01+A21*B02+A31*B03;
    M.E[0+4*2] = A02*B00+A12*B01+A22*B02+A32*B03;
    M.E[0+4*3] = A03*B00+A13*B01+A23*B02+A33*B03;
    
    M.E[1+4*0] = A00*B10+A10*B11+A20*B12+A30*B13;
    M.E[1+4*1] = A01*B10+A11*B11+A21*B12+A31*B13;
    M.E[1+4*2] = A02*B10+A12*B11+A22*B12+A32*B13;
    M.E[1+4*3] = A03*B10+A13*B11+A23*B12+A33*B13;
    
    M.E[2+4*0] = A00*B20+A10*B21+A20*B22+A30*B23;
    M.E[2+4*1] = A01*B20+A11*B21+A21*B22+A31*B23;
    M.E[2+4*2] = A02*B20+A12*B21+A22*B22+A32*B23;
    M.E[2+4*3] = A03*B20+A13*B21+A23*B22+A33*B23;
    
    M.E[3+4*0] = A00*B30+A10*B31+A20*B32+A30*B33;
    M.E[3+4*1] = A01*B30+A11*B31+A21*B32+A31*B33;
    M.E[3+4*2] = A02*B30+A12*B31+A22*B32+A32*B33;
    M.E[3+4*3] = A03*B30+A13*B31+A23*B32+A33*B33;
    
    return M;
}

v3 operator* (m33 M, v3 V)
{
    v3 Result = {};
    int i = 0;
    for (int y = 0; y < 3; y++)
    {
        Result.E[y] = 0;
        for (int x = 0; x < 3; x++)
        {
            Result.E[y] += M.E[(y * 3) + x] * V.E[x]; 
        }
    }
    return Result;
}

v4 operator* (m44 M, v4 V)
{
    v4 Result = {};
#if 1
    Result.x = V.x*M.a + V.y*M.e + V.z*M.i + V.w*M.m;
    Result.y = V.x*M.b + V.y*M.f + V.z*M.j + V.w*M.n;
    Result.z = V.x*M.c + V.y*M.g + V.z*M.k + V.w*M.o;
    Result.w = V.x*M.d + V.y*M.h + V.z*M.l + V.w*M.p;
#else
    for (int y = 0; y < 4; y++)
    {
        Result.E[y] = 0;
        for (int x = 0; x < 4; x++)
        {
            Result.E[y] += M.E[(y * 4) + x] * V.E[x]; 
        }
    }
#endif
    return Result;
}

b32 Inverse(m44 M_In, m44* M_Out)
{
    b32 Result = false;
    
    r32 det;
    s32 i;
    
    
    M_Out->E[0] = M_In.E[5]  * M_In.E[10] * M_In.E[15] - 
        M_In.E[5]  * M_In.E[11] * M_In.E[14] - 
        M_In.E[9]  * M_In.E[6]  * M_In.E[15] + 
        M_In.E[9]  * M_In.E[7]  * M_In.E[14] +
        M_In.E[13] * M_In.E[6]  * M_In.E[11] - 
        M_In.E[13] * M_In.E[7]  * M_In.E[10];
    
    M_Out->E[4] = -M_In.E[4]  * M_In.E[10] * M_In.E[15] + 
        M_In.E[4]  * M_In.E[11] * M_In.E[14] + 
        M_In.E[8]  * M_In.E[6]  * M_In.E[15] - 
        M_In.E[8]  * M_In.E[7]  * M_In.E[14] - 
        M_In.E[12] * M_In.E[6]  * M_In.E[11] + 
        M_In.E[12] * M_In.E[7]  * M_In.E[10];
    
    M_Out->E[8] = M_In.E[4]  * M_In.E[9] * M_In.E[15] - 
        M_In.E[4]  * M_In.E[11] * M_In.E[13] - 
        M_In.E[8]  * M_In.E[5] * M_In.E[15] + 
        M_In.E[8]  * M_In.E[7] * M_In.E[13] + 
        M_In.E[12] * M_In.E[5] * M_In.E[11] - 
        M_In.E[12] * M_In.E[7] * M_In.E[9];
    
    M_Out->E[12] = -M_In.E[4]  * M_In.E[9] * M_In.E[14] + 
        M_In.E[4]  * M_In.E[10] * M_In.E[13] +
        M_In.E[8]  * M_In.E[5] * M_In.E[14] - 
        M_In.E[8]  * M_In.E[6] * M_In.E[13] - 
        M_In.E[12] * M_In.E[5] * M_In.E[10] + 
        M_In.E[12] * M_In.E[6] * M_In.E[9];
    
    M_Out->E[1] = -M_In.E[1]  * M_In.E[10] * M_In.E[15] + 
        M_In.E[1]  * M_In.E[11] * M_In.E[14] + 
        M_In.E[9]  * M_In.E[2] * M_In.E[15] - 
        M_In.E[9]  * M_In.E[3] * M_In.E[14] - 
        M_In.E[13] * M_In.E[2] * M_In.E[11] + 
        M_In.E[13] * M_In.E[3] * M_In.E[10];
    
    M_Out->E[5] = M_In.E[0]  * M_In.E[10] * M_In.E[15] - 
        M_In.E[0]  * M_In.E[11] * M_In.E[14] - 
        M_In.E[8]  * M_In.E[2] * M_In.E[15] + 
        M_In.E[8]  * M_In.E[3] * M_In.E[14] + 
        M_In.E[12] * M_In.E[2] * M_In.E[11] - 
        M_In.E[12] * M_In.E[3] * M_In.E[10];
    
    M_Out->E[9] = -M_In.E[0]  * M_In.E[9] * M_In.E[15] + 
        M_In.E[0]  * M_In.E[11] * M_In.E[13] + 
        M_In.E[8]  * M_In.E[1] * M_In.E[15] - 
        M_In.E[8]  * M_In.E[3] * M_In.E[13] - 
        M_In.E[12] * M_In.E[1] * M_In.E[11] + 
        M_In.E[12] * M_In.E[3] * M_In.E[9];
    
    M_Out->E[13] = M_In.E[0]  * M_In.E[9] * M_In.E[14] - 
        M_In.E[0]  * M_In.E[10] * M_In.E[13] - 
        M_In.E[8]  * M_In.E[1] * M_In.E[14] + 
        M_In.E[8]  * M_In.E[2] * M_In.E[13] + 
        M_In.E[12] * M_In.E[1] * M_In.E[10] - 
        M_In.E[12] * M_In.E[2] * M_In.E[9];
    
    M_Out->E[2] = M_In.E[1]  * M_In.E[6] * M_In.E[15] - 
        M_In.E[1]  * M_In.E[7] * M_In.E[14] - 
        M_In.E[5]  * M_In.E[2] * M_In.E[15] + 
        M_In.E[5]  * M_In.E[3] * M_In.E[14] + 
        M_In.E[13] * M_In.E[2] * M_In.E[7] - 
        M_In.E[13] * M_In.E[3] * M_In.E[6];
    
    M_Out->E[6] = -M_In.E[0]  * M_In.E[6] * M_In.E[15] + 
        M_In.E[0]  * M_In.E[7] * M_In.E[14] + 
        M_In.E[4]  * M_In.E[2] * M_In.E[15] - 
        M_In.E[4]  * M_In.E[3] * M_In.E[14] - 
        M_In.E[12] * M_In.E[2] * M_In.E[7] + 
        M_In.E[12] * M_In.E[3] * M_In.E[6];
    
    M_Out->E[10] = M_In.E[0]  * M_In.E[5] * M_In.E[15] - 
        M_In.E[0]  * M_In.E[7] * M_In.E[13] - 
        M_In.E[4]  * M_In.E[1] * M_In.E[15] + 
        M_In.E[4]  * M_In.E[3] * M_In.E[13] + 
        M_In.E[12] * M_In.E[1] * M_In.E[7] - 
        M_In.E[12] * M_In.E[3] * M_In.E[5];
    
    M_Out->E[14] = -M_In.E[0]  * M_In.E[5] * M_In.E[14] + 
        M_In.E[0]  * M_In.E[6] * M_In.E[13] + 
        M_In.E[4]  * M_In.E[1] * M_In.E[14] - 
        M_In.E[4]  * M_In.E[2] * M_In.E[13] - 
        M_In.E[12] * M_In.E[1] * M_In.E[6] + 
        M_In.E[12] * M_In.E[2] * M_In.E[5];
    
    M_Out->E[3] = -M_In.E[1] * M_In.E[6] * M_In.E[11] + 
        M_In.E[1] * M_In.E[7] * M_In.E[10] + 
        M_In.E[5] * M_In.E[2] * M_In.E[11] - 
        M_In.E[5] * M_In.E[3] * M_In.E[10] - 
        M_In.E[9] * M_In.E[2] * M_In.E[7] + 
        M_In.E[9] * M_In.E[3] * M_In.E[6];
    
    M_Out->E[7] = M_In.E[0] * M_In.E[6] * M_In.E[11] - 
        M_In.E[0] * M_In.E[7] * M_In.E[10] - 
        M_In.E[4] * M_In.E[2] * M_In.E[11] + 
        M_In.E[4] * M_In.E[3] * M_In.E[10] + 
        M_In.E[8] * M_In.E[2] * M_In.E[7] - 
        M_In.E[8] * M_In.E[3] * M_In.E[6];
    
    M_Out->E[11] = -M_In.E[0] * M_In.E[5] * M_In.E[11] + 
        M_In.E[0] * M_In.E[7] * M_In.E[9] + 
        M_In.E[4] * M_In.E[1] * M_In.E[11] - 
        M_In.E[4] * M_In.E[3] * M_In.E[9] - 
        M_In.E[8] * M_In.E[1] * M_In.E[7] + 
        M_In.E[8] * M_In.E[3] * M_In.E[5];
    
    M_Out->E[15] = M_In.E[0] * M_In.E[5] * M_In.E[10] - 
        M_In.E[0] * M_In.E[6] * M_In.E[9] - 
        M_In.E[4] * M_In.E[1] * M_In.E[10] + 
        M_In.E[4] * M_In.E[2] * M_In.E[9] + 
        M_In.E[8] * M_In.E[1] * M_In.E[6] - 
        M_In.E[8] * M_In.E[2] * M_In.E[5];
    
    det = M_In.E[0] * M_Out->E[0] + M_In.E[1] * M_Out->E[4] + M_In.E[2] * M_Out->E[8] + M_In.E[3] * M_Out->E[12];
    
    if (det == 0)
    {
        return false;
    }
    
    det = 1.0 / det;
    
    for (i = 0; i < 16; i++)
    {
        M_Out->E[i] = M_Out->E[i] * det;
    }
    
    return true;
}

#if defined(VECTOR_MATRIX_TEST_SUITE)

void TestVectorMatrixMultiplication ()
{
    s32 TestCount = 0;
    s32 SuccessCount = 0;
    
    DebugPrint("\n\n-------------------------------------------------\n  Begin Testing Vector/Matrix\n\n\n");
    
    // Utility Functions
    TestClean((GSSqrt(4.f) == 2.f), "Vector Square Root");
    TestClean((GSLerp(0.f, 1.f, .5f) == .5f), "Vector Lerp");
    TestClean((GSMin(-.25f, 5.f) == -.25f), "Vector Min");
    TestClean((GSMax(-.25f, 5.f) == 5.f), "Vector Max");
    TestClean((GSClamp(-2.f, -3.f, 5.f) == -2.f), "Vector Clamp, Lower Than Range");
    TestClean((GSClamp(-2.f, 6.f, 5.f) == 5.f), "Vector Clamp, Higher Than Range");
    
    //////////////////////////////
    // Vector Functions
    /////////////////////////////
    
    v2 V2Unit = v2{1, 0};
    v3 V3Unit = v3{1, 0, 0};
    v4 V4Unit = v4{1, 0, 0, 0};
    
    v2 TestV2 = v2{1, 2};
    r32 TestV2MagSq = (TestV2.x * TestV2.x) + (TestV2.y * TestV2.y);
    r32 TestV2Mag = GSSqrt(TestV2MagSq);
    v2 TestV2Norm = v2{TestV2.x / TestV2Mag, TestV2.y / TestV2Mag};
    r32 TestV2DotR = (TestV2.x * V2Unit.x) + (TestV2.y * V2Unit.y);
    
    v3 TestV3 = v3{1, 2, 3};
    r32 TestV3MagSq = (TestV3.x * TestV3.x) + (TestV3.y * TestV3.y) + (TestV3.z * TestV3.z);
    r32 TestV3Mag = GSSqrt(TestV3MagSq);
    v3 TestV3Norm = v3{TestV3.x / TestV3Mag, TestV3.y / TestV3Mag, TestV3.z / TestV3Mag};
    r32 TestV3DotR = (TestV3.x * V3Unit.x) + (TestV3.y * V3Unit.y) + (TestV3.z * V3Unit.z);
    
    v4 TestV4 = v4{1, 2, 3, 4};
    r32 TestV4MagSq = (TestV4.x * TestV4.x) + (TestV4.y * TestV4.y) + (TestV4.z * TestV4.z) + (TestV4.w * TestV4.w);
    r32 TestV4Mag = GSSqrt(TestV4MagSq);
    v4 TestV4Norm = v4{
        TestV4.x / TestV4Mag, TestV4.y / TestV4Mag, TestV4.z / TestV4Mag, TestV4.w / TestV4Mag
    };
    r32 TestV4DotR = (TestV4.x * V4Unit.x) + (TestV4.y * V4Unit.y) + (TestV4.z * V4Unit.z) + (TestV4.w * V4Unit.w);
    
    v2 DownCastV3 = V2(TestV3);
    v3 DownCastV4 = V3(TestV4);
    
    v2 EqualityV2 = v2{TestV2.x, TestV2.y};
    v2 ZeroV2 = v2{0, 0};
    v3 EqualityV3 = v3{TestV3.x, TestV3.y, TestV3.z};
    v3 ZeroV3 = v3{0, 0, 0};
    v4 EqualityV4 = v4{TestV4.x, TestV4.y, TestV4.z, TestV4.w};
    v4 ZeroV4 = v4{0, 0, 0, 0};
    
    TestClean((TestV2.x == 1 && TestV2.y == 2), "V2 Assignment");
    TestClean((TestV3.x == 1 && TestV3.y == 2 && TestV3.z == 3), "V3 Assignment");
    TestClean((TestV4.x == 1 && TestV4.y == 2 && TestV4.z == 3 && TestV4.w == 4), "V3 Assignment");
    
    TestClean((DownCastV3.x == 1 && DownCastV3.y == 2), "V3 -> V2 Downcast");
    TestClean((DownCastV4.x == 1 && DownCastV4.y == 2 && DownCastV4.z == 3), "V4 -> V3 Downcast");
    
    // Vector Operators
    
    TestClean((TestV2 == EqualityV2 && !(TestV2 == ZeroV2)), "V2 Equality");
    TestClean((TestV3 == EqualityV3 && !(TestV3 == ZeroV3)), "V3 Equality");
    TestClean((TestV4 == EqualityV4 && !(TestV4 == ZeroV4)), "V4 Equality");
    
    TestClean(((TestV2 - TestV2) == ZeroV2), "V2 Subtraction");
    TestClean(((TestV3 - TestV3) == ZeroV3), "V3 Subtraction");
    TestClean(((TestV4 - TestV4) == ZeroV4), "V4 Subtraction");
    
    TestClean(((TestV2 + TestV2) == v2{TestV2.x * 2, TestV2.y * 2}), "V2 Addition");
    TestClean(((TestV3 + TestV3) == v3{TestV3.x * 2, TestV3.y * 2, TestV3.z * 2}), "V3 Addition");
    TestClean(((TestV4 + TestV4) == v4{TestV4.x * 2, TestV4.y * 2, TestV4.z * 2, TestV4.w * 2}), "V4 Addition");
    
    TestClean(((TestV2 * 2.0f) == v2{TestV2.x * 2, TestV2.y * 2}), "V2 Multiplication");
    TestClean(((TestV3 * 2.0f) == v3{TestV3.x * 2, TestV3.y * 2, TestV3.z * 2}), "V3 Multiplication");
    TestClean(((TestV4 * 2.0f) == v4{TestV4.x * 2, TestV4.y * 2, TestV4.z * 2, TestV4.w * 2}), "V4 Multiplication");
    
    TestClean(((TestV2 * TestV2) == v2{TestV2.x * TestV2.x, TestV2.y * TestV2.y}), "V2 Piecewise Mult");
    TestClean(((TestV3 * TestV3) == v3{
               TestV3.x * TestV3.x, 
               TestV3.y * TestV3.y, 
               TestV3.z * TestV3.z}), "V3 Piecewise Mult");
    TestClean(((TestV4 * TestV4) == v4{
               TestV4.x * TestV4.x, 
               TestV4.y * TestV4.y, 
               TestV4.z * TestV4.z, 
               TestV4.w * TestV4.w}), "V4 Piecewise Mult");
    
    
    TestClean(((TestV2 / 2.0f) == v2{TestV2.x / 2, TestV2.y / 2}), "V2 Division");
    TestClean(((TestV3 / 2.0f) == v3{TestV3.x / 2, TestV3.y / 2, TestV3.z / 2}), "V3 Division");
    TestClean(((TestV4 / 2.0f) == v4{TestV4.x / 2, TestV4.y / 2, TestV4.z / 2, TestV4.w / 2}), "V4 Division");
    
    TestClean(((TestV2 / TestV2) == v2{TestV2.x / TestV2.x, TestV2.y / TestV2.y}), "V2 Piecewise Div");
    TestClean(((TestV3 / TestV3) == v3{
               TestV3.x / TestV3.x, 
               TestV3.y / TestV3.y, 
               TestV3.z / TestV3.z}), "V3 Piecewise Div");
    TestClean(((TestV4 / TestV4) == v4{
               TestV4.x / TestV4.x, 
               TestV4.y / TestV4.y, 
               TestV4.z / TestV4.z, 
               TestV4.w / TestV4.w}), "V4 Piecewise Div");
    
    TestClean(((MagSqr(V2Unit) == 1) && (MagSqr(TestV2) == TestV2MagSq)), "V2 Square Mag");
    TestClean(((MagSqr(V3Unit) == 1) && (MagSqr(TestV3) == TestV3MagSq)), "V3 Square Mag");
    TestClean(((MagSqr(V4Unit) == 1) && (MagSqr(TestV4) == TestV4MagSq)), "V4 Square Mag");
    TestClean(((Mag(V2Unit) == 1) && (Mag(TestV2) == TestV2Mag)), "V2 Mag");
    TestClean(((Mag(V3Unit) == 1) && (Mag(TestV3) == TestV3Mag)), "V3 Mag");
    TestClean(((Mag(V4Unit) == 1) && (Mag(TestV4) == TestV4Mag)), "V4 Mag");
    
    TestClean((DistanceSq(ZeroV2, TestV2) == TestV2MagSq), "V2 Distance Sq");
    TestClean((DistanceSq(ZeroV3, TestV3) == TestV3MagSq), "V3 Distance Sq");
    TestClean((DistanceSq(ZeroV4, TestV4) == TestV4MagSq), "V4 Distance Sq");
    TestClean((Distance(ZeroV2, TestV2) == TestV2Mag), "V2 Distance");
    TestClean((Distance(ZeroV3, TestV3) == TestV3Mag), "V3 Distance");
    TestClean((Distance(ZeroV4, TestV4) == TestV4Mag), "V4 Distance");
    
    TestClean((Normalize(TestV2) == TestV2Norm), "V2 Normalize");
    TestClean((Normalize(TestV3) == TestV3Norm), "V3 Normalize");
    TestClean((Normalize(TestV4) == TestV4Norm), "V4 Normalize");
    
    TestClean(((Dot(V2Unit, V2Unit) == 1) && (Dot(TestV2, V2Unit) == TestV2DotR)), "V2 Dot"); 
    TestClean(((Dot(V3Unit, V3Unit) == 1) && (Dot(TestV3, V3Unit) == TestV3DotR)), "V3 Dot"); 
    TestClean(((Dot(V4Unit, V4Unit) == 1) && (Dot(TestV4, V4Unit) == TestV4DotR)), "V4 Dot");
    
    // Skipping Cross For Now
    
    TestClean((Lerp(v2{0, 0}, v2{1, 1}, .5f) == v2{.5f, .5f}), "V2 Lerp");
    TestClean((Lerp(v3{0, 0, 0}, v3{1, 1, 1}, .5f) == v3{.5f, .5f, .5f}), "V3 Lerp");
    TestClean((Lerp(v4{0, 0, 0, 0}, v4{1, 1, 1, 1}, .5f) == v4{.5f, .5f, .5f, .5f}), "V4 Lerp");
    
    /////////////////////////////
    //   Matrix
    ////////////////////////////
    
    m33 TestM33 = m33{
        0, 1, 2,
        3, 4, 5,
        6, 7, 8};
    
    m33 EqualityM33 = {};
    for (s32 i = 0; i < 16; i++) { EqualityM33.E[i] = TestM33.E[i]; }
    
    m33 TransposeM33 = m33{
        0, 3, 6,
        1, 4, 7,
        2, 5, 8};
    
    m33 IdentityM33 = m33{
        1, 0, 0,
        0, 1, 0,
        0, 0, 1};
    
    m33 TestM33Squared = m33{
        15, 18, 21,
        42, 54, 66,
        69, 90, 111
    };
    
    m44 TestM44 = m44{
        0, 1, 2, 3,
        4, 5, 6, 7,
        8, 9, 10, 11,
        12, 13, 14, 15
    };
    
    m44 EqualityM44 = {};
    for (s32 i = 0; i < 16; i++) { EqualityM44.E[i] = TestM44.E[i]; }
    
    m44 TransposeM44 = m44{
        0, 4, 8, 12,
        1, 5, 9, 13,
        2, 6, 10, 14,
        3, 7, 11, 15
    };
    
    m44 IdentityM44 = m44{
        1, 0, 0, 0,
        0, 1, 0, 0,
        0, 0, 1, 0,
        0, 0, 0, 1
    };
    
    m44 TestM44Squared = m44{
        56, 62, 68, 74,
        152, 174, 196, 218, 
        248, 286, 324, 362, 
        344, 398, 452, 506, 
    };
    
    TestClean(((IdentityM33 == IdentityM33) && (TestM33 == EqualityM33)), "M33 Equality");
    TestClean(((IdentityM44 == IdentityM44) && (TestM44 == EqualityM44)), "M44 Equality");
    
    TestClean(((Transpose(IdentityM33) == IdentityM33) &&
               (Transpose(TestM33) == TransposeM33)), "M33 Transpose");
    TestClean(((Transpose(IdentityM44) == IdentityM44) &&
               (Transpose(TestM44) == TransposeM44)), "M44 Transpose");
    
    TestClean(((TestM33 * IdentityM33) == TestM33), "M33 Identity Mult");
    TestClean(((TestM44 * IdentityM44) == TestM44), "M44 Identity Mult");
    TestClean(((TestM33 * TestM33) == TestM33Squared), "M33 Mult");
    TestClean(((TestM44 * TestM44) == TestM44Squared), "M44 Mult");
    
    
    // Useful Tests
    v4 Right = v4{1, 0, 0, 0};
    v4 Forward = v4{0, 0, 1, 0};
    v4 Up = v4{0, 1, 0, 0};
    v4 Left = v4{-1, 0, 0, 0};
    v4 Back = v4{0, 0, -1, 0};
    v4 Down = v4{0, -1, 0, 0};
    
    m44 NinetyDegreesAboutX = GetXRotation(M_PI / 2);
    v4 Rotated = NinetyDegreesAboutX * Forward;
    TestClean((Rotated == Up), "Rotation About X");
    
    m44 NinetyDegreesAboutY = GetYRotation(M_PI / 2);
    Rotated = NinetyDegreesAboutY * Right;
    TestClean((Rotated == Forward), "Rotation About Y");
    
    m44 NinetyDegreesAboutZ = GetZRotation(M_PI / 2);
    Rotated = NinetyDegreesAboutZ * Right;
    TestClean((Rotated == Down), "Rotation About Z");
    
    
    v4 A = v4{1, 2, 3, 4};
    m44 B = m44{
        1, 2, 3, 4,
        5, 6, 7, 8,
        9, 1, 2, 3,
        4, 5, 6, 7};
    v4 VTest = v4{30, 70, 29, 60};
    TestClean(((B * A) == VTest), "V4 M44 Multiplication");
    
    m44 C = m44{
        9, 8, 7, 6,
        5, 4, 3, 2,
        1, 0, 9, 8,
        7, 6, 5, 4
    };
    m44 MResult = B * C;
    m44 MTest = m44{
        50, 40, 60, 50,
        138, 112, 156, 130,
        109,  94,  99,  84, 
        116, 94, 132, 110 
    };
    TestClean(((B * C) == MTest), "M44 Mult Test 2");
    
    m44 Identity = M44Identity();
    m44 InvIdentity = {};
    Inverse(Identity, &InvIdentity);
    TestClean((Identity == InvIdentity), "Inverse Identity");
    
    m44 Test = m44{
        2, 4, 6, 7,
        5, 1, 8, 8,
        1, 7, 3, 1,
        3, 9, 2, 4
    };
    m44 PreCalcTestInv = m44{
        -0.3904761904761904762f, 0.26190476190476190475f, -0.02857142857142857139f, 0.16666666666666666668f,
        0.022222222222222222212f, -0.055555555555555555549f, 0.06666666666666666667f, 0.055555555555555555547f,
        -0.00317460317460317458f, 0.07936507936507936506f, 0.27619047619047619045f, -0.2222222222222222222f,
        0.24444444444444444444f, -0.1111111111111111111f, -0.26666666666666666667f, 0.1111111111111111111f
    };
    m44 InvTest = {};
    Inverse(Test, &InvTest);
    //TestClean((PreCalcTestInv == InvTest), "Inverse M44");
    
    DebugPrint("Results: Passed %d / %d\n\n\no", SuccessCount, TestCount);
}

#endif

#define GS_VECTOR_MATRIX_H
#endif