diff --git a/src/Magnum/Math/Matrix4.h b/src/Magnum/Math/Matrix4.h index 2008ac00f..dd7722efa 100644 --- a/src/Magnum/Math/Matrix4.h +++ b/src/Magnum/Math/Matrix4.h @@ -491,6 +491,10 @@ template class Matrix4: public Matrix4x4 { return rotationScaling().isOrthogonal() && row(3) == Vector4(T(0), T(0), T(0), T(1)); } + Vector3> toEuler() const; + Vector3> toEulerXZY() const; + Vector3> toEulerYZX() const; + /** * @brief 3D rotation and scaling part of the matrix * @@ -1088,6 +1092,80 @@ template Matrix4 Matrix4::lookAt(const Vector3& eye, const Vec return from({right, realUp, backward}, eye); } +template Vector3> Matrix4::toEuler() const { /* XYZ */ + Vector3> euler{Magnum::NoInit}; + + const T m11 = (*this)[0][0]; + const T m12 = (*this)[0][1]; + const T m13 = (*this)[0][2]; + const T m21 = (*this)[1][0]; + const T m22 = (*this)[1][1]; + const T m23 = (*this)[1][2]; + const T m33 = (*this)[2][2]; + + euler.y() = Rad(std::asin(-Math::min(Math::max(m13, T(-1.0)), T(1.0)))); + + if(!TypeTraits::equalsZero(m13 - T(1.0), T(1.0))) { + euler.x() = Rad(std::atan2(m23, m33)); + euler.z() = Rad(std::atan2(m12, m11)); + } else { + euler.x() = Rad(0.0); + euler.z() = Rad(std::atan2(-m21, m22)); + } + + return euler; +} + +/* https://github.com/mrdoob/three.js/blob/6892dd0aba1411d35c5e2b44dc6ff280b24d6aa2/src/math/Euler.js#L213 */ +template Vector3> Matrix4::toEulerXZY() const { + Vector3> euler{Magnum::NoInit}; + + const T m11 = (*this)[0][0]; + const T m12 = (*this)[0][1]; + const T m13 = (*this)[0][2]; + const T m22 = (*this)[1][1]; + const T m31 = (*this)[2][0]; + const T m32 = (*this)[2][1]; + const T m33 = (*this)[2][2]; + + euler.z() = Rad(std::asin(Math::min(Math::max(m12, T(-1.0)), T(1.0)))); + + if(!TypeTraits::equalsZero(m12 - T(1.0), T(1.0))) { + euler.x() = Rad(std::atan2(-m32, m22)); + euler.y() = Rad(std::atan2(-m13, m11)); + } else { + euler.x() = Rad(0.0); + euler.y() = Rad(std::atan2(m31, m33)); + } + + return euler; +} + +/* https://github.com/mrdoob/three.js/blob/6892dd0aba1411d35c5e2b44dc6ff280b24d6aa2/src/math/Euler.js#L229 */ +template Vector3> Matrix4::toEulerYZX() const { + Vector3> euler{Magnum::NoInit}; + + const T m11 = (*this)[0][0]; + const T m21 = (*this)[1][0]; + const T m22 = (*this)[1][1]; + const T m23 = (*this)[1][2]; + const T m31 = (*this)[2][0]; + const T m32 = (*this)[2][1]; + const T m33 = (*this)[2][2]; + + euler.z() = Rad(std::asin(-Math::min(Math::max(m21, T(-1.0)), T(1.0)))); + + if(!TypeTraits::equalsZero(m21 - T(1.0), T(1.0))) { + euler.x() = Rad(std::atan2(m23, m22)); + euler.y() = Rad(std::atan2(m31, m11)); + } else { + euler.x() = Rad(0.0); + euler.y() = Rad(std::atan2(-m32, m33)); + } + + return euler; +} + template Matrix3x3 Matrix4::rotation() const { Matrix3x3 rotation{(*this)[0].xyz().normalized(), (*this)[1].xyz().normalized(), diff --git a/src/Magnum/Math/Test/Matrix4Test.cpp b/src/Magnum/Math/Test/Matrix4Test.cpp index 4a45d200f..32e188152 100644 --- a/src/Magnum/Math/Test/Matrix4Test.cpp +++ b/src/Magnum/Math/Test/Matrix4Test.cpp @@ -115,6 +115,8 @@ struct Matrix4Test: Corrade::TestSuite::Tester { void transform(); void transformProjection(); + void euler(); + void strictWeakOrdering(); void debug(); @@ -184,6 +186,8 @@ Matrix4Test::Matrix4Test() { &Matrix4Test::transform, &Matrix4Test::transformProjection, + &Matrix4Test::euler, + &Matrix4Test::strictWeakOrdering, &Matrix4Test::debug}); @@ -986,6 +990,38 @@ void Matrix4Test::transformProjection() { CORRADE_COMPARE(a.transformPoint(v), Vector3(0.0f, 0.0f, 1.0f)); } +void Matrix4Test::euler() { + Matrix4 a{ + {-0.459509, -0.888116, -0.0100141, 0}, + {0.888142, -0.45937, -0.0135043, 0}, + {0.00739323, -0.0150993, 0.999859, 0}, + {-0.406013, 10.1443, -2.54406, 1}}; + Debug{} << a; + + { + Math::Vector3 b = a.toEuler(); // XYZ + Debug{} << b; + Debug{} << + Matrix4::rotationZ(b.z())* + Matrix4::rotationY(b.y())* + Matrix4::rotationX(b.x()); + } { + Math::Vector3 b = a.toEulerXZY(); + Debug{} << b; + Debug{} << Matrix4{ + Matrix4::rotationY(b.y())* + Matrix4::rotationZ(b.z())* + Matrix4::rotationX(b.x())}; + } { + Math::Vector3 b = a.toEulerYZX(); + Debug{} << b; + Debug{} << + Matrix4::rotationX(b.x())* + Matrix4::rotationZ(b.z())* + Matrix4::rotationY(b.y()); + } +} + void Matrix4Test::strictWeakOrdering() { StrictWeakOrdering o; const Matrix4 a(Vector4{1.0f, 1.0f, 2.0f, 2.0f}, Vector4{5.0f, 5.0f, 6.0f, 5.0f}, Vector4{5.0f, 5.0f, 6.0f, 5.0f}, Vector4{3.0f, 1.0f, 2.0f, 4.0f});