Merge pull request #1016 from tpdickso/fix-dual-quaternion-sclerp
Don't panic ScLERPing `UnitDualQuaternion` with equal rotation
This commit is contained in:
commit
10150ec783
|
@ -357,7 +357,8 @@ impl<T: RealField + UlpsEq<Epsilon = T>> UlpsEq for DualQuaternion<T> {
|
|||
}
|
||||
}
|
||||
|
||||
/// A unit quaternions. May be used to represent a rotation followed by a translation.
|
||||
/// A unit dual quaternion. May be used to represent a rotation followed by a
|
||||
/// translation.
|
||||
pub type UnitDualQuaternion<T> = Unit<DualQuaternion<T>>;
|
||||
|
||||
impl<T: Scalar + ClosedNeg + PartialEq + SimdRealField> PartialEq for UnitDualQuaternion<T> {
|
||||
|
@ -593,8 +594,9 @@ where
|
|||
/// Screw linear interpolation between two unit quaternions. This creates a
|
||||
/// smooth arc from one dual-quaternion to another.
|
||||
///
|
||||
/// Panics if the angle between both quaternion is 180 degrees (in which case the interpolation
|
||||
/// is not well-defined). Use `.try_sclerp` instead to avoid the panic.
|
||||
/// Panics if the angle between both quaternion is 180 degrees (in which
|
||||
/// case the interpolation is not well-defined). Use `.try_sclerp`
|
||||
/// instead to avoid the panic.
|
||||
///
|
||||
/// # Example
|
||||
/// ```
|
||||
|
@ -627,15 +629,16 @@ where
|
|||
.expect("DualQuaternion sclerp: ambiguous configuration.")
|
||||
}
|
||||
|
||||
/// Computes the screw-linear interpolation between two unit quaternions or returns `None`
|
||||
/// if both quaternions are approximately 180 degrees apart (in which case the interpolation is
|
||||
/// not well-defined).
|
||||
/// Computes the screw-linear interpolation between two unit quaternions or
|
||||
/// returns `None` if both quaternions are approximately 180 degrees
|
||||
/// apart (in which case the interpolation is not well-defined).
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `self`: the first quaternion to interpolate from.
|
||||
/// * `other`: the second quaternion to interpolate toward.
|
||||
/// * `t`: the interpolation parameter. Should be between 0 and 1.
|
||||
/// * `epsilon`: the value below which the sinus of the angle separating both quaternion
|
||||
/// * `epsilon`: the value below which the sinus of the angle separating
|
||||
/// both quaternion
|
||||
/// must be to return `None`.
|
||||
#[inline]
|
||||
#[must_use]
|
||||
|
@ -650,6 +653,10 @@ where
|
|||
// interpolation.
|
||||
let other = {
|
||||
let dot_product = self.as_ref().real.coords.dot(&other.as_ref().real.coords);
|
||||
if relative_eq!(dot_product, T::zero(), epsilon = epsilon.clone()) {
|
||||
return None;
|
||||
}
|
||||
|
||||
if dot_product < T::zero() {
|
||||
-other.clone()
|
||||
} else {
|
||||
|
@ -660,13 +667,21 @@ where
|
|||
let difference = self.as_ref().conjugate() * other.as_ref();
|
||||
let norm_squared = difference.real.vector().norm_squared();
|
||||
if relative_eq!(norm_squared, T::zero(), epsilon = epsilon) {
|
||||
return None;
|
||||
return Some(Self::from_parts(
|
||||
self.translation()
|
||||
.vector
|
||||
.lerp(&other.translation().vector, t)
|
||||
.into(),
|
||||
self.rotation(),
|
||||
));
|
||||
}
|
||||
|
||||
let inverse_norm_squared = T::one() / norm_squared;
|
||||
let scalar: T = difference.real.scalar();
|
||||
let mut angle = two.clone() * scalar.acos();
|
||||
|
||||
let inverse_norm_squared: T = T::one() / norm_squared;
|
||||
let inverse_norm = inverse_norm_squared.sqrt();
|
||||
|
||||
let mut angle = two.clone() * difference.real.scalar().acos();
|
||||
let mut pitch = -two * difference.dual.scalar() * inverse_norm.clone();
|
||||
let direction = difference.real.vector() * inverse_norm.clone();
|
||||
let moment = (difference.dual.vector()
|
||||
|
@ -678,6 +693,7 @@ where
|
|||
|
||||
let sin = (half.clone() * angle.clone()).sin();
|
||||
let cos = (half.clone() * angle).cos();
|
||||
|
||||
let real = Quaternion::from_parts(cos.clone(), direction.clone() * sin.clone());
|
||||
let dual = Quaternion::from_parts(
|
||||
-pitch.clone() * half.clone() * sin.clone(),
|
||||
|
|
|
@ -1,7 +1,7 @@
|
|||
#![cfg(feature = "proptest-support")]
|
||||
#![allow(non_snake_case)]
|
||||
|
||||
use na::{DualQuaternion, Point3, UnitDualQuaternion, Vector3};
|
||||
use na::{DualQuaternion, Point3, Unit, UnitDualQuaternion, UnitQuaternion, Vector3};
|
||||
|
||||
use crate::proptest::*;
|
||||
use proptest::{prop_assert, proptest};
|
||||
|
@ -74,6 +74,98 @@ proptest!(
|
|||
prop_assert!(relative_eq!((dq * t) * p, dq * (t * p), epsilon = 1.0e-7));
|
||||
}
|
||||
|
||||
#[cfg_attr(rustfmt, rustfmt_skip)]
|
||||
#[test]
|
||||
fn sclerp_is_defined_for_identical_orientations(
|
||||
dq in unit_dual_quaternion(),
|
||||
s in -1.0f64..2.0f64,
|
||||
t in translation3(),
|
||||
) {
|
||||
// Should not panic.
|
||||
prop_assert!(relative_eq!(dq.sclerp(&dq, 0.0), dq, epsilon = 1.0e-7));
|
||||
prop_assert!(relative_eq!(dq.sclerp(&dq, 0.5), dq, epsilon = 1.0e-7));
|
||||
prop_assert!(relative_eq!(dq.sclerp(&dq, 1.0), dq, epsilon = 1.0e-7));
|
||||
prop_assert!(relative_eq!(dq.sclerp(&dq, s), dq, epsilon = 1.0e-7));
|
||||
|
||||
let unit = UnitDualQuaternion::identity();
|
||||
prop_assert!(relative_eq!(unit.sclerp(&unit, 0.0), unit, epsilon = 1.0e-7));
|
||||
prop_assert!(relative_eq!(unit.sclerp(&unit, 0.5), unit, epsilon = 1.0e-7));
|
||||
prop_assert!(relative_eq!(unit.sclerp(&unit, 1.0), unit, epsilon = 1.0e-7));
|
||||
prop_assert!(relative_eq!(unit.sclerp(&unit, s), unit, epsilon = 1.0e-7));
|
||||
|
||||
// ScLERPing two unit dual quaternions with nearly equal rotation
|
||||
// components should result in a unit dual quaternion with a rotation
|
||||
// component nearly equal to either input.
|
||||
let dq2 = t * dq;
|
||||
prop_assert!(relative_eq!(dq.sclerp(&dq2, 0.0).real, dq.real, epsilon = 1.0e-7));
|
||||
prop_assert!(relative_eq!(dq.sclerp(&dq2, 0.5).real, dq.real, epsilon = 1.0e-7));
|
||||
prop_assert!(relative_eq!(dq.sclerp(&dq2, 1.0).real, dq.real, epsilon = 1.0e-7));
|
||||
prop_assert!(relative_eq!(dq.sclerp(&dq2, s).real, dq.real, epsilon = 1.0e-7));
|
||||
|
||||
// ScLERPing two unit dual quaternions with nearly equal rotation
|
||||
// components should result in a unit dual quaternion with a translation
|
||||
// component which is nearly equal to linearly interpolating the
|
||||
// translation components of the inputs.
|
||||
prop_assert!(relative_eq!(
|
||||
dq.sclerp(&dq2, s).translation().vector,
|
||||
dq.translation().vector.lerp(&dq2.translation().vector, s),
|
||||
epsilon = 1.0e-7
|
||||
));
|
||||
|
||||
let unit2 = t * unit;
|
||||
prop_assert!(relative_eq!(unit.sclerp(&unit2, 0.0).real, unit.real, epsilon = 1.0e-7));
|
||||
prop_assert!(relative_eq!(unit.sclerp(&unit2, 0.5).real, unit.real, epsilon = 1.0e-7));
|
||||
prop_assert!(relative_eq!(unit.sclerp(&unit2, 1.0).real, unit.real, epsilon = 1.0e-7));
|
||||
prop_assert!(relative_eq!(unit.sclerp(&unit2, s).real, unit.real, epsilon = 1.0e-7));
|
||||
|
||||
prop_assert!(relative_eq!(
|
||||
unit.sclerp(&unit2, s).translation().vector,
|
||||
unit.translation().vector.lerp(&unit2.translation().vector, s),
|
||||
epsilon = 1.0e-7
|
||||
));
|
||||
}
|
||||
|
||||
#[cfg_attr(rustfmt, rustfmt_skip)]
|
||||
#[test]
|
||||
fn sclerp_is_not_defined_for_opposite_orientations(
|
||||
dq in unit_dual_quaternion(),
|
||||
s in 0.1f64..0.9f64,
|
||||
t in translation3(),
|
||||
t2 in translation3(),
|
||||
v in vector3(),
|
||||
) {
|
||||
let iso = dq.to_isometry();
|
||||
let rot = iso.rotation;
|
||||
if let Some((axis, angle)) = rot.axis_angle() {
|
||||
let flipped = UnitQuaternion::from_axis_angle(&axis, angle + std::f64::consts::PI);
|
||||
let dqf = flipped * rot.inverse() * dq.clone();
|
||||
prop_assert!(dq.try_sclerp(&dqf, 0.5, 1.0e-7).is_none());
|
||||
prop_assert!(dq.try_sclerp(&dqf, s, 1.0e-7).is_none());
|
||||
}
|
||||
|
||||
let dq2 = t * dq;
|
||||
let iso2 = dq2.to_isometry();
|
||||
let rot2 = iso2.rotation;
|
||||
if let Some((axis, angle)) = rot2.axis_angle() {
|
||||
let flipped = UnitQuaternion::from_axis_angle(&axis, angle + std::f64::consts::PI);
|
||||
let dq3f = t2 * flipped * rot.inverse() * dq.clone();
|
||||
prop_assert!(dq2.try_sclerp(&dq3f, 0.5, 1.0e-7).is_none());
|
||||
prop_assert!(dq2.try_sclerp(&dq3f, s, 1.0e-7).is_none());
|
||||
}
|
||||
|
||||
if let Some(axis) = Unit::try_new(v, 1.0e-7) {
|
||||
let unit = UnitDualQuaternion::identity();
|
||||
let flip = UnitQuaternion::from_axis_angle(&axis, std::f64::consts::PI);
|
||||
let unitf = flip * unit;
|
||||
prop_assert!(unit.try_sclerp(&unitf, 0.5, 1.0e-7).is_none());
|
||||
prop_assert!(unit.try_sclerp(&unitf, s, 1.0e-7).is_none());
|
||||
|
||||
let unit2f = t * unit * flip;
|
||||
prop_assert!(unit.try_sclerp(&unit2f, 0.5, 1.0e-7).is_none());
|
||||
prop_assert!(unit.try_sclerp(&unit2f, s, 1.0e-7).is_none());
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg_attr(rustfmt, rustfmt_skip)]
|
||||
#[test]
|
||||
fn all_op_exist(
|
||||
|
|
Loading…
Reference in New Issue