Updating the mathlib

3 files changed, 122 insertions, 11 deletions

diff --git a/themes/TreeShape.cpp b/themes/TreeShape.cpp
index 08170ea..1680c55 100644
--- a/themes/TreeShape.cpp
+++ b/themes/TreeShape.cpp
@@ -6,10 +6,10 @@
 #include <ctime>
 
 void TreeBranchLoadData::fillVertices(Renderer2dVertex* vertices, int branchTier) {
-    bottomLeft = Vector2 { position.x - width / 2.f, position.y }.rotateAbout(rotation, position);
-    bottomRight = Vector2 { position.x + width / 2.f, position.y }.rotateAbout(rotation, position);
-    topLeft = (Vector2 { position.x - width / 2.f, position.y + height }).rotateAbout(rotation, position);
-    topRight = (Vector2 { position.x + width / 2.f, position.y + height }).rotateAbout(rotation, position);
+    bottomLeft = Vector2 { position.x - width / 2.f, position.y }.rotateAround(rotation, position);
+    bottomRight = Vector2 { position.x + width / 2.f, position.y }.rotateAround(rotation, position);
+    topLeft = (Vector2 { position.x - width / 2.f, position.y + height }).rotateAround(rotation, position);
+    topRight = (Vector2 { position.x + width / 2.f, position.y + height }).rotateAround(rotation, position);
 
     topMidpoint = topLeft + (topRight - topLeft) / 2.f;
 
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);
+}
diff --git a/themes/mathlib.h b/themes/mathlib.h
index 82f85a2..f89addc 100644
--- a/themes/mathlib.h
+++ b/themes/mathlib.h
@@ -18,9 +18,13 @@
 #define ABS(x) (x < 0 ? -x : x)
 #define SIGN(x) (x < 0 ? -1 : 1)
 #define PI 3.141592653589793238463
+#define E 2.71828182845904523536
 #define DEG_TO_RAD(x) (x * (PI / 180.f))
 #define RAD_TO_DEG(x) (x * (180.f / PI))
 
+struct Vector4;
+
+
 // -- Random
 inline float randomFloatBetween(float min, float max) {
     float random = static_cast<float>(rand()) / static_cast<float>(RAND_MAX);
@@ -36,19 +40,20 @@ struct Vector2 {
     float y = 0;
 
     Vector2();
-    Vector2(float x, float y);
+    Vector2(float inX, float inY);
     Vector2 operator+(Vector2 other);
     Vector2& operator+=(Vector2 other);
     Vector2 operator-(Vector2 other);
     Vector2 operator*(float s);
     Vector2 operator/(float s);
+	void operator=(const Vector4& other);
     float dot(Vector2 other);
     float length();
     Vector2 normalize();
     Vector2 negate();
     Vector2 getPerp();
     Vector2 rotate(float angle);
-    Vector2 rotateAbout(float angle, Vector2 p);
+    Vector2 rotateAround(float angle, const Vector2& other);
     float determinant(Vector2 other);
     
     void printDebug(const char* name);
@@ -95,6 +100,8 @@ struct Vector4 {
     Vector4(float inX, float inY, float inZ, float inW);
     Vector4 fromColor(float r, float g, float b, float a);
 	Vector4 toNormalizedColor();
+	Vector4(Vector2& v);
+	Vector4(Vector3& v);
 
     float length();
     Vector4 normalize();
@@ -105,6 +112,8 @@ struct Vector4 {
     Vector4 negate();
     Vector4 cross(const Vector4& other);
 
+	void operator=(const Vector2& v2);
+    void operator=(const Vector3& v2);
     Vector4 operator+(const Vector4& v2);
     Vector4 operator-(const Vector4& v2);
     Vector4 operator-();
@@ -115,6 +124,8 @@ struct Vector4 {
     void printDebug(const char* name);
 };
 
+Vector4 lerp(Vector4 start, Vector4 end, float t);
+
 struct Mat4x4 {
     float m[16] = {
         1, 0, 0, 0,
@@ -143,11 +154,13 @@ struct Mat4x4 {
 };
 
 struct Quaternion {
-    float w = 0;
+    float w = 1;
     float x = 0;
     float y = 0;
     float z = 0;
 
+    Quaternion();
+    Quaternion(float inW, float inX, float inY, float inZ);
     float operator [](int index);
 	Quaternion operator*(const Quaternion& other) const;
 	Quaternion operator*(const float& scale) const;
@@ -158,6 +171,9 @@ struct Quaternion {
 	Quaternion normalize() const;
 	float length() const;
 	float dot(const Quaternion& other) const;
+
+    void printDebug(const char* name);
 };
 
 Quaternion quaternionFromRotation(Vector3 axis, float angleRadians);
+Quaternion quaternionFromEulerAngle(float yaw, float pitch, float roll);