ToEulerAngle

src/J3ML/LinearAlgebra/Matrix3x3.cpp

@@ -109,11 +109,11 @@ namespace J3ML::LinearAlgebra {
         //this->elems[2][2] = r3.z;
     }
 
-    Matrix3x3::Matrix3x3(const Quaternion &orientation) {
+    Matrix3x3::Matrix3x3(const Quaternion& orientation) {
         SetRotatePart(orientation);
     }
 
-    Matrix3x3::Matrix3x3(const EulerAngle &orientation) {
+    Matrix3x3::Matrix3x3(const EulerAngle& orientation) {
         auto sa = std::sin(orientation.pitch);
         auto ca = std::cos(orientation.pitch);
         auto sb = std::sin(orientation.roll);
@@ -999,16 +999,31 @@ namespace J3ML::LinearAlgebra {
     }
 
     EulerAngle Matrix3x3::ToEulerAngle() const {
-        auto heading = std::atan2(-At(2, 0), At(0, 0));
-        auto attitude = std::asin(At(1, 0));
-        auto bank = std::atan2(-At(1,2), At(1,1));
-        if (At(1, 0) == 1 || At(1, 0) == -1) // North Pole || South Pole
-        {
-            heading = std::atan2(At(0, 2), At(2,2));
-            bank = 0;
+        EulerAngle result;
+
+        if (std::abs(At(2, 0)) < 1.0f - 1e-6f) {
+            result.yaw = Math::Asin(-At(2, 0));
+            float cos_y = Math::Cos(result.yaw);
+
+            result.pitch = Math::Atan2(At(2, 1) / cos_y, At(2, 2) / cos_y);
+            result.roll = Math::Atan2(At(1, 0) / cos_y, At(0, 0) / cos_y);
+
+            result.pitch = Math::Degrees(result.pitch);
+            result.yaw = Math::Degrees(result.yaw);
+            result.roll = Math::Degrees(result.roll);
+            return result;
         }
 
-        return {attitude, heading, bank};
+        // If there was gimbal lock.
+        result.roll = 0;
+        result.yaw = Math::Degrees((At(2,0) <= -1.0f) ? (Math::Pi / 2) : -(Math::Pi / 2));
+
+        if (At(2, 0) <= -1)
+            result.roll = Math::Degrees(Math::Atan2(-At(0, 1), At(0, 2)));
+        else
+            result.roll = Math::Degrees(Math::Atan2(At(0, 1), -At(0, 2)));
+
+        return result;
     }
 
     void Matrix3x3::BatchTransform(Vector3 *pointArray, int numPoints, int stride) const {

tests/LinearAlgebra/Matrix3x3Tests.hpp

@@ -11,6 +11,45 @@ namespace Matrix3x3Tests
         using namespace jtest;
         using namespace J3ML::LinearAlgebra;
 
+        Matrix3x3Unit += Test("To_EulerAngle", []
+        {
+            // Normal test.
+            EulerAngle expected(30.f, 45.f, 60.f);
+            Matrix3x3 rotation_matrix(
+                    0.3535533906f, -0.5732233047f,  0.739198919f,
+                    0.6123724357f,  0.739198919f,   0.2803300859f,
+                    -0.7071067812f,  0.3535533906f,  0.6123724357f
+            );
+
+            jtest::check(Math::EqualAbs(expected.pitch, rotation_matrix.ToEulerAngle().pitch, 1e-4f));
+            jtest::check(Math::EqualAbs(expected.yaw, rotation_matrix.ToEulerAngle().yaw, 1e-4f));
+            jtest::check(Math::EqualAbs(expected.roll, rotation_matrix.ToEulerAngle().roll, 1e-4f));
+
+            // Second test for a scenario which would cause gimbal lock.
+
+            EulerAngle expected_2(0, 90, 0);
+            Matrix3x3 rotation_matrix_2(Vector3(0, 0, -1), Vector3(0, 1, 0), Vector3(1, 0, 0));
+            jtest::check(Math::EqualAbs(expected_2.pitch, rotation_matrix_2.ToEulerAngle().pitch, 1e-4f));
+            jtest::check(Math::EqualAbs(expected_2.yaw, rotation_matrix_2.ToEulerAngle().yaw, 1e-4f));
+            jtest::check(Math::EqualAbs(expected_2.roll, rotation_matrix_2.ToEulerAngle().roll, 1e-4f));
+
+
+            // Third test where we build up the matrix. Fails.
+            // Result (0, -0, 0).
+            rotation_matrix_2 = Matrix3x3::RotateX(0) * Matrix3x3::RotateY(90) * Matrix3x3::RotateZ(0);
+            jtest::check(Math::EqualAbs(expected_2.pitch, rotation_matrix_2.ToEulerAngle().pitch, 1e-4f));
+            jtest::check(Math::EqualAbs(expected_2.yaw, rotation_matrix_2.ToEulerAngle().yaw, 1e-4f));
+            jtest::check(Math::EqualAbs(expected_2.roll, rotation_matrix_2.ToEulerAngle().roll, 1e-4f));
+            
+            // 4th test where we initialize with EulerAngle (close apart from the seeming mismatch of radians vs degrees? idk.)
+            // Result (0, 90, -45)
+            Matrix3x3 rotation_matrix_3(EulerAngle(Math::Radians(0),Math::Radians(90),Math::Radians(0)));
+            rotation_matrix_2 = rotation_matrix_3;
+            jtest::check(Math::EqualAbs(expected_2.pitch, rotation_matrix_2.ToEulerAngle().pitch, 1e-4f));
+            jtest::check(Math::EqualAbs(expected_2.yaw, rotation_matrix_2.ToEulerAngle().yaw, 1e-4f));
+            jtest::check(Math::EqualAbs(expected_2.roll, rotation_matrix_2.ToEulerAngle().roll, 1e-4f));
+        });
+
         Matrix3x3Unit += Test("Add_Unary", []
         {
             Matrix3x3 m(1,2,3, 4,5,6, 7,8,9);