Add euler conversion from Quaternion

doc/snippets/MagnumMath.cpp

@@ -987,6 +987,17 @@ Math::Matrix2x2<Byte> integral{floatingPoint}; // {{1, 2}, {-15, 7}}
 /* [Matrix-conversion] */
 }
 
+{
+/* [Quaternion-fromEuler] */
+Rad x, y, z;
+Quaternion a =
+    Quaternion::rotation(z, Vector3::zAxis())*
+    Quaternion::rotation(y, Vector3::yAxis())*
+    Quaternion::rotation(x, Vector3::xAxis());
+/* [Quaternion-fromEuler] */
+static_cast<void>(a);
+}
+
 {
 /* [unpack-template-explicit] */
 // Literal type is (signed) char, but we assumed unsigned char, a != 1.0f

src/Magnum/Math/Quaternion.h

@@ -399,6 +399,19 @@ template<class T> class Quaternion {
          */
         Matrix3x3<T> toMatrix() const;
 
+        /**
+         * @brief Convert to an euler vector
+         *
+         * Expects that the quaternion is normalized and that the values
+         * are combined in ZYX order.
+         * The returned vector of radians is in XYZ order.
+         * @see @ref rotation(), @ref DualQuaternion::rotation()
+         * To convert the euler angles back to the Quaternion you can use:
+         *
+         * @snippet MagnumMath.cpp Quaternion-fromEuler
+         */
+        Vector3<Rad<T>> toEuler() const;
+
         /**
          * @brief Negated quaternion
          *
@@ -733,6 +746,37 @@ template<class T> Matrix3x3<T> Quaternion<T>::toMatrix() const {
     };
 }
 
+/* Algorithm from:
+   https://github.com/mrdoob/three.js/blob/6892dd0aba1411d35c5e2b44dc6ff280b24d6aa2/src/math/Euler.js#L197 */
+template<class T> Vector3<Rad<T>> Quaternion<T>::toEuler() const {
+    CORRADE_ASSERT(isNormalized(),
+        "Math::Quaternion::toEuler():" << *this << "is not normalized", {});
+
+    Vector3<Rad<T>> euler{NoInit};
+
+    Matrix3x3<T> rotMatrix = toMatrix();
+
+    T m11 = rotMatrix[0][0];
+    T m12 = rotMatrix[0][1];
+    T m13 = rotMatrix[0][2];
+    T m21 = rotMatrix[1][0];
+    T m22 = rotMatrix[1][1];
+    T m23 = rotMatrix[1][2];
+    T m33 = rotMatrix[2][2];
+
+    euler.y() = Rad<T>(std::asin(-Math::min(Math::max(m13, T(-1.0)), T(1.0))));
+
+    if(!TypeTraits<T>::equalsZero(m13 - T(1.0), T(1.0))) {
+        euler.x() = Rad<T>(std::atan2(m23, m33));
+        euler.z() = Rad<T>(std::atan2(m12, m11));
+    } else {
+        euler.x() = Rad<T>(0.0);
+        euler.z() = Rad<T>(std::atan2(-m21, m22));
+    }
+
+    return euler;
+}
+
 template<class T> inline Quaternion<T> Quaternion<T>::operator*(const Quaternion<T>& other) const {
     return {_scalar*other._vector + other._scalar*_vector + Math::cross(_vector, other._vector),
             _scalar*other._scalar - Math::dot(_vector, other._vector)};

src/Magnum/Math/Test/QuaternionTest.cpp

@@ -96,6 +96,8 @@ struct QuaternionTest: Corrade::TestSuite::Tester {
     void angleNotNormalized();
     void matrix();
     void matrixNotOrthogonal();
+    void euler();
+    void eulerNotNormalized();
 
     void lerp();
     void lerp2D();
@@ -171,6 +173,8 @@ QuaternionTest::QuaternionTest() {
               &QuaternionTest::angleNotNormalized,
               &QuaternionTest::matrix,
               &QuaternionTest::matrixNotOrthogonal,
+              &QuaternionTest::euler,
+              &QuaternionTest::eulerNotNormalized,
 
               &QuaternionTest::lerp,
               &QuaternionTest::lerp2D,
@@ -554,6 +558,35 @@ void QuaternionTest::matrixNotOrthogonal() {
         "       -0.376049, -0.552819, 1.88493)\n");
 }
 
+void QuaternionTest::euler() {
+    Quaternion a = Quaternion({0.35f, 0.134f, 0.37f}, 0.02f).normalized();
+    Math::Vector3<Rad> b{1.59867_radf, -1.15100_radf, 1.85697_radf};
+
+    CORRADE_COMPARE(a.toEuler(), b);
+    CORRADE_COMPARE(a,
+        Quaternion::rotation(b.z(), Vector3::zAxis())*
+        Quaternion::rotation(b.y(), Vector3::yAxis())*
+        Quaternion::rotation(b.x(), Vector3::xAxis()));
+
+    Quaternion a2 = Quaternion({-0.624252f, -0.331868f, -0.624468f}, 0.331983f);
+    Math::Vector3<Rad> b2{0.0_radf, -1.57045_radf, -2.16434_radf};
+
+    CORRADE_COMPARE(a2.toEuler(), b2);
+    CORRADE_COMPARE(a2,
+        Quaternion::rotation(b2.z(), Vector3::zAxis())*
+        Quaternion::rotation(b2.y(), Vector3::yAxis())*
+        Quaternion::rotation(b2.x(), Vector3::xAxis()));
+}
+
+void QuaternionTest::eulerNotNormalized() {
+    std::ostringstream out;
+    Error redirectError{&out};
+
+    Quaternion{{1.0f, 3.0f, -2.0f}, -4.0f}.toEuler();
+    CORRADE_COMPARE(out.str(),
+        "Math::Quaternion::toEuler(): Quaternion({1, 3, -2}, -4) is not normalized\n");
+}
+
 void QuaternionTest::lerp() {
     Quaternion a = Quaternion::rotation(15.0_degf, Vector3(1.0f/Constants<Float>::sqrt3()));
     Quaternion b = Quaternion::rotation(23.0_degf, Vector3::xAxis());