Don't panic ScLERPing `UnitDualQuaternion` with equal rotation
Solves #1013. Previously, when screw-linearly interpolating two unit dual quaternions that had an identical orientation, `try_sclerp` would return `None`, as the operation would introduce a division-by-zero. This PR splits out the cases where two unit dual quaternions have an identical orientation from the cases where they have opposite orientations. In the case where they have identical orientations, the operation is well-defined, but the exponential parameterization could not handle it without introducing NaNs. Therefore, the function detects this case and simply defaults to linearly interpolating the translational components and using one of the two inputs' rotation components. The case where the inputs have opposite rotations is now detected separately using the dot product of the real (rotation) parts, which was already being computed anyway. Also introduces proptests for these specific scenarios, to avoid any regression.
This commit is contained in:
parent
e05bfe48b3
commit
dfb7b6af22
|
@ -57,10 +57,7 @@ impl<T: Scalar> PartialEq for DualQuaternion<T> {
|
||||||
|
|
||||||
impl<T: Scalar + Zero> Default for DualQuaternion<T> {
|
impl<T: Scalar + Zero> Default for DualQuaternion<T> {
|
||||||
fn default() -> Self {
|
fn default() -> Self {
|
||||||
Self {
|
Self { real: Quaternion::default(), dual: Quaternion::default() }
|
||||||
real: Quaternion::default(),
|
|
||||||
dual: Quaternion::default(),
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -357,7 +354,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 quaternions. May be used to represent a rotation followed by a
|
||||||
|
/// translation.
|
||||||
pub type UnitDualQuaternion<T> = Unit<DualQuaternion<T>>;
|
pub type UnitDualQuaternion<T> = Unit<DualQuaternion<T>>;
|
||||||
|
|
||||||
impl<T: Scalar + ClosedNeg + PartialEq + SimdRealField> PartialEq for UnitDualQuaternion<T> {
|
impl<T: Scalar + ClosedNeg + PartialEq + SimdRealField> PartialEq for UnitDualQuaternion<T> {
|
||||||
|
@ -474,9 +472,7 @@ where
|
||||||
#[inline]
|
#[inline]
|
||||||
#[must_use = "Did you mean to use inverse_mut()?"]
|
#[must_use = "Did you mean to use inverse_mut()?"]
|
||||||
pub fn inverse(&self) -> Self {
|
pub fn inverse(&self) -> Self {
|
||||||
let real = Unit::new_unchecked(self.as_ref().real.clone())
|
let real = Unit::new_unchecked(self.as_ref().real.clone()).inverse().into_inner();
|
||||||
.inverse()
|
|
||||||
.into_inner();
|
|
||||||
let dual = -real.clone() * self.as_ref().dual.clone() * real.clone();
|
let dual = -real.clone() * self.as_ref().dual.clone() * real.clone();
|
||||||
UnitDualQuaternion::new_unchecked(DualQuaternion { real, dual })
|
UnitDualQuaternion::new_unchecked(DualQuaternion { real, dual })
|
||||||
}
|
}
|
||||||
|
@ -498,9 +494,7 @@ where
|
||||||
#[inline]
|
#[inline]
|
||||||
pub fn inverse_mut(&mut self) {
|
pub fn inverse_mut(&mut self) {
|
||||||
let quat = self.as_mut_unchecked();
|
let quat = self.as_mut_unchecked();
|
||||||
quat.real = Unit::new_unchecked(quat.real.clone())
|
quat.real = Unit::new_unchecked(quat.real.clone()).inverse().into_inner();
|
||||||
.inverse()
|
|
||||||
.into_inner();
|
|
||||||
quat.dual = -quat.real.clone() * quat.dual.clone() * quat.real.clone();
|
quat.dual = -quat.real.clone() * quat.dual.clone() * quat.real.clone();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -593,8 +587,9 @@ where
|
||||||
/// Screw linear interpolation between two unit quaternions. This creates a
|
/// Screw linear interpolation between two unit quaternions. This creates a
|
||||||
/// smooth arc from one dual-quaternion to another.
|
/// smooth arc from one dual-quaternion to another.
|
||||||
///
|
///
|
||||||
/// Panics if the angle between both quaternion is 180 degrees (in which case the interpolation
|
/// Panics if the angle between both quaternion is 180 degrees (in which
|
||||||
/// is not well-defined). Use `.try_sclerp` instead to avoid the panic.
|
/// case the interpolation is not well-defined). Use `.try_sclerp`
|
||||||
|
/// instead to avoid the panic.
|
||||||
///
|
///
|
||||||
/// # Example
|
/// # Example
|
||||||
/// ```
|
/// ```
|
||||||
|
@ -627,15 +622,16 @@ where
|
||||||
.expect("DualQuaternion sclerp: ambiguous configuration.")
|
.expect("DualQuaternion sclerp: ambiguous configuration.")
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Computes the screw-linear interpolation between two unit quaternions or returns `None`
|
/// Computes the screw-linear interpolation between two unit quaternions or
|
||||||
/// if both quaternions are approximately 180 degrees apart (in which case the interpolation is
|
/// returns `None` if both quaternions are approximately 180 degrees
|
||||||
/// not well-defined).
|
/// apart (in which case the interpolation is not well-defined).
|
||||||
///
|
///
|
||||||
/// # Arguments
|
/// # Arguments
|
||||||
/// * `self`: the first quaternion to interpolate from.
|
/// * `self`: the first quaternion to interpolate from.
|
||||||
/// * `other`: the second quaternion to interpolate toward.
|
/// * `other`: the second quaternion to interpolate toward.
|
||||||
/// * `t`: the interpolation parameter. Should be between 0 and 1.
|
/// * `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`.
|
/// must be to return `None`.
|
||||||
#[inline]
|
#[inline]
|
||||||
#[must_use]
|
#[must_use]
|
||||||
|
@ -650,6 +646,10 @@ where
|
||||||
// interpolation.
|
// interpolation.
|
||||||
let other = {
|
let other = {
|
||||||
let dot_product = self.as_ref().real.coords.dot(&other.as_ref().real.coords);
|
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() {
|
if dot_product < T::zero() {
|
||||||
-other.clone()
|
-other.clone()
|
||||||
} else {
|
} else {
|
||||||
|
@ -660,17 +660,23 @@ where
|
||||||
let difference = self.as_ref().conjugate() * other.as_ref();
|
let difference = self.as_ref().conjugate() * other.as_ref();
|
||||||
let norm_squared = difference.real.vector().norm_squared();
|
let norm_squared = difference.real.vector().norm_squared();
|
||||||
if relative_eq!(norm_squared, T::zero(), epsilon = epsilon) {
|
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 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 mut pitch = -two * difference.dual.scalar() * inverse_norm.clone();
|
||||||
let direction = difference.real.vector() * inverse_norm.clone();
|
let direction = difference.real.vector() * inverse_norm.clone();
|
||||||
let moment = (difference.dual.vector()
|
let moment = (difference.dual.vector()
|
||||||
- direction.clone() * (pitch.clone() * difference.real.scalar() * half.clone()))
|
- direction.clone()
|
||||||
|
* (pitch.clone() * difference.real.scalar() * half.clone()))
|
||||||
* inverse_norm;
|
* inverse_norm;
|
||||||
|
|
||||||
angle *= t.clone();
|
angle *= t.clone();
|
||||||
|
@ -678,6 +684,7 @@ where
|
||||||
|
|
||||||
let sin = (half.clone() * angle.clone()).sin();
|
let sin = (half.clone() * angle.clone()).sin();
|
||||||
let cos = (half.clone() * angle).cos();
|
let cos = (half.clone() * angle).cos();
|
||||||
|
|
||||||
let real = Quaternion::from_parts(cos.clone(), direction.clone() * sin.clone());
|
let real = Quaternion::from_parts(cos.clone(), direction.clone() * sin.clone());
|
||||||
let dual = Quaternion::from_parts(
|
let dual = Quaternion::from_parts(
|
||||||
-pitch.clone() * half.clone() * sin.clone(),
|
-pitch.clone() * half.clone() * sin.clone(),
|
||||||
|
@ -967,8 +974,7 @@ impl<T: RealField + RelativeEq<Epsilon = T>> RelativeEq for UnitDualQuaternion<T
|
||||||
epsilon: Self::Epsilon,
|
epsilon: Self::Epsilon,
|
||||||
max_relative: Self::Epsilon,
|
max_relative: Self::Epsilon,
|
||||||
) -> bool {
|
) -> bool {
|
||||||
self.as_ref()
|
self.as_ref().relative_eq(other.as_ref(), epsilon, max_relative)
|
||||||
.relative_eq(other.as_ref(), epsilon, max_relative)
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -1,7 +1,7 @@
|
||||||
#![cfg(feature = "proptest-support")]
|
#![cfg(feature = "proptest-support")]
|
||||||
#![allow(non_snake_case)]
|
#![allow(non_snake_case)]
|
||||||
|
|
||||||
use na::{DualQuaternion, Point3, UnitDualQuaternion, Vector3};
|
use na::{DualQuaternion, Point3, Unit, UnitDualQuaternion, UnitQuaternion, Vector3};
|
||||||
|
|
||||||
use crate::proptest::*;
|
use crate::proptest::*;
|
||||||
use proptest::{prop_assert, 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));
|
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)]
|
#[cfg_attr(rustfmt, rustfmt_skip)]
|
||||||
#[test]
|
#[test]
|
||||||
fn all_op_exist(
|
fn all_op_exist(
|
||||||
|
|
Loading…
Reference in New Issue