Merge pull request #1016 from tpdickso/fix-dual-quaternion-sclerp

Don't panic ScLERPing `UnitDualQuaternion` with equal rotation

src/geometry/dual_quaternion.rs

@@ -357,7 +357,8 @@ impl<T: RealField + UlpsEq<Epsilon = T>> UlpsEq for DualQuaternion<T> {
     }
 }
 
-/// A unit quaternions. May be used to represent a rotation followed by a translation.
+/// A unit dual quaternion. May be used to represent a rotation followed by a
+/// translation.
 pub type UnitDualQuaternion<T> = Unit<DualQuaternion<T>>;
 
 impl<T: Scalar + ClosedNeg + PartialEq + SimdRealField> PartialEq for UnitDualQuaternion<T> {
@@ -593,8 +594,9 @@ where
     /// Screw linear interpolation between two unit quaternions. This creates a
     /// smooth arc from one dual-quaternion to another.
     ///
-    /// Panics if the angle between both quaternion is 180 degrees (in which case the interpolation
+    /// Panics if the angle between both quaternion is 180 degrees (in which
-    /// is not well-defined). Use `.try_sclerp` instead to avoid the panic.
+    /// case the interpolation is not well-defined). Use `.try_sclerp`
+    /// instead to avoid the panic.
     ///
     /// # Example
     /// ```
@@ -627,15 +629,16 @@ where
             .expect("DualQuaternion sclerp: ambiguous configuration.")
     }
 
-    /// Computes the screw-linear interpolation between two unit quaternions or returns `None`
+    /// Computes the screw-linear interpolation between two unit quaternions or
-    /// if both quaternions are approximately 180 degrees apart (in which case the interpolation is
+    /// returns `None` if both quaternions are approximately 180 degrees
-    /// not well-defined).
+    /// apart (in which case the interpolation is not well-defined).
     ///
     /// # Arguments
     /// * `self`: the first quaternion to interpolate from.
     /// * `other`: the second quaternion to interpolate toward.
     /// * `t`: the interpolation parameter. Should be between 0 and 1.
-    /// * `epsilon`: the value below which the sinus of the angle separating both quaternion
+    /// * `epsilon`: the value below which the sinus of the angle separating
+    ///   both quaternion
     /// must be to return `None`.
     #[inline]
     #[must_use]
@@ -650,6 +653,10 @@ where
         // interpolation.
         let other = {
             let dot_product = self.as_ref().real.coords.dot(&other.as_ref().real.coords);
+            if relative_eq!(dot_product, T::zero(), epsilon = epsilon.clone()) {
+                return None;
+            }
+
             if dot_product < T::zero() {
                 -other.clone()
             } else {
@@ -660,13 +667,21 @@ where
         let difference = self.as_ref().conjugate() * other.as_ref();
         let norm_squared = difference.real.vector().norm_squared();
         if relative_eq!(norm_squared, T::zero(), epsilon = epsilon) {
-            return None;
+            return Some(Self::from_parts(
+                self.translation()
+                    .vector
+                    .lerp(&other.translation().vector, t)
+                    .into(),
+                self.rotation(),
+            ));
         }
 
-        let inverse_norm_squared = T::one() / norm_squared;
+        let scalar: T = difference.real.scalar();
+        let mut angle = two.clone() * scalar.acos();
+
+        let inverse_norm_squared: T = T::one() / norm_squared;
         let inverse_norm = inverse_norm_squared.sqrt();
 
-        let mut angle = two.clone() * difference.real.scalar().acos();
         let mut pitch = -two * difference.dual.scalar() * inverse_norm.clone();
         let direction = difference.real.vector() * inverse_norm.clone();
         let moment = (difference.dual.vector()
@@ -678,6 +693,7 @@ where
 
         let sin = (half.clone() * angle.clone()).sin();
         let cos = (half.clone() * angle).cos();
+
         let real = Quaternion::from_parts(cos.clone(), direction.clone() * sin.clone());
         let dual = Quaternion::from_parts(
             -pitch.clone() * half.clone() * sin.clone(),

tests/geometry/dual_quaternion.rs

@@ -1,7 +1,7 @@
 #![cfg(feature = "proptest-support")]
 #![allow(non_snake_case)]
 
-use na::{DualQuaternion, Point3, UnitDualQuaternion, Vector3};
+use na::{DualQuaternion, Point3, Unit, UnitDualQuaternion, UnitQuaternion, Vector3};
 
 use crate::proptest::*;
 use proptest::{prop_assert, proptest};
@@ -74,6 +74,98 @@ proptest!(
         prop_assert!(relative_eq!((dq * t) * p, dq * (t * p), epsilon = 1.0e-7));
     }
 
+    #[cfg_attr(rustfmt, rustfmt_skip)]
+    #[test]
+    fn sclerp_is_defined_for_identical_orientations(
+        dq in unit_dual_quaternion(),
+        s in -1.0f64..2.0f64,
+        t in translation3(),
+    ) {
+        // Should not panic.
+        prop_assert!(relative_eq!(dq.sclerp(&dq, 0.0), dq, epsilon = 1.0e-7));
+        prop_assert!(relative_eq!(dq.sclerp(&dq, 0.5), dq, epsilon = 1.0e-7));
+        prop_assert!(relative_eq!(dq.sclerp(&dq, 1.0), dq, epsilon = 1.0e-7));
+        prop_assert!(relative_eq!(dq.sclerp(&dq, s), dq, epsilon = 1.0e-7));
+
+        let unit = UnitDualQuaternion::identity();
+        prop_assert!(relative_eq!(unit.sclerp(&unit, 0.0), unit, epsilon = 1.0e-7));
+        prop_assert!(relative_eq!(unit.sclerp(&unit, 0.5), unit, epsilon = 1.0e-7));
+        prop_assert!(relative_eq!(unit.sclerp(&unit, 1.0), unit, epsilon = 1.0e-7));
+        prop_assert!(relative_eq!(unit.sclerp(&unit, s), unit, epsilon = 1.0e-7));
+
+        // ScLERPing two unit dual quaternions with nearly equal rotation
+        // components should result in a unit dual quaternion with a rotation
+        // component nearly equal to either input.
+        let dq2 = t * dq;
+        prop_assert!(relative_eq!(dq.sclerp(&dq2, 0.0).real, dq.real, epsilon = 1.0e-7));
+        prop_assert!(relative_eq!(dq.sclerp(&dq2, 0.5).real, dq.real, epsilon = 1.0e-7));
+        prop_assert!(relative_eq!(dq.sclerp(&dq2, 1.0).real, dq.real, epsilon = 1.0e-7));
+        prop_assert!(relative_eq!(dq.sclerp(&dq2, s).real, dq.real, epsilon = 1.0e-7));
+
+        // ScLERPing two unit dual quaternions with nearly equal rotation
+        // components should result in a unit dual quaternion with a translation
+        // component which is nearly equal to linearly interpolating the
+        // translation components of the inputs.
+        prop_assert!(relative_eq!(
+            dq.sclerp(&dq2, s).translation().vector,
+            dq.translation().vector.lerp(&dq2.translation().vector, s),
+            epsilon = 1.0e-7
+        ));
+
+        let unit2 = t * unit;
+        prop_assert!(relative_eq!(unit.sclerp(&unit2, 0.0).real, unit.real, epsilon = 1.0e-7));
+        prop_assert!(relative_eq!(unit.sclerp(&unit2, 0.5).real, unit.real, epsilon = 1.0e-7));
+        prop_assert!(relative_eq!(unit.sclerp(&unit2, 1.0).real, unit.real, epsilon = 1.0e-7));
+        prop_assert!(relative_eq!(unit.sclerp(&unit2, s).real, unit.real, epsilon = 1.0e-7));
+
+        prop_assert!(relative_eq!(
+            unit.sclerp(&unit2, s).translation().vector,
+            unit.translation().vector.lerp(&unit2.translation().vector, s),
+            epsilon = 1.0e-7
+        ));
+    }
+
+    #[cfg_attr(rustfmt, rustfmt_skip)]
+    #[test]
+    fn sclerp_is_not_defined_for_opposite_orientations(
+        dq in unit_dual_quaternion(),
+        s in 0.1f64..0.9f64,
+        t in translation3(),
+        t2 in translation3(),
+        v in vector3(),
+    ) {
+        let iso = dq.to_isometry();
+        let rot = iso.rotation;
+        if let Some((axis, angle)) = rot.axis_angle() {
+            let flipped = UnitQuaternion::from_axis_angle(&axis, angle + std::f64::consts::PI);
+            let dqf = flipped * rot.inverse() * dq.clone();
+            prop_assert!(dq.try_sclerp(&dqf, 0.5, 1.0e-7).is_none());
+            prop_assert!(dq.try_sclerp(&dqf, s, 1.0e-7).is_none());
+        }
+
+        let dq2 = t * dq;
+        let iso2 = dq2.to_isometry();
+        let rot2 = iso2.rotation;
+        if let Some((axis, angle)) = rot2.axis_angle() {
+            let flipped = UnitQuaternion::from_axis_angle(&axis, angle + std::f64::consts::PI);
+            let dq3f = t2 * flipped * rot.inverse() * dq.clone();
+            prop_assert!(dq2.try_sclerp(&dq3f, 0.5, 1.0e-7).is_none());
+            prop_assert!(dq2.try_sclerp(&dq3f, s, 1.0e-7).is_none());
+        }
+
+        if let Some(axis) = Unit::try_new(v, 1.0e-7) {
+            let unit = UnitDualQuaternion::identity();
+            let flip = UnitQuaternion::from_axis_angle(&axis, std::f64::consts::PI);
+            let unitf = flip * unit;
+            prop_assert!(unit.try_sclerp(&unitf, 0.5, 1.0e-7).is_none());
+            prop_assert!(unit.try_sclerp(&unitf, s, 1.0e-7).is_none());
+
+            let unit2f = t * unit * flip;
+            prop_assert!(unit.try_sclerp(&unit2f, 0.5, 1.0e-7).is_none());
+            prop_assert!(unit.try_sclerp(&unit2f, s, 1.0e-7).is_none());
+        }
+    }
+
     #[cfg_attr(rustfmt, rustfmt_skip)]
     #[test]
     fn all_op_exist(