diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..7a73a41 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,2 @@ +{ +} \ No newline at end of file diff --git a/benches/construction.rs b/benches/construction.rs index ea0796b..f672474 100644 --- a/benches/construction.rs +++ b/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::*; diff --git a/benches/mat.rs b/benches/mat.rs index d9e93c4..c8130c6 100644 --- a/benches/mat.rs +++ b/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; diff --git a/benches/quat.rs b/benches/quat.rs index a3e2338..31f9131 100644 --- a/benches/quat.rs +++ b/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; diff --git a/benches/vec.rs b/benches/vec.rs index 0e3060b..7b197af 100644 --- a/benches/vec.rs +++ b/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; diff --git a/src/point.rs b/src/point.rs index 5eb83c6..faf2a6d 100644 --- a/src/point.rs +++ b/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::*; diff --git a/src/quaternion.rs b/src/quaternion.rs index 57a6131..3f704a1 100644 --- a/src/quaternion.rs +++ b/src/quaternion.rs @@ -106,7 +106,14 @@ impl Quaternion { } /// Do a normalized linear interpolation with `other`, by `amount`. - pub fn nlerp(self, other: Quaternion, amount: S) -> Quaternion { + /// + /// 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, amount: S) -> Quaternion { + if self.dot(other) < S::zero() { + other = -other; + } + (self * (S::one() - amount) + other * amount).normalize() } @@ -114,6 +121,9 @@ impl Quaternion { /// /// 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 Quaternion { /// (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, amount: S) -> Quaternion { - let dot = self.dot(other); - let dot_threshold = cast(0.9995f64).unwrap(); + pub fn slerp(self, mut other: Quaternion, amount: S) -> Quaternion { + let mut dot = self.dot(other); + 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 = if dot > S::one() { - S::one() - } else if dot < -S::one() { - -S::one() - } else { - dot - }; + let robust_dot = dot.min(S::one()).max(-S::one()); - 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::::new(0.00052311074, 0.9999999, 0.00014682197, -0.000016342687); + let b = Quaternion::::new(0.019973433, -0.99980056, -0.00015678025, 0.000013882192); + + assert_ulps_eq!(a.slerp(b, 0.5).magnitude(), 1.0); + } } diff --git a/src/structure.rs b/src/structure.rs index 22cd2b0..9fe7928 100644 --- a/src/structure.rs +++ b/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::Scalar> + Self: MetricSpace::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 where Self::Scalar: BaseFloat { + fn angle(self, other: Self) -> Rad + 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()) } diff --git a/src/vector.rs b/src/vector.rs index f22c59b..adcc581 100644 --- a/src/vector.rs +++ b/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 InnerSpace for Vector2 { } #[inline] - fn angle(self, other: Vector2) -> Rad where S: BaseFloat { + fn angle(self, other: Vector2) -> Rad + where + S: BaseFloat, + { Rad::atan2(Self::perp_dot(self, other), Self::dot(self, other)) } } @@ -551,7 +554,10 @@ impl InnerSpace for Vector3 { } #[inline] - fn angle(self, other: Vector3) -> Rad where S: BaseFloat { + fn angle(self, other: Vector3) -> Rad + where + S: BaseFloat, + { Rad::atan2(self.cross(other).magnitude(), Self::dot(self, other)) } }