From 0e05c7289e21b21596161e8f3c30cc3da6f624ec Mon Sep 17 00:00:00 2001 From: Squareys Date: Wed, 18 Nov 2015 20:31:20 +0100 Subject: [PATCH] Math: Fix sclerp of dual quaternions with equal rotation and add test. When rotation is identical, the rotation of the first dual quaternion is returned instead, together with the linearly interpolated translation of both (lerp of the vectors of the dual part). The additional include is needed for `Math::lerp(Vector<3, T>, Vector<3, T>, T)`. Signed-off-by: Squareys --- src/Magnum/Math/DualQuaternion.h | 7 +++++++ src/Magnum/Math/Test/DualQuaternionTest.cpp | 11 +++++++++++ 2 files changed, 18 insertions(+) diff --git a/src/Magnum/Math/DualQuaternion.h b/src/Magnum/Math/DualQuaternion.h index 58b10a672..7eed6bf8a 100644 --- a/src/Magnum/Math/DualQuaternion.h +++ b/src/Magnum/Math/DualQuaternion.h @@ -34,6 +34,7 @@ #include "Magnum/Math/Dual.h" #include "Magnum/Math/Matrix4.h" #include "Magnum/Math/Quaternion.h" +#include "Magnum/Math/Functions.h" namespace Magnum { namespace Math { @@ -67,6 +68,12 @@ template inline DualQuaternion sclerp(const DualQuaternion& norma "Math::sclerp(): dual quaternions must be normalized", {}); const T dotResult = dot(normalizedA.real().vector(), normalizedB.real().vector()); + /* Avoid division by zero */ + const T cosHalfAngle = dotResult + normalizedA.real().scalar()*normalizedB.real().scalar(); + if(std::abs(cosHalfAngle) >= T(1)) { + return DualQuaternion{normalizedA.real(), Quaternion{Math::lerp(normalizedA.dual().vector(), normalizedB.dual().vector(), t), T(0)}}; + } + /* l + εm = q_A^**q_B, multiplying with -1 ensures shortest path when dot < 0 */ const DualQuaternion diff = normalizedA.quaternionConjugated()*(dotResult < T(0) ? -normalizedB : normalizedB); const Quaternion& l = diff.real(); diff --git a/src/Magnum/Math/Test/DualQuaternionTest.cpp b/src/Magnum/Math/Test/DualQuaternionTest.cpp index cc1cfc7f7..b84d8a3ab 100644 --- a/src/Magnum/Math/Test/DualQuaternionTest.cpp +++ b/src/Magnum/Math/Test/DualQuaternionTest.cpp @@ -433,6 +433,17 @@ void DualQuaternionTest::sclerp() { CORRADE_COMPARE(interp1, expected1); CORRADE_COMPARE(interp2, expected2); CORRADE_COMPARE(interp3, expected3); + + /* Edge cases: */ + + /* Dual quaternions with identical rotation */ + CORRADE_COMPARE(Math::sclerp(from, from, 0.42f), from); + CORRADE_COMPARE(Math::sclerp(from, DualQuaternion(-from.real(), from.dual()), 0.42f), from); + + /* No difference in rotation, but in translation */ + const auto rotation = DualQuaternion::rotation(35.0_degf, Vector3{0.3f, 0.2f, 0.1f}); + CORRADE_COMPARE(Math::sclerp(DualQuaternion::translation(Vector3{1.0f, 2.0f, 4.0f})*rotation, DualQuaternion::translation(Vector3{5, -6, 2})*rotation, 0.25f), + DualQuaternion::translation(Vector3{2.0f, 0.0f, 3.5f})*rotation); } }}}