From 4b3fe2b9e27e523ea2aec2f6030ee82f3c0399b8 Mon Sep 17 00:00:00 2001
From: josh <josh@stepbro.wtf>
Date: Thu, 25 Apr 2024 16:02:32 -0400
Subject: [PATCH] Added Quaternion constructors, SetFrom(AxisAngle)

---
 README.md                               |  2 +-
 include/J3ML/LinearAlgebra/AxisAngle.h  |  1 +
 include/J3ML/LinearAlgebra/Quaternion.h | 19 +++++++++++++++
 src/J3ML/LinearAlgebra/Quaternion.cpp   | 31 +++++++++++++++++++++++++
 4 files changed, 52 insertions(+), 1 deletion(-)

diff --git a/README.md b/README.md
index 9029a71..681f239 100644
--- a/README.md
+++ b/README.md
@@ -1,5 +1,5 @@
 # Josh's 3D Math Library - J3ML
-===============================
+
 
 Yet Another C++ Math Standard
 
diff --git a/include/J3ML/LinearAlgebra/AxisAngle.h b/include/J3ML/LinearAlgebra/AxisAngle.h
index 1e54e1b..05be469 100644
--- a/include/J3ML/LinearAlgebra/AxisAngle.h
+++ b/include/J3ML/LinearAlgebra/AxisAngle.h
@@ -11,6 +11,7 @@ namespace J3ML::LinearAlgebra
     /// Transitional datatype, not useful for internal representation of rotation
     /// But has uses for conversion and manipulation.
     class AxisAngle {
+    public:
         Vector3 axis;
         float angle;
     public:
diff --git a/include/J3ML/LinearAlgebra/Quaternion.h b/include/J3ML/LinearAlgebra/Quaternion.h
index a1277e3..016014f 100644
--- a/include/J3ML/LinearAlgebra/Quaternion.h
+++ b/include/J3ML/LinearAlgebra/Quaternion.h
@@ -38,10 +38,13 @@ namespace J3ML::LinearAlgebra
         //void Inverse();
 
         explicit Quaternion(Vector4 vector4);
+        explicit Quaternion(const EulerAngle& angle);
+        explicit Quaternion(const AxisAngle& angle);
 
         void SetFromAxisAngle(const Vector3 &vector3, float between);
 
         void SetFromAxisAngle(const Vector4 &vector4, float between);
+        void SetFrom(const AxisAngle& angle);
 
         Quaternion Inverse() const;
 
@@ -58,6 +61,22 @@ namespace J3ML::LinearAlgebra
 
         float GetAngle() const;
 
+        EulerAngle ToEulerAngle() const
+        {
+            // roll (x-axis rotation)
+            double sinr_cosp = 2 * (w * x + y * z);
+            double cosr_cosp = 1 - 2 * (x * x + y * y);
+
+            // pitch (y-axis rotation)
+            double sinp = std::sqrt(1 + 2 * (w * y - x * z));
+            double
+
+            return {
+                .roll = std::atan2(sinr_cosp, cosr_cosp),
+                .pitch =
+            };
+        }
+
 
         Matrix3x3 ToMatrix3x3() const;
 
diff --git a/src/J3ML/LinearAlgebra/Quaternion.cpp b/src/J3ML/LinearAlgebra/Quaternion.cpp
index 8fb9b53..37e89bd 100644
--- a/src/J3ML/LinearAlgebra/Quaternion.cpp
+++ b/src/J3ML/LinearAlgebra/Quaternion.cpp
@@ -201,4 +201,35 @@ namespace J3ML::LinearAlgebra {
     Quaternion::Quaternion(const Vector4 &rotationAxis, float rotationAngleBetween) {
         SetFromAxisAngle(rotationAxis, rotationAngleBetween);
     }
+
+    Quaternion::Quaternion(const AxisAngle &angle) {
+        double s = std::sin(angle.angle / 2);
+        x = angle.axis.x * s;
+        y = angle.axis.y * s;
+        z = angle.axis.z * s;
+        w = std::cos(angle.angle / 2);
+    }
+
+    Quaternion::Quaternion(const EulerAngle &angle) {
+        // Abbreviations for the various angular functions
+        double cr = std::cos(angle.roll * 0.5);
+        double sr = std::sin(angle.roll * 0.5);
+        double cp = std::cos(angle.pitch * 0.5);
+        double sp = std::sin(angle.pitch * 0.5);
+        double cy = std::cos(angle.yaw * 0.5);
+        double sy = std::sin(angle.yaw * 0.5);
+
+        w = cr * cp * cy + sr * sp * sy;
+        x = sr * cp * cy - cr * sp * sy;
+        y = cr * sp * cy + sr * cp * sy;
+        z = cr * cp * sy - sr * sp * cy;
+    }
+
+    void Quaternion::SetFrom(const AxisAngle &angle) {
+        double s = std::sin(angle.angle / 2);
+        x = angle.axis.x * s;
+        y = angle.axis.y * s;
+        z = angle.axis.z * s;
+        w = std::cos(angle.angle / 2);
+    }
 }
\ No newline at end of file