1 files changed, 54 insertions, 0 deletions
diff --git a/shared_cpp/mathlib.cpp b/shared_cpp/mathlib.cpp
index fb09cd9..3402f0e 100644
--- a/shared_cpp/mathlib.cpp
+++ b/shared_cpp/mathlib.cpp
@@ -521,6 +521,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) {
@@ -612,6 +620,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 {
 		{
@@ -651,3 +685,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);
+}