Implement ToQuat

src/J3ML/LinearAlgebra/Matrix3x3.cpp

@@ -175,5 +175,26 @@ namespace LinearAlgebra {
         };
     }
 
+    Quaternion Matrix3x3::ToQuat() const {
+        auto m00 = At(0,0);
+        auto m01 = At(0, 1);
+        auto m02 = At(0, 2);
+        auto m10 = At(1,0);
+        auto m11 = At(1, 1);
+        auto m12 = At(1, 2);
+        auto m20 = At(2,0);
+        auto m21 = At(2, 1);
+        auto m22 = At(2, 2);
+
+        auto w = std::sqrt(1.f + m00 + m11 + m22) / 2.f;
+        float w4 = (4.f * w);
+        return {
+                (m21 - m12) / w4,
+                (m02 - m20) / w4,
+                (m10 - m01) / w4,
+                w
+        };
+    }
+
 }