From 097ae44efad1b52f6f1e31df99f6510e52b34979 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= <developer@crozet.re>
Date: Sun, 25 Oct 2020 11:39:27 +0100
Subject: [PATCH] Add `lerp_slerp` to isometries for interpolation.

---
 src/geometry/abstract_rotation.rs | 44 +++++++++++++++++++++
 src/geometry/isometry.rs          | 66 +++++++++++++++++++++++++++++++
 2 files changed, 110 insertions(+)

diff --git a/src/geometry/abstract_rotation.rs b/src/geometry/abstract_rotation.rs
index 52471851..e1cf7c10 100644
--- a/src/geometry/abstract_rotation.rs
+++ b/src/geometry/abstract_rotation.rs
@@ -35,6 +35,14 @@ pub trait AbstractRotation<N: Scalar, D: DimName>: PartialEq + ClosedMul + Clone
     fn inverse_transform_point(&self, p: &Point<N, D>) -> Point<N, D>
     where
         DefaultAllocator: Allocator<N, D>;
+    /// Perfom a spherical interpolation between two rolations.
+    fn slerp(&self, other: &Self, t: N) -> Self
+    where
+        DefaultAllocator: Allocator<N, D>;
+    /// Attempts to perfom a spherical interpolation between two rolations.
+    fn try_slerp(&self, other: &Self, t: N, epsilon: N) -> Option<Self>
+    where
+        DefaultAllocator: Allocator<N, D>;
 }
 
 impl<N: SimdRealField, D: DimName> AbstractRotation<N, D> for Rotation<N, D>
@@ -96,6 +104,22 @@ where
     {
         self.inverse_transform_point(p)
     }
+
+    #[inline]
+    fn slerp(&self, other: &Self, t: N) -> Self
+    where
+        DefaultAllocator: Allocator<N, D>,
+    {
+        self.slerp(other, t)
+    }
+
+    #[inline]
+    fn try_slerp(&self, other: &Self, t: N, epsilon: N) -> Option<Self>
+    where
+        DefaultAllocator: Allocator<N, D>,
+    {
+        self.try_slerp(other, t, epsilon)
+    }
 }
 
 impl<N: SimdRealField> AbstractRotation<N, U3> for UnitQuaternion<N>
@@ -136,6 +160,16 @@ where
     fn inverse_transform_point(&self, p: &Point<N, U3>) -> Point<N, U3> {
         self.inverse_transform_point(p)
     }
+
+    #[inline]
+    fn slerp(&self, other: &Self, t: N) -> Self {
+        self.slerp(other, t)
+    }
+
+    #[inline]
+    fn try_slerp(&self, other: &Self, t: N, epsilon: N) -> Option<Self> {
+        self.try_slerp(other, t, epsilon)
+    }
 }
 
 impl<N: SimdRealField> AbstractRotation<N, U2> for UnitComplex<N>
@@ -176,4 +210,14 @@ where
     fn inverse_transform_point(&self, p: &Point<N, U2>) -> Point<N, U2> {
         self.inverse_transform_point(p)
     }
+
+    #[inline]
+    fn slerp(&self, other: &Self, t: N) -> Self {
+        self.slerp(other, t)
+    }
+
+    #[inline]
+    fn try_slerp(&self, other: &Self, t: N, epsilon: N) -> Option<Self> {
+        self.try_slerp(other, t, epsilon)
+    }
 }
diff --git a/src/geometry/isometry.rs b/src/geometry/isometry.rs
index b59c8b60..5b7e5dc7 100755
--- a/src/geometry/isometry.rs
+++ b/src/geometry/isometry.rs
@@ -373,6 +373,72 @@ where
     pub fn inverse_transform_unit_vector(&self, v: &Unit<VectorN<N, D>>) -> Unit<VectorN<N, D>> {
         self.rotation.inverse_transform_unit_vector(v)
     }
+
+    /// Interpolates between two isometries using a linear interpolation for the translation part,
+    /// and a spherical interpolation for the rotation part.
+    ///
+    /// Panics if the angle between both rotations is 180 degrees (in which case the interpolation
+    /// is not well-defined). Use `.try_lerp_slerp` instead to avoid the panic.
+    ///
+    /// # Examples:
+    ///
+    /// ```
+    /// # use nalgebra::geometry::{Translation3, UnitQuaternion};
+    ///
+    /// let t1 = Translation3::new(1.0, 2.0, 3.0);
+    /// let t2 = Translation3::new(3.0, 6.0, 9.0);
+    /// let q1 = UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_4, 0.0, 0.0);
+    /// let q2 = UnitQuaternion::from_euler_angles(-std::f32::consts::PI, 0.0, 0.0);
+    /// let iso1 = Isometry3::from_parts(t1, q1);
+    /// let iso2 = Isometry3::from_parts(t2, q2);
+    ///
+    /// let iso3 = iso1.lerp_slerp(&iso2, 1.0 / 3.0);
+    ///
+    /// assert_eq!(iso3.translation_vector, Vector3::new(2.0, 4.0, 6.0));
+    /// assert_eq!(iso3.rotation.euler_angles(), (std::f32::consts::FRAC_PI_2, 0.0, 0.0));
+    /// ```
+    #[inline]
+    pub fn lerp_slerp(&self, other: &Self, t: N) -> Self
+    where
+        N: RealField,
+    {
+        let tr = self.translation.vector.lerp(&other.translation.vector, t);
+        let rot = self.rotation.slerp(&other.rotation, t);
+        Self::from_parts(tr.into(), rot)
+    }
+
+    /// Attempts to interpolate between two isometries using a linear interpolation for the translation part,
+    /// and a spherical interpolation for the rotation part.
+    ///
+    /// Retuns `None` if the angle between both rotations is 180 degrees (in which case the interpolation
+    /// is not well-defined).
+    ///
+    /// # Examples:
+    ///
+    /// ```
+    /// # use nalgebra::geometry::{Translation3, UnitQuaternion};
+    ///
+    /// let t1 = Translation3::new(1.0, 2.0, 3.0);
+    /// let t2 = Translation3::new(3.0, 6.0, 9.0);
+    /// let q1 = UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_4, 0.0, 0.0);
+    /// let q2 = UnitQuaternion::from_euler_angles(-std::f32::consts::PI, 0.0, 0.0);
+    /// let iso1 = Isometry3::from_parts(t1, q1);
+    /// let iso2 = Isometry3::from_parts(t2, q2);
+    ///
+    /// let iso3 = iso1.lerp_slerp(&iso2, 1.0 / 3.0);
+    ///
+    /// assert_eq!(iso3.translation_vector, Vector3::new(2.0, 4.0, 6.0));
+    /// assert_eq!(iso3.rotation.euler_angles(), (std::f32::consts::FRAC_PI_2, 0.0, 0.0));
+    /// ```
+    #[inline]
+    pub fn try_lerp_slerp(&self, other: &Self, t: N, epsilon: N) -> Option<Self>
+    where
+        N: RealField,
+    {
+        let tr = self.translation.vector.lerp(&other.translation.vector, t);
+        let rot = self.rotation.try_slerp(&other.rotation, t, epsilon)?;
+        Some(Self::from_parts(tr.into(), rot))
+    }
 }
 
 // NOTE: we don't require `R: Rotation<...>` here because this is not useful for the implementation