From 832bf42b56ba46cf11751da4a93496660d113419 Mon Sep 17 00:00:00 2001
From: sebcrozet <developer@crozet.re>
Date: Sat, 22 Sep 2018 15:38:51 +0200
Subject: [PATCH] Add slerp for unit vectors.

---
 src/base/matrix.rs         | 39 ++++++++++++++++++++++++++++++++++++++
 src/geometry/quaternion.rs | 27 ++++----------------------
 2 files changed, 43 insertions(+), 23 deletions(-)

diff --git a/src/base/matrix.rs b/src/base/matrix.rs
index ee656bdc..579848d2 100644
--- a/src/base/matrix.rs
+++ b/src/base/matrix.rs
@@ -1247,6 +1247,45 @@ impl<N: Real, R: Dim, C: Dim, S: Storage<N, R, C>> Matrix<N, R, C, S> {
     }
 }
 
+impl<N: Real, D: Dim, S: Storage<N, D>> Unit<Vector<N, D, S>> {
+    /// Computes the spherical linear interpolation between two unit vectors.
+    pub fn slerp<S2: Storage<N, D>>(&self, rhs: &Unit<Vector<N, D, S2>>, t: N) -> Unit<VectorN<N, D>>
+        where
+            DefaultAllocator: Allocator<N, D> {
+        // FIXME: the result is wrong when self and rhs are collinear with opposite direction.
+        self.try_slerp(rhs, t, N::default_epsilon()).unwrap_or(Unit::new_unchecked(self.clone_owned()))
+    }
+
+    /// Computes the spherical linear interpolation between two unit vectors.
+    ///
+    /// Returns `None` if the two vectors are almost collinear and with opposite direction
+    /// (in this case, there is an infinity of possible results).
+    pub fn try_slerp<S2: Storage<N, D>>(&self, rhs: &Unit<Vector<N, D, S2>>, t: N, epsilon: N) -> Option<Unit<VectorN<N, D>>>
+    where
+        DefaultAllocator: Allocator<N, D> {
+        let c_hang = self.dot(rhs);
+
+        // self == other
+        if c_hang.abs() >= N::one() {
+            return Some(Unit::new_unchecked(self.clone_owned()));
+        }
+
+        let hang = c_hang.acos();
+        let s_hang = (N::one() - c_hang * c_hang).sqrt();
+
+        // FIXME: what if s_hang is 0.0 ? The result is not well-defined.
+        if relative_eq!(s_hang, N::zero(), epsilon = epsilon) {
+            None
+        } else {
+            let ta = ((N::one() - t) * hang).sin() / s_hang;
+            let tb = (t * hang).sin() / s_hang;
+            let res = &**self * ta + &**rhs * tb;
+
+            Some(Unit::new_unchecked(res))
+        }
+    }
+}
+
 impl<N: Real, R: Dim, C: Dim, S: StorageMut<N, R, C>> Matrix<N, R, C, S> {
     /// Normalizes this matrix in-place and returns its norm.
     #[inline]
diff --git a/src/geometry/quaternion.rs b/src/geometry/quaternion.rs
index 3196b94c..375c6b57 100644
--- a/src/geometry/quaternion.rs
+++ b/src/geometry/quaternion.rs
@@ -451,9 +451,8 @@ impl<N: Real> UnitQuaternion<N> {
     /// is not well-defined).
     #[inline]
     pub fn slerp(&self, other: &UnitQuaternion<N>, t: N) -> UnitQuaternion<N> {
-        self.try_slerp(other, t, N::zero()).expect(
-            "Unable to perform a spherical quaternion interpolation when they \
-             are 180 degree apart (the result is not unique).",
+        Unit::new_unchecked(
+            Quaternion::from_vector(Unit::new_unchecked(self.coords).slerp(&Unit::new_unchecked(other.coords), t).unwrap())
         )
     }
 
@@ -474,26 +473,8 @@ impl<N: Real> UnitQuaternion<N> {
         t: N,
         epsilon: N,
     ) -> Option<UnitQuaternion<N>> {
-        let c_hang = self.coords.dot(&other.coords);
-
-        // self == other
-        if c_hang.abs() >= N::one() {
-            return Some(*self);
-        }
-
-        let hang = c_hang.acos();
-        let s_hang = (N::one() - c_hang * c_hang).sqrt();
-
-        // FIXME: what if s_hang is 0.0 ? The result is not well-defined.
-        if relative_eq!(s_hang, N::zero(), epsilon = epsilon) {
-            None
-        } else {
-            let ta = ((N::one() - t) * hang).sin() / s_hang;
-            let tb = (t * hang).sin() / s_hang;
-            let res = self.as_ref() * ta + other.as_ref() * tb;
-
-            Some(UnitQuaternion::new_unchecked(res))
-        }
+        Unit::new_unchecked(self.coords).try_slerp(&Unit::new_unchecked(other.coords), t, epsilon)
+            .map(|q| Unit::new_unchecked(Quaternion::from_vector(q.unwrap())))
     }
 
     /// Compute the conjugate of this unit quaternion in-place.