diff --git a/src/J3ML/LinearAlgebra/Matrix3x3.cpp b/src/J3ML/LinearAlgebra/Matrix3x3.cpp index da03d86..eb1a733 100644 --- a/src/J3ML/LinearAlgebra/Matrix3x3.cpp +++ b/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 + }; + } + }