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 <Squareys@googlemail.com>

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<class T> inline DualQuaternion<T> sclerp(const DualQuaternion<T>& 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<T>{normalizedA.real(), Quaternion<T>{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<T> diff = normalizedA.quaternionConjugated()*(dotResult < T(0) ? -normalizedB : normalizedB);
     const Quaternion<T>& l = diff.real();

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);
 }
 
 }}}