(mkosarek) better mathlib now containing quaternions

2 files changed, 449 insertions, 276 deletions

diff --git a/shared_cpp/mathlib.cpp b/shared_cpp/mathlib.cpp
index 8222d00..4dd3dd0 100644
--- a/shared_cpp/mathlib.cpp
+++ b/shared_cpp/mathlib.cpp
@@ -1,6 +1,8 @@
 #include "mathlib.h"
 #include <cstdlib>
 
+// ***************************************
+// Vector2
 Vector2 getRandomNormalVector2() {
     Vector2 retval = {
 		static_cast<float>(rand()) / static_cast<float>(RAND_MAX),
@@ -9,3 +11,397 @@ Vector2 getRandomNormalVector2() {
 
 	return retval.normalize();
 }
+
+Vector2 Vector2::operator+(Vector2 other) {
+    return { x + other.x, y + other.y };
+}
+
+Vector2& Vector2::operator+=(Vector2 other) {
+    x += other.x;
+    y += other.y;
+    return *this;
+}
+
+Vector2 Vector2::operator-(Vector2 other) {
+    return { x - other.x, y - other.y };
+}
+
+Vector2 Vector2::operator*(float s) {
+    return { x * s, y * s };
+}
+
+Vector2 Vector2::operator/(float s) {
+    return { x / s, y / s };
+}
+
+float Vector2::dot(Vector2 other) {
+    return x * other.x + y * other.y;
+}
+
+float Vector2::length() {
+    return sqrtf(x * x + y * y);
+}
+
+Vector2 Vector2::normalize() {
+    float len = length();
+    float inverseLength = len == 0 ? 1.0 : 1.0 / len;
+
+    return { x * inverseLength, y * inverseLength };
+}
+
+Vector2 Vector2::negate() {
+    return { -x, -y };
+}
+
+Vector2 Vector2::getPerp() {
+    return { y, -x };
+}
+
+Vector2 Vector2::rotate(float angle) {
+    return {
+        x * cosf(angle) - y * sinf(angle),
+        x * sinf(angle) + y * cosf(angle)
+    };
+}
+
+void Vector2::printDebug(const char* name) {
+    printf("%s=Vector2(%f, %f)\n", name, x, y);
+}
+
+
+// ***************************************
+// Vector3
+float Vector3::length() {
+    return sqrtf(x * x + y * y + z * z);
+}
+
+Vector3 Vector3::operator+(const Vector3& other) {
+    return { x + other.x, y + other.y, z + other.z };
+}
+
+
+// ***************************************
+// Vector4
+float Vector4::length() {
+    return sqrtf(x * x + y * y + z * z + w * w);
+}
+
+Vector4 Vector4::normalize() {
+    float len = length();
+    float inverseLength = len == 0 ? 1.0 : 1.0 / len;
+
+    return { x * inverseLength, y * inverseLength, z * inverseLength, w * inverseLength };
+}
+
+Vector4 Vector4::fromColor(float r, float g, float b, float a) {
+    float scale = 1.f / 255.f;
+    return { r * scale, g * scale, b * scale, a * scale };
+}
+
+Vector4 Vector4::toNormalizedColor() {
+    return fromColor(x, y, z, w);
+}
+
+
+// ***************************************
+// Mat4x4
+Mat4x4 Mat4x4::copy() {
+    Mat4x4 result;
+    memcpy(result.m, m, sizeof(float) * 16);
+    return result;
+}
+
+Mat4x4 Mat4x4::scale(Vector3 v) {
+    Mat4x4 result = copy();
+    result.m[0] = result.m[0] * v.x;
+    result.m[5] = result.m[5] *v.y;
+    result.m[10] = result.m[10] * v.z;
+    return result;
+}
+
+Mat4x4 Mat4x4::translate(Vector3 v) {
+    Mat4x4 result = copy();
+    result.m[12] += v.x;
+    result.m[13] += v.y;
+    result.m[14] += v.z;
+    return result;
+}
+
+Mat4x4 Mat4x4::translateByVec2(Vector2 v) {
+    Mat4x4 result = copy();
+    result.m[12] += v.x;
+    result.m[13] += v.y;
+    return result;
+}
+
+Mat4x4 Mat4x4::rotate2D(float angle) {
+    Mat4x4 result = copy();
+    result.m[0] = cos(angle);
+    result.m[1] = -sin(angle);
+    result.m[4] = sin(angle);
+    result.m[5] = cos(angle);
+    return result;
+}
+
+Mat4x4 Mat4x4::getXRotationMatrix(float angleRadians) {
+    return {
+        { 1, 0, 0, 0,
+          0, cos(angleRadians), -sin(angleRadians), 0,
+          0, sin(angleRadians), cos(angleRadians), 0,
+          0, 0, 0, 1 }
+    };
+}
+
+Mat4x4 Mat4x4::getYRotationMatrix(float angleRadians) {
+    return {
+        { cos(angleRadians), 0, sin(angleRadians), 0,
+          0, 1, 0, 0,
+          -sin(angleRadians), 0, cos(angleRadians), 0,
+          0, 0, 0, 1 }
+    };
+}
+
+Mat4x4 Mat4x4::getZRotationMatrix(float angleRadians) {
+    return {
+        { cos(angleRadians), -sin(angleRadians), 0, 0,
+          sin(angleRadians), cos(angleRadians), 0, 0,
+          0, 0, 1, 0,
+          0, 0, 0, 1 }
+    };
+}
+
+Mat4x4 Mat4x4::rotate(float xRadians, float yRadians, float zRadians) {
+    Mat4x4 result = copy();
+        
+    Mat4x4 rotationMatrix;
+    if (xRadians != 0) {
+        rotationMatrix = getXRotationMatrix(xRadians);
+        result = result * rotationMatrix;
+    }
+
+    if (yRadians != 0) {
+        rotationMatrix = getYRotationMatrix(yRadians);
+        result = result * rotationMatrix;
+    }
+
+    if (zRadians != 0) {
+        rotationMatrix = getZRotationMatrix(zRadians);
+        result = result * rotationMatrix;
+    }
+
+    return result;
+}
+
+Vector2 Mat4x4::multByVec2(Vector2 v) {
+    Vector4 vec4 = { v.x, v.y, 0.0, 1.0 };
+    return {
+        vec4.x * m[0] + vec4.y * m[4] + vec4.z * m[8] + vec4.w * m[12],
+        vec4.x * m[1] + vec4.y * m[5] + vec4.z * m[9] + vec4.w * m[13]
+    };
+}
+
+Vector2 Mat4x4::operator*(Vector2 v) {
+    return multByVec2(v);
+}
+
+Mat4x4 Mat4x4::multMat4x4(const Mat4x4& other) {
+    Mat4x4 result;
+    for (int i = 0; i < 4; ++i) {
+        for (int j = 0; j < 4; ++j) {
+            int row = i * 4;
+            result.m[row + j] = m[row + 0] * other.m[0 + j] + m[row + 1] * other.m[4 + j] + m[row + 2] * other.m[8 + j] + m[row + 3] * other.m[12 + j];
+        }
+    }
+
+    return result;
+}
+
+Mat4x4 Mat4x4::operator*(const Mat4x4& other) {
+    return multMat4x4(other);
+}
+
+Mat4x4 Mat4x4::getOrthographicMatrix(float left, float right, float bottom, float top) {
+    Mat4x4 result;
+    result.m[0] = 2.0 / (right - left);
+    result.m[5] = 2.0 / (top - bottom);
+    result.m[10] = 1.0;
+    result.m[12] = -(right + left) / (right - left);
+    result.m[13] = -(top + bottom) / (top - bottom);
+    return result;
+}
+
+Mat4x4 Mat4x4::inverse() {
+    Mat4x4 inv;
+
+    inv.m[0] = m[5] * m[10] * m[15] - m[5] * m[11] * m[14] - m[9] * m[6] * m[15] + m[9] * m[7] * m[14] + m[13] * m[6] * m[11] - m[13] * m[7] * m[10];
+    inv.m[4] = -m[4] * m[10] * m[15] + m[4] * m[11] * m[14] + m[8] * m[6] * m[15] - m[8] * m[7] * m[14] - m[12] * m[6] * m[11] + m[12] * m[7] * m[10];
+    inv.m[8] = m[4] * m[9] * m[15] - m[4] * m[11] * m[13] - m[8] * m[5] * m[15] + m[8] * m[7] * m[13] + m[12] * m[5] * m[11] - m[12] * m[7] * m[9];
+    inv.m[12] = -m[4] * m[9] * m[14] + m[4] * m[10] * m[13] + m[8] * m[5] * m[14] - m[8] * m[6] * m[13] - m[12] * m[5] * m[10] + m[12] * m[6] * m[9];
+    inv.m[1] = -m[1] * m[10] * m[15] + m[1] * m[11] * m[14] + m[9] * m[2] * m[15] - m[9] * m[3] * m[14] - m[13] * m[2] * m[11] + m[13] * m[3] * m[10];
+    inv.m[5] = m[0] * m[10] * m[15] - m[0] * m[11] * m[14] - m[8] * m[2] * m[15] + m[8] * m[3] * m[14] + m[12] * m[2] * m[11] - m[12] * m[3] * m[10];
+    inv.m[9] = -m[0] * m[9] * m[15] + m[0] * m[11] * m[13] + m[8] * m[1] * m[15] - m[8] * m[3] * m[13] - m[12] * m[1] * m[11] + m[12] * m[3] * m[9];
+    inv.m[13] = m[0] * m[9] * m[14] - m[0] * m[10] * m[13] - m[8] * m[1] * m[14] + m[8] * m[2] * m[13] + m[12] * m[1] * m[10] - m[12] * m[2] * m[9];
+    inv.m[2] = m[1] * m[6] * m[15] - m[1] * m[7] * m[14] - m[5] * m[2] * m[15] + m[5] * m[3] * m[14] + m[13] * m[2] * m[7] - m[13] * m[3] * m[6];
+    inv.m[6] = -m[0] * m[6] * m[15] + m[0] * m[7] * m[14] + m[4] * m[2] * m[15] - m[4] * m[3] * m[14] - m[12] * m[2] * m[7] + m[12] * m[3] * m[6];
+    inv.m[10] = m[0] * m[5] * m[15] - m[0] * m[7] * m[13] - m[4] * m[1] * m[15] + m[4] * m[3] * m[13] + m[12] * m[1] * m[7] - m[12] * m[3] * m[5];
+    inv.m[14] = -m[0] * m[5] * m[14] + m[0] * m[6] * m[13] + m[4] * m[1] * m[14] - m[4] * m[2] * m[13] - m[12] * m[1] * m[6] + m[12] * m[2] * m[5];
+    inv.m[3] = -m[1] * m[6] * m[11] + m[1] * m[7] * m[10] + m[5] * m[2] * m[11] - m[5] * m[3] * m[10] - m[9] * m[2] * m[7] + m[9] * m[3] * m[6];
+    inv.m[7] = m[0] * m[6] * m[11] - m[0] * m[7] * m[10] - m[4] * m[2] * m[11] + m[4] * m[3] * m[10] + m[8] * m[2] * m[7] - m[8] * m[3] * m[6];
+    inv.m[11] = -m[0] * m[5] * m[11] + m[0] * m[7] * m[9] + m[4] * m[1] * m[11] - m[4] * m[3] * m[9] - m[8] * m[1] * m[7] + m[8] * m[3] * m[5];
+    inv.m[15] = m[0] * m[5] * m[10] - m[0] * m[6] * m[9] - m[4] * m[1] * m[10] + m[4] * m[2] * m[9] + m[8] * m[1] * m[6] - m[8] * m[2] * m[5];
+
+    float det = m[0] * inv.m[0] + m[1] * inv.m[4] + m[2] * inv.m[8] + m[3] * inv.m[12];
+
+    if (det == 0)
+        return Mat4x4();
+
+    det = 1.f / det;
+
+    for (int i = 0; i < 16; i++)
+        inv.m[i] = inv.m[i] * det;
+
+    return inv;
+}
+
+Mat4x4 Mat4x4::getPerspectiveProjection(float near, float far, float fieldOfViewRadians, float aspectRatio) {
+    float halfFieldOfView = fieldOfViewRadians * 0.5f;
+    float top = tan(halfFieldOfView) * near;
+    float bottom = -top;
+    float right = top * aspectRatio;
+    float left = -right;
+
+    return {
+        { (2 * near) / (right - left), 0, 0, 0,
+          0, (2 * near) / (top - bottom), 0, 0,
+          (right + left) / (right - left), (top + bottom) / (top - bottom), -(far + near) / (far - near), -1,
+          0, 0, (-2 * far * near) / (far - near), 0 }
+    };
+}
+
+void Mat4x4::print() {
+    printf("[ ");
+    for (int idx = 0; idx < 16; idx++) {
+        printf("%f, ", m[idx]);
+    }
+    printf(" ]\n");
+}
+
+// ***************************************
+// Quaternion
+
+float Quaternion::operator [](int index) {
+	switch (index) {
+	case 0:
+		return x;
+	case 1:
+		return y;
+	case 2:
+		return z;
+	case 3:
+		return w;
+	default:
+		return 0;
+	}
+}
+
+Quaternion Quaternion::operator*(const Quaternion& other) const {
+	return {
+		w * other.w - x * other.x - y * other.y - z * other.z,  // w
+        w * other.x + x * other.w + y * other.z - z * other.y,  // i
+        w * other.y - x * other.z + y * other.w + z * other.x,  // j
+        w * other.z + x * other.y - y * other.x + z * other.w   // k
+	};
+}
+
+Quaternion Quaternion::operator*(const float& scale) const {
+	return {
+		w * scale,
+		x * scale,
+		y * scale,
+		z * scale
+	};
+}
+
+Quaternion Quaternion::operator+(const Quaternion& other) const {
+	return {
+		w + other.w,
+		x + other.x,
+		y + other.y,
+		z + other.z
+	};
+}
+
+Quaternion Quaternion::operator-(const Quaternion& other) const {
+	return {
+		w - other.w,
+		x - other.x,
+		y - other.y,
+		z - other.z
+	};
+}
+
+const float DOT_THRESHOLD = 0.9999f; 
+
+// Using a slerp here, mostly taken from here: http://number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/.
+// As JBlow says, it's expensive as heck. I will address this if it becomes a problem.
+Quaternion Quaternion::interpolate(const Quaternion& other, const float factor) {
+	Quaternion newOther = other;
+	float dotProduct = dot(other);
+
+	if (dotProduct < 0) {
+		newOther = other * -1;
+		dotProduct *= -1;
+	}
+
+	if (dotProduct > DOT_THRESHOLD) {
+		return (*this + ((newOther - *this) * factor)).normalize();
+	}
+
+	float thetaZero = acos(dotProduct); // angle between input vectors
+	float theta = thetaZero * factor;
+
+	Quaternion v2 = (newOther - (*this * dotProduct)).normalize();
+
+	return (*this * cos(theta)) + (v2 * sin(theta));
+}
+
+float Quaternion::length() const {
+	return sqrtf(x * x + y * y + z * z + w * w);
+}
+
+Quaternion Quaternion::normalize() const {
+	float l = length();
+	return {
+		w / l,
+		x / l,
+		y / l,
+		z / l,
+	};
+}
+
+Mat4x4 Quaternion::toMatrix() const {
+	return {
+		{
+			1 - 2 * (y * y +  z * z), 
+			2 * (x * y + z * w),
+			2 * (x * z - y * w),
+			0,
+			2 * (x * y - w * z),
+			1 - 2 * (x * x + z * z),
+			2 * (y * z + w * x),
+			0,
+			2 * (x * z + w * y),
+			2 * (y * z - w * x),
+			1 - 2 * (x * x + y * y),
+			0,
+			0,
+			0,
+			0,
+			1
+		}
+	};
+}
+
+float Quaternion::dot(const Quaternion& second) const {
+	return w * second.w + x * second.x + y * second.y + z * second.z;
+}
diff --git a/shared_cpp/mathlib.h b/shared_cpp/mathlib.h
index fc2ef0c..cff9ece 100644
--- a/shared_cpp/mathlib.h
+++ b/shared_cpp/mathlib.h
@@ -13,75 +13,19 @@ struct Vector2 {
     float x = 0;
     float y = 0;
 
-    Vector2 operator+(Vector2 other) {
-        return { x + other.x, y + other.y };
-    }
-
-    Vector2& operator+=(Vector2 other) {
-        x += other.x;
-        y += other.y;
-        return *this;
-    }
-
-    Vector2 operator-(Vector2 other) {
-        return { x - other.x, y - other.y };
-    }
-
-    Vector2 operator*(float s) {
-        return { x * s, y * s };
-    }
-
-    Vector2 operator/(float s) {
-        return { x / s, y / s };
-    }
-
-    float dot(Vector2 other) {
-        return x * other.x + y * other.y;
-    }
-
-    float length() {
-        return sqrtf(x * x + y * y);
-    }
-
-    Vector2 normalize() {
-        float len = length();
-        float inverseLength = len == 0 ? 1.0 : 1.0 / len;
-
-        return { x * inverseLength, y * inverseLength };
-    }
-
-    Vector2 negate() {
-        return { -x, -y };
-    }
-
-    Vector2 getPerp() {
-        return { y, -x };
-    }
-
-    Vector2 rotate(float angle) {
-        return {
-            x * cosf(angle) - y * sinf(angle),
-            x * sinf(angle) + y * cosf(angle)
-        };
-    }
-
-    void printDebug(const char* name) {
-        printf("%s=Vector2(%f, %f)\n", name, x, y);
-    }
-
-    float determinant(Vector2 other) {
-        //
-        // [ a b ]
-        // [ c d ]
-        //
-        // [ x other.x ]
-        // [ y other.y ]
-        //
-        // det = a * d - b * c
-        // det = x * other.y - other.x * y
-        //
-        return x * other.y - other.x * y;
-    }
+    Vector2 operator+(Vector2 other);
+    Vector2& operator+=(Vector2 other);
+    Vector2 operator-(Vector2 other);
+    Vector2 operator*(float s);
+    Vector2 operator/(float s);
+    float dot(Vector2 other);
+    float length();
+    Vector2 normalize();
+    Vector2 negate();
+    Vector2 getPerp();
+    Vector2 rotate(float angle);
+    void printDebug(const char* name);
+    float determinant(Vector2 other);
 };
 
 struct Vector3 {
@@ -89,13 +33,8 @@ struct Vector3 {
     float y = 0.f;
     float z = 0.f;
 
-    float length() {
-		return sqrtf(x * x + y * y + z * z);
-	}
-
-    Vector3 operator+(const Vector3& other) {
-        return { x + other.x, y + other.y, z + other.z };
-    }
+    float length();
+    Vector3 operator+(const Vector3& other);
 };
 
 struct Vector4 {
@@ -104,25 +43,10 @@ struct Vector4 {
     float z = 0.f;
     float w = 0.f;
 
-    float length() {
-        return sqrtf(x * x + y * y + z * z + w * w);
-    }
-
-    Vector4 normalize() {
-        float len = length();
-        float inverseLength = len == 0 ? 1.0 : 1.0 / len;
-
-        return { x * inverseLength, y * inverseLength, z * inverseLength, w * inverseLength };
-    }
-
-    Vector4 fromColor(float r, float g, float b, float a) {
-        float scale = 1.f / 255.f;
-        return { r * scale, g * scale, b * scale, a * scale };
-    }
-
-	Vector4 toNormalizedColor() {
-		return fromColor(x, y, z, w);
-	}
+    float length();
+    Vector4 normalize();
+    Vector4 fromColor(float r, float g, float b, float a);
+	Vector4 toNormalizedColor();
 };
 
 struct Mat4x4 {
@@ -133,186 +57,39 @@ struct Mat4x4 {
         0, 0, 0, 1
     };
 
-    Mat4x4 copy() {
-        Mat4x4 result;
-        memcpy(result.m, m, sizeof(float) * 16);
-        return result;
-    }
-
-    Mat4x4 scale(Vector3 v) {
-        Mat4x4 result = copy();
-        result.m[0] = result.m[0] * v.x;
-        result.m[5] = result.m[5] *v.y;
-        result.m[10] = result.m[10] * v.z;
-        return result;
-    }
-
-    Mat4x4 translate(Vector3 v) {
-        Mat4x4 result = copy();
-        result.m[12] += v.x;
-        result.m[13] += v.y;
-        result.m[14] += v.z;
-        return result;
-    }
-
-	Mat4x4 translateByVec2(Vector2 v) {
-        Mat4x4 result = copy();
-        result.m[12] += v.x;
-        result.m[13] += v.y;
-        return result;
-    }
-
-    Mat4x4 rotate2D(float angle) {
-        Mat4x4 result = copy();
-        result.m[0] = cos(angle);
-        result.m[1] = -sin(angle);
-        result.m[4] = sin(angle);
-        result.m[5] = cos(angle);
-        return result;
-    }
-
-    Mat4x4 getXRotationMatrix(float angleRadians) {
-        return {
-            { 1, 0, 0, 0,
-              0, cos(angleRadians), -sin(angleRadians), 0,
-              0, sin(angleRadians), cos(angleRadians), 0,
-              0, 0, 0, 1 }
-        };
-    }
-
-    Mat4x4 getYRotationMatrix(float angleRadians) {
-        return {
-            { cos(angleRadians), 0, sin(angleRadians), 0,
-              0, 1, 0, 0,
-              -sin(angleRadians), 0, cos(angleRadians), 0,
-              0, 0, 0, 1 }
-        };
-    }
-
-    Mat4x4 getZRotationMatrix(float angleRadians) {
-        return {
-            { cos(angleRadians), -sin(angleRadians), 0, 0,
-              sin(angleRadians), cos(angleRadians), 0, 0,
-              0, 0, 1, 0,
-              0, 0, 0, 1 }
-        };
-    }
-
-    Mat4x4 rotate(float xRadians, float yRadians, float zRadians) {
-        Mat4x4 result = copy();
-        
-        Mat4x4 rotationMatrix;
-        if (xRadians != 0) {
-            rotationMatrix = getXRotationMatrix(xRadians);
-            result = result * rotationMatrix;
-        }
-
-        if (yRadians != 0) {
-            rotationMatrix = getYRotationMatrix(yRadians);
-            result = result * rotationMatrix;
-        }
-
-        if (zRadians != 0) {
-            rotationMatrix = getZRotationMatrix(zRadians);
-            result = result * rotationMatrix;
-        }
-
-        return result;
-    }
-
-    Vector2 multByVec2(Vector2 v) {
-        Vector4 vec4 = { v.x, v.y, 0.0, 1.0 };
-        return {
-            vec4.x * m[0] + vec4.y * m[4] + vec4.z * m[8] + vec4.w * m[12],
-            vec4.x * m[1] + vec4.y * m[5] + vec4.z * m[9] + vec4.w * m[13]
-        };
-    }
-
-    Vector2 operator*(Vector2 v) {
-        return multByVec2(v);
-    }
-
-    Mat4x4 multMat4x4(const Mat4x4& other) {
-        Mat4x4 result;
-        for (int i = 0; i < 4; ++i) {
-            for (int j = 0; j < 4; ++j) {
-                int row = i * 4;
-                result.m[row + j] = m[row + 0] * other.m[0 + j] + m[row + 1] * other.m[4 + j] + m[row + 2] * other.m[8 + j] + m[row + 3] * other.m[12 + j];
-            }
-        }
-
-        return result;
-    }
-
-    Mat4x4 operator*(const Mat4x4& other) {
-        return multMat4x4(other);
-    }
-
-    Mat4x4 getOrthographicMatrix(float left, float right, float bottom, float top) {
-        Mat4x4 result;
-        result.m[0] = 2.0 / (right - left);
-        result.m[5] = 2.0 / (top - bottom);
-        result.m[10] = 1.0;
-        result.m[12] = -(right + left) / (right - left);
-        result.m[13] = -(top + bottom) / (top - bottom);
-        return result;
-    }
-
-    Mat4x4 inverse() {
-        Mat4x4 inv;
-
-        inv.m[0] = m[5] * m[10] * m[15] - m[5] * m[11] * m[14] - m[9] * m[6] * m[15] + m[9] * m[7] * m[14] + m[13] * m[6] * m[11] - m[13] * m[7] * m[10];
-        inv.m[4] = -m[4] * m[10] * m[15] + m[4] * m[11] * m[14] + m[8] * m[6] * m[15] - m[8] * m[7] * m[14] - m[12] * m[6] * m[11] + m[12] * m[7] * m[10];
-        inv.m[8] = m[4] * m[9] * m[15] - m[4] * m[11] * m[13] - m[8] * m[5] * m[15] + m[8] * m[7] * m[13] + m[12] * m[5] * m[11] - m[12] * m[7] * m[9];
-        inv.m[12] = -m[4] * m[9] * m[14] + m[4] * m[10] * m[13] + m[8] * m[5] * m[14] - m[8] * m[6] * m[13] - m[12] * m[5] * m[10] + m[12] * m[6] * m[9];
-        inv.m[1] = -m[1] * m[10] * m[15] + m[1] * m[11] * m[14] + m[9] * m[2] * m[15] - m[9] * m[3] * m[14] - m[13] * m[2] * m[11] + m[13] * m[3] * m[10];
-        inv.m[5] = m[0] * m[10] * m[15] - m[0] * m[11] * m[14] - m[8] * m[2] * m[15] + m[8] * m[3] * m[14] + m[12] * m[2] * m[11] - m[12] * m[3] * m[10];
-        inv.m[9] = -m[0] * m[9] * m[15] + m[0] * m[11] * m[13] + m[8] * m[1] * m[15] - m[8] * m[3] * m[13] - m[12] * m[1] * m[11] + m[12] * m[3] * m[9];
-        inv.m[13] = m[0] * m[9] * m[14] - m[0] * m[10] * m[13] - m[8] * m[1] * m[14] + m[8] * m[2] * m[13] + m[12] * m[1] * m[10] - m[12] * m[2] * m[9];
-        inv.m[2] = m[1] * m[6] * m[15] - m[1] * m[7] * m[14] - m[5] * m[2] * m[15] + m[5] * m[3] * m[14] + m[13] * m[2] * m[7] - m[13] * m[3] * m[6];
-        inv.m[6] = -m[0] * m[6] * m[15] + m[0] * m[7] * m[14] + m[4] * m[2] * m[15] - m[4] * m[3] * m[14] - m[12] * m[2] * m[7] + m[12] * m[3] * m[6];
-        inv.m[10] = m[0] * m[5] * m[15] - m[0] * m[7] * m[13] - m[4] * m[1] * m[15] + m[4] * m[3] * m[13] + m[12] * m[1] * m[7] - m[12] * m[3] * m[5];
-        inv.m[14] = -m[0] * m[5] * m[14] + m[0] * m[6] * m[13] + m[4] * m[1] * m[14] - m[4] * m[2] * m[13] - m[12] * m[1] * m[6] + m[12] * m[2] * m[5];
-        inv.m[3] = -m[1] * m[6] * m[11] + m[1] * m[7] * m[10] + m[5] * m[2] * m[11] - m[5] * m[3] * m[10] - m[9] * m[2] * m[7] + m[9] * m[3] * m[6];
-        inv.m[7] = m[0] * m[6] * m[11] - m[0] * m[7] * m[10] - m[4] * m[2] * m[11] + m[4] * m[3] * m[10] + m[8] * m[2] * m[7] - m[8] * m[3] * m[6];
-        inv.m[11] = -m[0] * m[5] * m[11] + m[0] * m[7] * m[9] + m[4] * m[1] * m[11] - m[4] * m[3] * m[9] - m[8] * m[1] * m[7] + m[8] * m[3] * m[5];
-        inv.m[15] = m[0] * m[5] * m[10] - m[0] * m[6] * m[9] - m[4] * m[1] * m[10] + m[4] * m[2] * m[9] + m[8] * m[1] * m[6] - m[8] * m[2] * m[5];
-
-        float det = m[0] * inv.m[0] + m[1] * inv.m[4] + m[2] * inv.m[8] + m[3] * inv.m[12];
-
-        if (det == 0)
-            return Mat4x4();
-
-        det = 1.f / det;
-
-        for (int i = 0; i < 16; i++)
-            inv.m[i] = inv.m[i] * det;
-
-        return inv;
-    }
-
-	Mat4x4 getPerspectiveProjection(float near, float far, float fieldOfViewRadians, float aspectRatio) {
-		float halfFieldOfView = fieldOfViewRadians * 0.5f;
-		float top = tan(halfFieldOfView) * near;
-		float bottom = -top;
-		float right = top * aspectRatio;
-		float left = -right;
-
-		return {
-			{ (2 * near) / (right - left), 0, 0, 0,
-			  0, (2 * near) / (top - bottom), 0, 0,
-			  (right + left) / (right - left), (top + bottom) / (top - bottom), -(far + near) / (far - near), -1,
-			  0, 0, (-2 * far * near) / (far - near), 0 }
-		};
-	}
-
-	void print() {
-		printf("[ ");
-		for (int idx = 0; idx < 16; idx++) {
-			printf("%f, ", m[idx]);
-		}
-		printf(" ]\n");
-	}
+    Mat4x4 copy();
+    Mat4x4 scale(Vector3 v);
+    Mat4x4 translate(Vector3 v);
+	Mat4x4 translateByVec2(Vector2 v);
+    Mat4x4 rotate2D(float angle);
+    Mat4x4 getXRotationMatrix(float angleRadians);
+    Mat4x4 getYRotationMatrix(float angleRadians);
+    Mat4x4 getZRotationMatrix(float angleRadians);
+    Mat4x4 rotate(float xRadians, float yRadians, float zRadians);
+    Vector2 multByVec2(Vector2 v);
+    Vector2 operator*(Vector2 v);
+    Mat4x4 multMat4x4(const Mat4x4& other);
+    Mat4x4 operator*(const Mat4x4& other);
+    Mat4x4 getOrthographicMatrix(float left, float right, float bottom, float top);
+    Mat4x4 inverse();
+	Mat4x4 getPerspectiveProjection(float near, float far, float fieldOfViewRadians, float aspectRatio);
+	void print();
 };
 
-
+struct Quaternion {
+    float x = 0;
+    float y = 0;
+    float z = 0;
+    float w = 0;
+
+    float operator [](int index);
+	Quaternion operator*(const Quaternion& other) const;
+	Quaternion operator*(const float& scale) const;
+	Quaternion operator+(const Quaternion& other) const;
+	Quaternion operator-(const Quaternion& other) const;
+	Quaternion interpolate(const Quaternion& other, const float factor);
+	Mat4x4 toMatrix() const;
+	Quaternion normalize() const;
+	float length() const;
+	float dot(const Quaternion& other) const;
+};