Fix Spacing

include/J3ML/LinearAlgebra/AxisAngle.h

@@ -16,6 +16,50 @@ namespace J3ML::LinearAlgebra
         float angle;
     public:
         AxisAngle();
+        explicit AxisAngle(const Quaternion& q)
+        {
+            auto theta = std::acos(q.w) * 2.f;
+            auto ax = q.x / std::sin(std::acos(theta));
+            auto ay = q.y / std::sin(std::acos(theta));
+            auto az = q.z / std::sin(std::acos(theta));
+        }
+        explicit AxisAngle(const EulerAngle& e)
+        {
+
+            // Assuming the angles are in radians
+
+            float heading = e.pitch;
+            float attitude = e.yaw;
+            float bank = e.roll;
+
+            float c1 = std::cos(heading / 2.f);
+            float s1 = std::sin(heading / 2.f);
+            float c2 = std::cos(attitude / 2.f);
+            float s2 = std::sin(attitude / 2.f);
+            float c3 = std::cos(bank / 2.f);
+            float s3 = std::sin(bank / 2.f);
+
+            float w = c1*c2*c3 - s1*s2*s3;
+            float x = c1*c2*c3 + s1*s2*s3;
+            float y = s1*c2*c3 + c1*s2*s3;
+            float z = c1*s2*c3 - s1*c2*s3;
+
+            angle = 2.f * std::acos(w);
+
+            double norm = x*x + y*y + z*z;
+            if (norm < 0.001) { // when all euler angles are zero angle=0, so
+                // we can set axis to anything to avoid divide by zero
+                x = 1;
+                y = z = 0;
+            } else {
+                norm = std::sqrt(norm);
+                x /= norm;
+                y /= norm;
+                z /= norm;
+            }
+
+            axis = {x, y, z};
+        }
 
         AxisAngle(const Vector3 &axis, float angle);
 

include/J3ML/LinearAlgebra/EulerAngle.h

@@ -19,8 +19,8 @@ public:
     AxisAngle ToAxisAngle() const;
 
 
-    explicit EulerAngle(const Quaternion& orientation);
-    explicit EulerAngle(const AxisAngle& orientation);
+    explicit EulerAngle(const Quaternion& rhs);
+    explicit EulerAngle(const AxisAngle& rhs);
 
     /// TODO: Implement separate upper and lower bounds
     /// Preserves internal value of euler angles, normalizes and clamps the output.

src/J3ML/LinearAlgebra/EulerAngle.cpp

@@ -48,4 +48,64 @@ namespace J3ML::LinearAlgebra {
     }
 
     EulerAngle::EulerAngle() : pitch(0), yaw(0), roll(0) {}
+
+    EulerAngle::EulerAngle(const AxisAngle &rhs) {
+
+        float x = rhs.axis.x;
+        float y = rhs.axis.y;
+        float z = rhs.axis.z;
+        float angle = rhs.angle;
+
+        double s = std::sin(rhs.angle);
+
+        double c = std::cos(rhs.angle);
+
+        double t = 1-c;
+
+        // if axis is not already normalized then uncomment this
+
+        // double magnitude = std::sqrt(x*x + y*y + z*z);
+        // if (magnitude == 0) throw error;
+        // x /= magnitude;
+        // y /= magnitude;
+        // z /= magnitude;
+
+        if ((x*y*t + z*s) > 0.998) { // North pole singularity detected
+            pitch = 2 * std::atan2(x * std::sin(angle/2.f), std::cos(angle/2.f));
+            yaw = M_PI / 2.f;
+            roll = 0;
+            return;
+        }
+
+        if ((x*y*t + z*s) < -0.998) { // South pole singularity detected
+            pitch = -2 * std::atan2(x * std::sin(angle/2.f), std::cos(angle/2.f));
+            yaw = -M_PI / 2.f;
+            roll = 0;
+            return;
+        }
+
+        pitch = std::atan2(y * s-x * z * t, 1 - (y*y + z*z) * t);
+        yaw = std::asin(x * y * t + z * s);
+        roll = std::atan2(x * s - y * z * t, 1 - (x*x + z*z) * t);
+    }
+
+    EulerAngle::EulerAngle(const Quaternion &rhs) {
+        double test = rhs.x * rhs.y + rhs.z * rhs.w;
+        if (test > 0.499) { // Singularity at north pole
+            pitch = 2 * std::atan2(rhs.x, rhs.w);
+            yaw = M_PI / 2.f;
+            roll = 0;
+            return;
+        }
+
+        if (test < -0.499) { // Singularity at south pole
+            pitch = -2 * std::atan2(rhs.x, rhs.y);
+            yaw = - M_PI / 2.f;
+            roll = 0;
+            return;
+        }
+        float sqx = rhs.x * rhs.x;
+        float sqy = rhs.y * rhs.y;
+        float sqz = rhs.z * rhs.z;
+    }
 }