Fixed opposite quaternion slerp bug (#515)

* Fixed opposite quaternion slerp bug

* Removed an unnecessary

* nlerp and slerp always take the shortest path now

.vscode/settings.json

@@ -0,0 +1,2 @@
+{
+}

benches/construction.rs

@@ -20,7 +20,7 @@ extern crate cgmath;
 extern crate rand;
 extern crate test;
 
-use rand::{Rng, SeedableRng, rngs::SmallRng};
+use rand::{rngs::SmallRng, Rng, SeedableRng};
 use test::Bencher;
 
 use cgmath::*;

benches/mat.rs

@@ -20,7 +20,7 @@ extern crate cgmath;
 extern crate rand;
 extern crate test;
 
-use rand::{Rng, SeedableRng, rngs::SmallRng};
+use rand::{rngs::SmallRng, Rng, SeedableRng};
 use std::ops::*;
 use test::Bencher;
 

benches/quat.rs

@@ -20,7 +20,7 @@ extern crate cgmath;
 extern crate rand;
 extern crate test;
 
-use rand::{Rng, SeedableRng, rngs::SmallRng};
+use rand::{rngs::SmallRng, Rng, SeedableRng};
 use std::ops::*;
 use test::Bencher;
 

benches/vec.rs

@@ -20,7 +20,7 @@ extern crate cgmath;
 extern crate rand;
 extern crate test;
 
-use rand::{Rng, SeedableRng, rngs::SmallRng};
+use rand::{rngs::SmallRng, Rng, SeedableRng};
 use std::ops::*;
 use test::Bencher;
 

src/point.rs

@@ -17,7 +17,7 @@
 //! distinguishes them from vectors, which have a length and direction, but do
 //! not have a fixed position.
 
-use num_traits::{Float, Bounded, NumCast};
+use num_traits::{Bounded, Float, NumCast};
 use std::fmt;
 use std::mem;
 use std::ops::*;

src/quaternion.rs

@@ -106,7 +106,14 @@ impl<S: BaseFloat> Quaternion<S> {
     }
 
     /// Do a normalized linear interpolation with `other`, by `amount`.
-    pub fn nlerp(self, other: Quaternion<S>, amount: S) -> Quaternion<S> {
+    /// 
+    /// This takes the shortest path, so if the quaternions have a negative
+    /// dot product, the interpolation will be between `self` and `-other`.
+    pub fn nlerp(self, mut other: Quaternion<S>, amount: S) -> Quaternion<S> {
+        if self.dot(other) < S::zero() {
+            other = -other;
+        }
+
         (self * (S::one() - amount) + other * amount).normalize()
     }
 
@@ -114,6 +121,9 @@ impl<S: BaseFloat> Quaternion<S> {
     ///
     /// Return the spherical linear interpolation between the quaternion and
     /// `other`. Both quaternions should be normalized first.
+    /// 
+    /// This takes the shortest path, so if the quaternions have a negative
+    /// dot product, the interpolation will be between `self` and `-other`.
     ///
     /// # Performance notes
     ///
@@ -126,30 +136,28 @@ impl<S: BaseFloat> Quaternion<S> {
     ///   (http://number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/)
     /// - [Arcsynthesis OpenGL tutorial]
     ///   (http://www.arcsynthesis.org/gltut/Positioning/Tut08%20Interpolation.html)
-    pub fn slerp(self, other: Quaternion<S>, amount: S) -> Quaternion<S> {
+    pub fn slerp(self, mut other: Quaternion<S>, amount: S) -> Quaternion<S> {
-        let dot = self.dot(other);
+        let mut dot = self.dot(other);
-        let dot_threshold = cast(0.9995f64).unwrap();
+        let dot_threshold: S = cast(0.9995f64).unwrap();
+
+        if dot < S::zero() {
+            other = -other;
+            dot = -dot;
+        }
 
         // if quaternions are close together use `nlerp`
         if dot > dot_threshold {
             self.nlerp(other, amount)
         } else {
             // stay within the domain of acos()
-            // TODO REMOVE WHEN https://github.com/mozilla/rust/issues/12068 IS RESOLVED
+            let robust_dot = dot.min(S::one()).max(-S::one());
-            let robust_dot = if dot > S::one() {
-                S::one()
-            } else if dot < -S::one() {
-                -S::one()
-            } else {
-                dot
-            };
 
-            let theta = Rad::acos(robust_dot.clone());
+            let theta = Rad::acos(robust_dot);
 
             let scale1 = Rad::sin(theta * (S::one() - amount));
             let scale2 = Rad::sin(theta * amount);
 
-            (self * scale1 + other * scale2) * Rad::sin(theta).recip()
+            (self * scale1 + other * scale2).normalize()
         }
     }
 
@@ -758,4 +766,197 @@ mod tests {
             assert_eq!(v, &QUATERNION);
         }
     }
+
+    #[test]
+    fn test_nlerp_same() {
+        let q = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
+        assert_ulps_eq!(q, q.nlerp(q, 0.1234));
+    }
+
+    #[test]
+    fn test_nlerp_start() {
+        let q = Quaternion::from([0.5f64.sqrt(), 0.0, 0.5f64.sqrt(), 0.0]);
+        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
+        assert_ulps_eq!(q, q.nlerp(r, 0.0));
+    }
+
+    #[test]
+    fn test_nlerp_end() {
+        let q = Quaternion::from([0.5f64.sqrt(), 0.0, 0.5f64.sqrt(), 0.0]);
+        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
+        assert_ulps_eq!(r, q.nlerp(r, 1.0));
+    }
+
+    #[test]
+    fn test_nlerp_half() {
+        let q = Quaternion::from([-0.5, 0.5, 0.5, 0.5]);
+        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
+
+        let expected =
+            Quaternion::from([0.0, 1.0 / 3f64.sqrt(), 1.0 / 3f64.sqrt(), 1.0 / 3f64.sqrt()]);
+        assert_ulps_eq!(expected, q.nlerp(r, 0.5));
+    }
+
+    #[test]
+    fn test_nlerp_quarter() {
+        let q = Quaternion::from([-0.5, 0.5, 0.5, 0.5]);
+        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
+
+        let expected = Quaternion::from([
+            -1.0 / 13f64.sqrt(),
+            2.0 / 13f64.sqrt(),
+            2.0 / 13f64.sqrt(),
+            2.0 / 13f64.sqrt(),
+        ]);
+        assert_ulps_eq!(expected, q.nlerp(r, 0.25));
+    }
+
+    #[test]
+    fn test_nlerp_zero_dot() {
+        let q = Quaternion::from([-0.5, -0.5, 0.5, 0.5]);
+        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
+
+        let expected = Quaternion::from([
+            -1.0 / 10f64.sqrt(),
+            -1.0 / 10f64.sqrt(),
+            2.0 / 10f64.sqrt(),
+            2.0 / 10f64.sqrt(),
+        ]);
+        assert_ulps_eq!(expected, q.nlerp(r, 0.25));
+    }
+
+    #[test]
+    fn test_nlerp_negative_dot() {
+        let q = Quaternion::from([-0.5, -0.5, -0.5, 0.5]);
+        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
+
+        let expected = Quaternion::from([
+            -2.0 / 13f64.sqrt(),
+            -2.0 / 13f64.sqrt(),
+            -2.0 / 13f64.sqrt(),
+            1.0 / 13f64.sqrt(),
+        ]);
+        assert_ulps_eq!(expected, q.nlerp(r, 0.25));
+    }
+
+    #[test]
+    fn test_nlerp_opposite() {
+        let q = Quaternion::from([-0.5, -0.5, -0.5, -0.5]);
+        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
+
+        assert_ulps_eq!(q, q.nlerp(r, 0.25));
+        assert_ulps_eq!(q, q.nlerp(r, 0.75));
+    }
+
+    #[test]
+    fn test_nlerp_extrapolate() {
+        let q = Quaternion::from([-0.5, -0.5, -0.5, 0.5]);
+        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
+
+        let expected = Quaternion::from([
+            -1.0 / 12f64.sqrt(),
+            -1.0 / 12f64.sqrt(),
+            -1.0 / 12f64.sqrt(),
+            3.0 / 12f64.sqrt(),
+        ]);
+        assert_ulps_eq!(expected, q.nlerp(r, -1.0));
+    }
+
+    #[test]
+    fn test_slerp_same() {
+        let q = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
+        assert_ulps_eq!(q, q.slerp(q, 0.1234));
+    }
+
+    #[test]
+    fn test_slerp_start() {
+        let q = Quaternion::from([0.5f64.sqrt(), 0.0, 0.5f64.sqrt(), 0.0]);
+        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
+        assert_ulps_eq!(q, q.slerp(r, 0.0));
+    }
+
+    #[test]
+    fn test_slerp_end() {
+        let q = Quaternion::from([0.5f64.sqrt(), 0.0, 0.5f64.sqrt(), 0.0]);
+        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
+        assert_ulps_eq!(r, q.slerp(r, 1.0));
+    }
+
+    #[test]
+    fn test_slerp_half() {
+        let q = Quaternion::from([-0.5, 0.5, 0.5, 0.5]);
+        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
+
+        let expected =
+            Quaternion::from([0.0, 1.0 / 3f64.sqrt(), 1.0 / 3f64.sqrt(), 1.0 / 3f64.sqrt()]);
+        assert_ulps_eq!(expected, q.slerp(r, 0.5));
+    }
+
+    #[test]
+    fn test_slerp_quarter() {
+        let q = Quaternion::from([-0.5, 0.5, 0.5, 0.5]);
+        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
+
+        let expected = Quaternion::from([
+            -0.2588190451025208,
+            0.5576775358252053,
+            0.5576775358252053,
+            0.5576775358252053,
+        ]);
+        assert_ulps_eq!(expected, q.slerp(r, 0.25));
+    }
+
+    #[test]
+    fn test_slerp_zero_dot() {
+        let q = Quaternion::from([-0.5, -0.5, 0.5, 0.5]);
+        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
+
+        let expected = Quaternion::from([
+            -0.27059805007309845,
+            -0.27059805007309845,
+            0.6532814824381883,
+            0.6532814824381883,
+        ]);
+        assert_ulps_eq!(expected, q.slerp(r, 0.25));
+    }
+
+    #[test]
+    fn test_slerp_negative_dot() {
+        let q = Quaternion::from([-0.5, -0.5, -0.5, 0.5]);
+        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
+
+        let expected = Quaternion::from([
+            -0.5576775358252053,
+            -0.5576775358252053,
+            -0.5576775358252053,
+            0.2588190451025208
+        ]);
+        assert_ulps_eq!(expected, q.slerp(r, 0.25));
+    }
+
+    #[test]
+    fn test_slerp_opposite() {
+        let q = Quaternion::from([-0.5, -0.5, -0.5, -0.5]);
+        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
+
+        assert_ulps_eq!(q, q.slerp(r, 0.25));
+        assert_ulps_eq!(q, q.slerp(r, 0.75));
+    }
+
+    #[test]
+    fn test_slerp_extrapolate() {
+        let q = Quaternion::from([-0.5, -0.5, -0.5, 0.5]);
+        let r = Quaternion::from([0.5, 0.5, 0.5, 0.5]);
+
+        let expected = Quaternion::from([0.0, 0.0, 0.0, 1.0]);
+        assert_ulps_eq!(expected, q.slerp(r, -1.0));
+    }
+
+    #[test]
+    fn test_slerp_regression() {
+        let a = Quaternion::<f32>::new(0.00052311074, 0.9999999, 0.00014682197, -0.000016342687);
+        let b = Quaternion::<f32>::new(0.019973433, -0.99980056, -0.00015678025, 0.000013882192);
+
+        assert_ulps_eq!(a.slerp(b, 0.5).magnitude(), 1.0);
+    }
 }

src/structure.rs

@@ -205,7 +205,10 @@ pub trait MetricSpace: Sized {
     fn distance2(self, other: Self) -> Self::Metric;
 
     /// The distance between two values.
-    fn distance(self, other: Self) -> Self::Metric where Self::Metric: Float {
+    fn distance(self, other: Self) -> Self::Metric
+    where
+        Self::Metric: Float,
+    {
         Float::sqrt(Self::distance2(self, other))
     }
 }
@@ -220,14 +223,17 @@ pub trait MetricSpace: Sized {
 pub trait InnerSpace: VectorSpace
 where
     // FIXME: Ugly type signatures - blocked by rust-lang/rust#24092
-    Self: MetricSpace<Metric = <Self as VectorSpace>::Scalar>
+    Self: MetricSpace<Metric = <Self as VectorSpace>::Scalar>,
 {
     /// Vector dot (or inner) product.
     fn dot(self, other: Self) -> Self::Scalar;
 
     /// Returns `true` if the vector is perpendicular (at right angles) to the
     /// other vector.
-    fn is_perpendicular(self, other: Self) -> bool where Self::Scalar: approx::UlpsEq {
+    fn is_perpendicular(self, other: Self) -> bool
+    where
+        Self::Scalar: approx::UlpsEq,
+    {
         ulps_eq!(Self::dot(self, other), &Self::Scalar::zero())
     }
 
@@ -242,7 +248,10 @@ where
     }
 
     /// Returns the angle between two vectors in radians.
-    fn angle(self, other: Self) -> Rad<Self::Scalar> where Self::Scalar: BaseFloat {
+    fn angle(self, other: Self) -> Rad<Self::Scalar>
+    where
+        Self::Scalar: BaseFloat,
+    {
         Rad::acos(Self::dot(self, other) / (self.magnitude() * other.magnitude()))
     }
 
@@ -256,19 +265,28 @@ where
 
     /// The distance from the tail to the tip of the vector.
     #[inline]
-    fn magnitude(self) -> Self::Scalar where Self::Scalar: Float {
+    fn magnitude(self) -> Self::Scalar
+    where
+        Self::Scalar: Float,
+    {
         Float::sqrt(self.magnitude2())
     }
 
     /// Returns a vector with the same direction, but with a magnitude of `1`.
     #[inline]
-    fn normalize(self) -> Self where Self::Scalar: Float {
+    fn normalize(self) -> Self
+    where
+        Self::Scalar: Float,
+    {
         self.normalize_to(Self::Scalar::one())
     }
 
     /// Returns a vector with the same direction and a given magnitude.
     #[inline]
-    fn normalize_to(self, magnitude: Self::Scalar) -> Self where Self::Scalar: Float {
+    fn normalize_to(self, magnitude: Self::Scalar) -> Self
+    where
+        Self::Scalar: Float,
+    {
         self * (magnitude / self.magnitude())
     }
 }
@@ -536,14 +554,20 @@ where
 
     /// Test if this matrix is invertible.
     #[inline]
-    fn is_invertible(&self) -> bool where Self::Scalar: approx::UlpsEq {
+    fn is_invertible(&self) -> bool
+    where
+        Self::Scalar: approx::UlpsEq,
+    {
         ulps_ne!(self.determinant(), &Self::Scalar::zero())
     }
 
     /// Test if this matrix is the identity matrix. That is, it is diagonal
     /// and every element in the diagonal is one.
     #[inline]
-    fn is_identity(&self) -> bool where Self: approx::UlpsEq {
+    fn is_identity(&self) -> bool
+    where
+        Self: approx::UlpsEq,
+    {
         ulps_eq!(self, &Self::identity())
     }
 

src/vector.rs

@@ -13,7 +13,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-use num_traits::{Float, Bounded, NumCast};
+use num_traits::{Bounded, Float, NumCast};
 #[cfg(feature = "rand")]
 use rand::{
     distributions::{Distribution, Standard},
@@ -539,7 +539,10 @@ impl<S: BaseNum> InnerSpace for Vector2<S> {
     }
 
     #[inline]
-    fn angle(self, other: Vector2<S>) -> Rad<S> where S: BaseFloat {
+    fn angle(self, other: Vector2<S>) -> Rad<S>
+    where
+        S: BaseFloat,
+    {
         Rad::atan2(Self::perp_dot(self, other), Self::dot(self, other))
     }
 }
@@ -551,7 +554,10 @@ impl<S: BaseNum> InnerSpace for Vector3<S> {
     }
 
     #[inline]
-    fn angle(self, other: Vector3<S>) -> Rad<S> where S: BaseFloat {
+    fn angle(self, other: Vector3<S>) -> Rad<S>
+    where
+        S: BaseFloat,
+    {
         Rad::atan2(self.cross(other).magnitude(), Self::dot(self, other))
     }
 }