Fixed opposite quaternion slerp bug (#515)

* Fixed opposite quaternion slerp bug

* Removed an unnecessary

* nlerp and slerp always take the shortest path now
This commit is contained in:
josh65536 2020-08-11 23:04:27 -04:00 committed by GitHub
parent c96cd57efc
commit 816c043223
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
9 changed files with 264 additions and 31 deletions

2
.vscode/settings.json vendored Normal file
View file

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

View file

@ -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::*;

View file

@ -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;

View file

@ -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;

View file

@ -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;

View file

@ -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::*;

View file

@ -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()
}
@ -115,6 +122,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
///
/// The `acos` operation used in `slerp` is an expensive operation, so
@ -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> {
let dot = self.dot(other);
let dot_threshold = cast(0.9995f64).unwrap();
pub fn slerp(self, mut other: Quaternion<S>, amount: S) -> Quaternion<S> {
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::<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);
}
}

View file

@ -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())
}

View file

@ -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))
}
}