From 0ace7c530f7d0e6882d19ae5cf5e2f69bf75372b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Vladim=C3=ADr=20Vondru=C5=A1?= Date: Fri, 26 Jan 2024 19:31:24 +0100 Subject: [PATCH] Math: add Quaternion::rotation() taking two vectors. --- doc/changelog.dox | 2 + src/Magnum/Math/Quaternion.h | 56 ++++++++++++++++- src/Magnum/Math/Test/QuaternionTest.cpp | 83 ++++++++++++++++++++++++- 3 files changed, 138 insertions(+), 3 deletions(-) diff --git a/doc/changelog.dox b/doc/changelog.dox index 5088f7e1c..0012a599b 100644 --- a/doc/changelog.dox +++ b/doc/changelog.dox @@ -240,6 +240,8 @@ See also: @relativeref{Math::Intersection,pointSphere()}, which are just wrappers over trivial code but easier to discover - Added an unary @cpp operator+() @ce to all @ref Math classes +- Added @ref Math::Quaternion::rotation(const Vector3&, const Vector3&) + for creating a quaternion that rotates from one vector to another - Added @ref Math::Quaternion::reflection() and @ref Math::Quaternion::reflectVector(), but mainly just for documentation purposes as reflections cannot be combined with rotations and thus are diff --git a/src/Magnum/Math/Quaternion.h b/src/Magnum/Math/Quaternion.h index 75a80001c..74ff5e16d 100644 --- a/src/Magnum/Math/Quaternion.h +++ b/src/Magnum/Math/Quaternion.h @@ -302,13 +302,31 @@ template class Quaternion { * Expects that the rotation axis is normalized. @f[ * q = [\boldsymbol a \cdot \sin(\frac{\theta}{2}), \cos(\frac{\theta}{2})] * @f] - * @see @ref angle(), @ref axis(), @ref DualQuaternion::rotation(), + * @see @ref rotation(const Vector3&, const Vector3&), + * @ref angle(), @ref axis(), @ref DualQuaternion::rotation(), * @ref Matrix4::rotation(), @ref Complex::rotation(), * @ref Vector3::xAxis(), @ref Vector3::yAxis(), * @ref Vector3::zAxis(), @ref Vector::isNormalized() */ static Quaternion rotation(Rad angle, const Vector3& normalizedAxis); + /** + * @brief Quaternion rotating from a vector to another + * @param normalizedFrom Normalized vector from which to rotate + * @param normalizedTo Normalized vector to which to rotate + * @m_since_latest + * + * Returns a quaternion that transforms @p normalizedFrom into + * @p normalizedTo. Expects that both vectors are normalized. If the + * vectors are parallel, returns an identity quaternion, if they're + * antiparallel, picks an arbitrary rotation axis. + * + * Based on *The Shortest Arc Quaternion* by Stan Melax, + * [Game Programming Gems 1, page 214](https://archive.org/details/game-programming-gems-1/page/214/mode/2up). + * @see @ref rotation(Rad, const Vector3&) + */ + static Quaternion rotation(const Vector3& normalizedFrom, const Vector3& normalizedTo); + /** * @brief Reflection quaternion * @param normal Normal of the plane through which to reflect @@ -852,6 +870,42 @@ template inline Quaternion Quaternion::rotation(const Rad angl return {normalizedAxis*std::sin(T(angle)/2), std::cos(T(angle)/2)}; } +template Quaternion Quaternion::rotation(const Vector3& normalizedFrom, const Vector3& normalizedTo) { + CORRADE_DEBUG_ASSERT(normalizedFrom.isNormalized() && normalizedTo.isNormalized(), + "Math::Quaternion::rotation(): vectors" << normalizedFrom << "and" << normalizedTo << "are not normalized", {}); + + const T cosHalfAngle = Math::dot(normalizedFrom, normalizedTo); + + /* Vectors point in (almost) the same direction, don't need to rotate + anything */ + if(cosHalfAngle > T(1) - TypeTraits::epsilon()) + return Quaternion{IdentityInit}; + + /* Vectors point in an (almost) opposite direction, pick some arbitrary + axis as there's no single solution */ + if(cosHalfAngle < T(-1) + TypeTraits::epsilon()) { + /* Try rotating around Y. If Y is parallel with the input vector, + rotate around X instead. */ + Vector3 rotationAxis = cross(Vector3::yAxis(), normalizedFrom); + T dot = rotationAxis.dot(); + if(dot < TypeTraits::epsilon()) { + rotationAxis = cross(Vector3::xAxis(), normalizedFrom); + dot = rotationAxis.dot(); + } + + /* Reuse the dot product to normalize the axis */ + rotationAxis /= std::sqrt(dot); + + /* Same as Quaternion::rotation(axis, 180°) */ + return {rotationAxis, 0.0f}; + } + + /* Vectors are not colinear, calculate a rotation axis */ + const Vector3 rotationAxis = cross(normalizedFrom, normalizedTo); + const T sqrt = std::sqrt((T(1) + cosHalfAngle)*T(2)); + return {rotationAxis/sqrt, T(0.5)*sqrt}; +} + template inline Quaternion Quaternion::reflection(const Vector3& normal) { CORRADE_DEBUG_ASSERT(normal.isNormalized(), "Math::Quaternion::reflection(): normal" << normal << "is not normalized", {}); diff --git a/src/Magnum/Math/Test/QuaternionTest.cpp b/src/Magnum/Math/Test/QuaternionTest.cpp index 1a476864d..82249269f 100644 --- a/src/Magnum/Math/Test/QuaternionTest.cpp +++ b/src/Magnum/Math/Test/QuaternionTest.cpp @@ -93,6 +93,8 @@ struct QuaternionTest: TestSuite::Tester { void rotation(); void rotationNotNormalized(); + void rotationFromTwoVectors(); + void rotationFromTwoVectorsNotNormalized(); void reflection(); void reflectionNotNormalized(); void angle(); @@ -134,6 +136,7 @@ using Magnum::Rad; using Magnum::Matrix3x3; using Magnum::Matrix4; using Magnum::Quaternion; +using Magnum::Vector2; using Magnum::Vector3; using Magnum::Vector4; @@ -179,6 +182,8 @@ QuaternionTest::QuaternionTest() { &QuaternionTest::rotation, &QuaternionTest::rotationNotNormalized, + &QuaternionTest::rotationFromTwoVectors, + &QuaternionTest::rotationFromTwoVectorsNotNormalized, &QuaternionTest::reflection, &QuaternionTest::reflectionNotNormalized, &QuaternionTest::angle, @@ -522,6 +527,80 @@ void QuaternionTest::rotationNotNormalized() { CORRADE_COMPARE(out.str(), "Math::Quaternion::rotation(): axis Vector(-1, 2, 2) is not normalized\n"); } +void QuaternionTest::rotationFromTwoVectors() { + Vector3 a{1.0f/Constants::sqrt3()}; + Vector3 b{Vector2{1.0f/Constants::sqrt2()}, 0.0f}; + Vector3 c{0.0f, 0.0f, 1.0f}; + + /* Usual cases */ + { + Quaternion q1 = Quaternion::rotation(a, b); + Quaternion q2 = Quaternion::rotation(b, a); + CORRADE_COMPARE(q1.transformVector(a), b); + CORRADE_COMPARE(q2.transformVector(b), a); + CORRADE_COMPARE(q1, (Quaternion{{-0.214186f, 0.214186f, 0.0f}, 0.953021f})); + /* The reverse rotation is the same axis, different angle */ + CORRADE_COMPARE(q2, (Quaternion{-q1.vector(), q1.scalar()})); + } { + Quaternion q1 = Quaternion::rotation(a, c); + Quaternion q2 = Quaternion::rotation(c, a); + CORRADE_COMPARE(q1.transformVector(a), c); + CORRADE_COMPARE(q2.transformVector(c), a); + CORRADE_COMPARE(q1, (Quaternion{{0.325058f, -0.325058f, 0.0f}, 0.888074f})); + /* The reverse rotation is the same axis, different angle */ + CORRADE_COMPARE(q2, (Quaternion{-q1.vector(), q1.scalar()})); + } { + Quaternion q1 = Quaternion::rotation(b, c); + Quaternion q2 = Quaternion::rotation(c, b); + CORRADE_COMPARE(q1.transformVector(b), c); + CORRADE_COMPARE(q2.transformVector(c), b); + CORRADE_COMPARE(q1, (Quaternion{{0.5f, -0.5f, 0.0f}, 0.707107f})); + CORRADE_COMPARE(q2, (Quaternion{-q1.vector(), q1.scalar()})); + + /* Same direction, identity rotation */ + } { + Quaternion q1 = Quaternion::rotation(a, a); + Quaternion q2 = Quaternion::rotation(b, b); + CORRADE_COMPARE(q1.transformVector(a), a); + CORRADE_COMPARE(q2.transformVector(b), b); + CORRADE_COMPARE(q1, Quaternion{}); + CORRADE_COMPARE(q2, Quaternion{}); + + /* Oppposite direction, picking Y axis */ + } { + Quaternion q1 = Quaternion::rotation(a, -a); + Quaternion q2 = Quaternion::rotation(-a, a); + CORRADE_COMPARE(q1.transformVector(a), -a); + CORRADE_COMPARE(q2.transformVector(-a), a); + CORRADE_COMPARE(q1, (Quaternion{{0.707107f, 0.0f, -0.707107f}, 0.0f})); + /* The reverse rotation is the same axis, different angle */ + CORRADE_COMPARE(q2, (Quaternion{-q1.vector(), q1.scalar()})); + + /* Opposite direction, picking X axis as a fallback */ + } { + Quaternion q1 = Quaternion::rotation(Vector3::yAxis(), -Vector3::yAxis()); + Quaternion q2 = Quaternion::rotation(-Vector3::yAxis(), Vector3::yAxis()); + CORRADE_COMPARE(q1.transformVector(Vector3::yAxis()), -Vector3::yAxis()); + CORRADE_COMPARE(q2.transformVector(-Vector3::yAxis()), Vector3::yAxis()); + CORRADE_COMPARE(q1, (Quaternion{{0.0f, 0.0f, 1.0f}, 0.0f})); + /* The reverse rotation is the same axis, different angle */ + CORRADE_COMPARE(q2, (Quaternion{-q1.vector(), q1.scalar()})); + } +} + +void QuaternionTest::rotationFromTwoVectorsNotNormalized() { + CORRADE_SKIP_IF_NO_DEBUG_ASSERT(); + + std::ostringstream out; + Error redirectError{&out}; + + Quaternion::rotation({2.0f, 0.0f, 0.0f}, {0.0f, 1.0f, 0.0f}); + Quaternion::rotation({1.0f, 0.0f, 0.0f}, {0.0f, 2.0f, 0.0f}); + CORRADE_COMPARE(out.str(), + "Math::Quaternion::rotation(): vectors Vector(2, 0, 0) and Vector(0, 1, 0) are not normalized\n" + "Math::Quaternion::rotation(): vectors Vector(1, 0, 0) and Vector(0, 2, 0) are not normalized\n"); +} + void QuaternionTest::reflection() { Vector3 axis(1.0f/Constants::sqrt3()); Quaternion q = Quaternion::reflection(axis); @@ -797,7 +876,7 @@ void QuaternionTest::slerpLinearFallback() { template void QuaternionTest::slerpLinearFallbackIsNormalized() { setTestCaseTemplateName(TypeTraits::name()); - Math::Quaternion a = Math::Quaternion::rotation({}, Math::Vector3::xAxis()); + Math::Quaternion a = Math::Quaternion::rotation(Math::Rad{}, Math::Vector3::xAxis()); Math::Quaternion b = Math::Quaternion::rotation(Math::acos(T(1) - T(0.49999)*TypeTraits::epsilon()), Math::Vector3::xAxis()); /* Ensure we're in the special case */ @@ -884,7 +963,7 @@ void QuaternionTest::slerpShortestPathLinearFallback() { template void QuaternionTest::slerpShortestPathLinearFallbackIsNormalized() { setTestCaseTemplateName(TypeTraits::name()); - Math::Quaternion a = Math::Quaternion::rotation({}, Math::Vector3::xAxis()); + Math::Quaternion a = Math::Quaternion::rotation(Math::Rad{}, Math::Vector3::xAxis()); Math::Quaternion b = Math::Quaternion::rotation(Math::acos(T(1) - T(0.49999)*TypeTraits::epsilon()), Math::Vector3::xAxis()); /* Ensure we're in the special case */