diff --git a/doc/snippets/MagnumMath.cpp b/doc/snippets/MagnumMath.cpp index c9001f5600..350e465fe5 100644 --- a/doc/snippets/MagnumMath.cpp +++ b/doc/snippets/MagnumMath.cpp @@ -987,6 +987,17 @@ Math::Matrix2x2 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(a); +} + { /* [unpack-template-explicit] */ // Literal type is (signed) char, but we assumed unsigned char, a != 1.0f diff --git a/src/Magnum/Math/Quaternion.h b/src/Magnum/Math/Quaternion.h index d185de6636..e2b634390c 100644 --- a/src/Magnum/Math/Quaternion.h +++ b/src/Magnum/Math/Quaternion.h @@ -399,6 +399,19 @@ template class Quaternion { */ Matrix3x3 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> toEuler() const; + /** * @brief Negated quaternion * @@ -733,6 +746,37 @@ template Matrix3x3 Quaternion::toMatrix() const { }; } +/* Algorithm from: + https://github.com/mrdoob/three.js/blob/6892dd0aba1411d35c5e2b44dc6ff280b24d6aa2/src/math/Euler.js#L197 */ +template Vector3> Quaternion::toEuler() const { + CORRADE_ASSERT(isNormalized(), + "Math::Quaternion::toEuler():" << *this << "is not normalized", {}); + + Vector3> euler{NoInit}; + + Matrix3x3 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(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; +} + template inline Quaternion Quaternion::operator*(const Quaternion& other) const { return {_scalar*other._vector + other._scalar*_vector + Math::cross(_vector, other._vector), _scalar*other._scalar - Math::dot(_vector, other._vector)}; diff --git a/src/Magnum/Math/Test/QuaternionTest.cpp b/src/Magnum/Math/Test/QuaternionTest.cpp index b0b350077f..58d0216b3d 100644 --- a/src/Magnum/Math/Test/QuaternionTest.cpp +++ b/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 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 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::sqrt3())); Quaternion b = Quaternion::rotation(23.0_degf, Vector3::xAxis());