Fix Unit/Vector slerp endpoints for antiparallel inputs

src/base/interpolation.rs

@@ -65,6 +65,9 @@ impl<
 impl<T: RealField, D: Dim, S: Storage<T, D>> Unit<Vector<T, D, S>> {
     /// Computes the spherical linear interpolation between two unit vectors.
     ///
+    /// When the vectors are antiparallel the geodesic is ambiguous; an arbitrary but
+    /// deterministic one is used, still honoring `slerp(_, 0) == self` and `slerp(_, 1) == rhs`.
+    ///
     /// # Examples:
     ///
     /// ```
@@ -86,9 +89,50 @@ impl<T: RealField, D: Dim, S: Storage<T, D>> Unit<Vector<T, D, S>> {
     where
         DefaultAllocator: Allocator<D>,
     {
-        // TODO: the result is wrong when self and rhs are collinear with opposite direction.
-        self.try_slerp(rhs, t, T::default_epsilon())
-            .unwrap_or_else(|| Unit::new_unchecked(self.clone_owned()))
+        if let Some(result) = self.try_slerp(rhs, t.clone(), T::default_epsilon()) {
+            return result;
+        }
+
+        // `self` and `rhs` are (nearly) antiparallel: the great circle is ambiguous, but the
+        // endpoints are not. Rotate through a deterministic axis orthogonal to `self`, so
+        // slerp(_, 0) == self, slerp(_, 1) == rhs, and the path stays continuous. See #657.
+        let n = self.clone_owned();
+        let dim = n.len();
+
+        // Canonical basis vector least aligned with `n`, for numerical robustness.
+        let mut axis = 0;
+        let mut min_sq = n[0].clone() * n[0].clone();
+        for i in 1..dim {
+            let sq = n[i].clone() * n[i].clone();
+            if sq < min_sq {
+                min_sq = sq;
+                axis = i;
+            }
+        }
+
+        let mut ortho = n.clone();
+        ortho.fill(T::zero());
+        ortho[axis] = T::one();
+        let dot = ortho.dot(&n);
+        ortho.axpy(-dot, &n, T::one()); // ortho = e_axis - (e_axis . n) n
+
+        let ortho_norm = ortho.norm();
+        if relative_eq!(ortho_norm, T::zero()) {
+            // No orthogonal direction exists (1D): only the endpoints are defined.
+            let half = T::one() / (T::one() + T::one());
+            return if t <= half {
+                Unit::new_unchecked(n)
+            } else {
+                Unit::new_unchecked(rhs.clone_owned())
+            };
+        }
+        ortho.unscale_mut(ortho_norm);
+
+        let theta = T::pi() * t;
+        let mut res = n.scale(theta.clone().cos());
+        res.axpy(theta.sin(), &ortho, T::one());
+
+        Unit::new_unchecked(res)
     }
 
     /// Computes the spherical linear interpolation between two unit vectors.
@@ -112,6 +156,12 @@ impl<T: RealField, D: Dim, S: Storage<T, D>> Unit<Vector<T, D, S>> {
             return Some(Unit::new_unchecked(self.clone_owned()));
         }
 
+        // self == -other, up to rounding pushing the dot product past -1 (which would make the
+        // acos/sqrt below NaN): opposite direction, so the interpolation is not well-defined.
+        if c_hang <= -T::one() {
+            return None;
+        }
+
         let hang = c_hang.clone().acos();
         let s_hang = (T::one() - c_hang.clone() * c_hang).sqrt();
 

tests/core/interpolation.rs

@@ -0,0 +1,66 @@
+use na::{Unit, Vector2, Vector3, Vector4};
+
+// Antiparallel unit vectors used to return `self` for every `t`, breaking the documented
+// endpoint contract slerp(a, b, 1) == b. See issue #657.
+#[test]
+fn unit_slerp_antiparallel_honors_endpoints_2d() {
+    let a = Unit::new_normalize(Vector2::new(1.0, 0.0));
+    let b = Unit::new_normalize(Vector2::new(-1.0, 0.0));
+
+    assert_relative_eq!(a.slerp(&b, 0.0).into_inner(), a.into_inner());
+    assert_relative_eq!(a.slerp(&b, 1.0).into_inner(), b.into_inner());
+}
+
+#[test]
+fn unit_slerp_antiparallel_honors_endpoints_3d() {
+    let a = Unit::new_normalize(Vector3::new(0.0, 1.0, 0.0));
+    let b = Unit::new_normalize(Vector3::new(0.0, -1.0, 0.0));
+
+    assert_relative_eq!(a.slerp(&b, 0.0).into_inner(), a.into_inner());
+    assert_relative_eq!(a.slerp(&b, 1.0).into_inner(), b.into_inner());
+}
+
+// The interior path for antiparallel inputs is arbitrary but must stay on the unit sphere and
+// move away from `self` (the old code stayed pinned to `self`). At t = 0.5 the midpoint is a
+// quarter turn from both endpoints, hence orthogonal to `self`.
+#[test]
+fn unit_slerp_antiparallel_interior_is_unit_and_moves() {
+    let a = Unit::new_normalize(Vector3::new(1.0, 2.0, -2.0));
+    let b = Unit::new_normalize(-a.into_inner());
+
+    for &t in &[0.1, 0.25, 0.5, 0.75, 0.9] {
+        let m = a.slerp(&b, t).into_inner();
+        assert_relative_eq!(m.norm(), 1.0);
+    }
+    let mid = a.slerp(&b, 0.5).into_inner();
+    assert_relative_eq!(mid.dot(&a), 0.0, epsilon = 1.0e-12);
+}
+
+#[test]
+fn unit_slerp_antiparallel_4d_endpoints() {
+    let a = Unit::new_normalize(Vector4::new(1.0, -1.0, 2.0, 0.5));
+    let b = Unit::new_normalize(-a.into_inner());
+
+    assert_relative_eq!(a.slerp(&b, 0.0).into_inner(), a.into_inner());
+    assert_relative_eq!(a.slerp(&b, 1.0).into_inner(), b.into_inner());
+}
+
+// The non-degenerate path must be untouched.
+#[test]
+fn unit_slerp_generic_endpoints() {
+    let a = Unit::new_normalize(Vector2::new(1.0, 2.0));
+    let b = Unit::new_normalize(Vector2::new(2.0, -3.0));
+
+    assert_relative_eq!(a.slerp(&b, 0.0).into_inner(), a.into_inner());
+    assert_relative_eq!(a.slerp(&b, 1.0).into_inner(), b.into_inner());
+}
+
+// The `Vector::slerp` wrapper normalizes and delegates, so it inherits the fix.
+#[test]
+fn vector_slerp_antiparallel_honors_endpoints() {
+    let a = Vector3::new(0.0, 3.0, 0.0);
+    let b = Vector3::new(0.0, -5.0, 0.0);
+
+    assert_relative_eq!(a.slerp(&b, 1.0), b.normalize());
+    assert_relative_eq!(a.slerp(&b, 0.0), a.normalize());
+}

tests/core/mod.rs

@@ -3,6 +3,7 @@ mod cg;
 mod conversion;
 mod edition;
 mod empty;
+mod interpolation;
 mod matrix;
 mod matrix_view;
 #[cfg(feature = "mint")]