1 files changed, 99 insertions, 4 deletions

diff --git a/themes/mathlib.cpp b/themes/mathlib.cpp
index ee9bb1d..2e92aa8 100644
--- a/themes/mathlib.cpp
+++ b/themes/mathlib.cpp
@@ -80,10 +80,14 @@ Vector2 Vector2::rotate(float angle) {
     };
 }
 
-Vector2 Vector2::rotateAbout(float angle, Vector2 p) {
-    Vector2 prev = { x - p.x, y - p.y }; // Translate the point back to the origin
-    Vector2 rot = prev.rotate(angle);    // Do the rotation
-    return { rot.x + p.x, rot.y + p.y }; // Translate back to original position
+Vector2 Vector2::rotateAround(float angle, const Vector2& other) {
+    Vector2 point = { x - other.x, y - other.y };
+    point = {
+        point.x * cosf(angle) - point.y * sinf(angle),
+        point.x * sinf(angle) + point.y * cosf(angle)
+    };
+    point = point + other;
+    return point;
 }
 
 void Vector2::printDebug(const char* name) {
@@ -216,6 +220,10 @@ float Vector3::operator [](int index) {
 	}
 }
 
+void Vector2::operator=(const Vector4& other) {
+	x = other.x;
+	y = other.y;
+}
 
 void Vector3::printDebug(const char* name) {
     printf("%s=Vector3(%f, %f, %f)\n", name, x, y, z);
@@ -240,6 +248,20 @@ Vector4::Vector4(float inX, float inY, float inZ, float inW) {
     w = inW;
 }
 
+Vector4::Vector4(Vector2& v) {
+	x = v.x;
+	y = v.y;
+	z = 0;
+	w = 1;
+}
+
+Vector4::Vector4(Vector3& v) {
+	x = v.x;
+	y = v.y;
+	z = v.z;
+	w = 1;
+}
+
 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 };
@@ -304,6 +326,24 @@ Vector4 Vector4::cross(const Vector4& other) {
 	};
 }
 
+Vector4 lerp(Vector4 start, Vector4 end, float t) {
+    return (end - start) * t + start;
+}
+
+void Vector4::operator=(const Vector2& v2) {
+	x = v2.x;
+	y = v2.y;
+	z = 0;
+	w = 1;
+}
+
+void Vector4::operator=(const Vector3& v2) {
+	x = v2.x;
+	y = v2.y;
+	z = v2.z;
+	w = 1;	
+}
+
 Vector4 Vector4::operator+(const Vector4& v2) {
     return add(v2);
 }
@@ -348,6 +388,7 @@ void Vector4::printDebug(const char* name) {
     printf("%s=Vector4(%f, %f, %f, %f)\n", name, x, y, z, w);
 }
 
+
 // ***************************************
 // Mat4x4
 Mat4x4 Mat4x4::copy() {
@@ -533,6 +574,14 @@ void Mat4x4::print() {
 
 // ***************************************
 // Quaternion
+Quaternion::Quaternion() { };
+
+Quaternion::Quaternion(float inW, float inX, float inY, float inZ) {
+    w = inW;
+    x = inX;
+    y = inY;
+    z = inZ;
+}
 
 float Quaternion::operator [](int index) {
 	switch (index) {
@@ -624,6 +673,32 @@ Quaternion Quaternion::normalize() const {
 	};
 }
 
+/*Mat4x4 Quaternion::toMatrix() const {
+	return {
+		{
+			1 - 2 * (y * y - z * z), 
+			2 * (x * y - z * w),
+            2 * (x * z + w * y),
+            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
+		}
+	};
+}*/
+
 Mat4x4 Quaternion::toMatrix() const {
 	return {
 		{
@@ -663,3 +738,23 @@ Quaternion quaternionFromRotation(Vector3 axis, float angleRadians) {
         axis.z * sinHalfAngRad
     };
 }
+
+Quaternion quaternionFromEulerAngle(float yaw, float pitch, float roll) {
+    float cy = cosf(yaw * 0.5f);
+    float sy = sinf(yaw * 0.5f);
+    float cp = cosf(pitch * 0.5f);
+    float sp = sinf(pitch * 0.5f);
+    float cr = cosf(roll * 0.5f);
+    float sr = sinf(roll * 0.5f);
+
+    return {
+        cr * cp * cy + sr * sp * sy,
+        sr * cp * cy - cr * sp * sy,
+        cr * sp * cy + sr * cp * sy,
+        cr * cp * sy - sr * sp * cy
+    };
+}
+
+void Quaternion::printDebug(const char* name) {
+    printf("%s=Quaternion(%f, %f, %f, %f)\n", name, x, y, z, w);
+}