diff --git a/src/base/interpolation.rs b/src/base/interpolation.rs index 95710d091..5a175c385 100644 --- a/src/base/interpolation.rs +++ b/src/base/interpolation.rs @@ -65,6 +65,9 @@ impl< impl> Unit> { /// 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> Unit> { where DefaultAllocator: Allocator, { - // 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> Unit> { 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(); diff --git a/tests/core/interpolation.rs b/tests/core/interpolation.rs new file mode 100644 index 000000000..623b9d3d5 --- /dev/null +++ b/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()); +} diff --git a/tests/core/mod.rs b/tests/core/mod.rs index f0484e4dc..a442a9d3d 100644 --- a/tests/core/mod.rs +++ b/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")]