M-Labs/nalgebra
3
0
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Don't panic ScLERPing UnitDualQuaternion with equal rotation

Browse Source 
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:
Terence 2021-10-27 23:59:27 -04:00
parent e05bfe48b3
commit dfb7b6af22
2 changed files with 122 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
src/geometry/dual_quaternion.rs
View File

@ -57,10 +57,7 @@ impl<T: Scalar> PartialEq for DualQuaternion<T> {
impl<T: Scalar + Zero> Default for DualQuaternion<T> {
fn default() -> Self {
Self {
real: Quaternion::default(),
dual: Quaternion::default(),
}
Self { 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>>;
impl<T: Scalar + ClosedNeg + PartialEq + SimdRealField> PartialEq for UnitDualQuaternion<T> {
@ -474,9 +472,7 @@ where
#[inline]
#[must_use = "Did you mean to use inverse_mut()?"]
pub fn inverse(&self) -> Self {
let real = Unit::new_unchecked(self.as_ref().real.clone())
.inverse()
.into_inner();
let real = Unit::new_unchecked(self.as_ref().real.clone()).inverse().into_inner();
let dual = -real.clone() * self.as_ref().dual.clone() * real.clone();
UnitDualQuaternion::new_unchecked(DualQuaternion { real, dual })
}
@ -498,9 +494,7 @@ where
#[inline]
pub fn inverse_mut(&mut self) {
let quat = self.as_mut_unchecked();
quat.real = Unit::new_unchecked(quat.real.clone())
.inverse()
.into_inner();
quat.real = Unit::new_unchecked(quat.real.clone()).inverse().into_inner();
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
/// 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 +622,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 +646,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,17 +660,23 @@ 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()
- direction.clone() * (pitch.clone() * difference.real.scalar() * half.clone()))
- direction.clone()
* (pitch.clone() * difference.real.scalar() * half.clone()))
* inverse_norm;
angle *= t.clone();
@ -678,6 +684,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(),
@ -967,8 +974,7 @@ impl<T: RealField + RelativeEq<Epsilon = T>> RelativeEq for UnitDualQuaternion<T
epsilon: Self::Epsilon,
max_relative: Self::Epsilon,
) -> bool {
self.as_ref()
.relative_eq(other.as_ref(), epsilon, max_relative)
self.as_ref().relative_eq(other.as_ref(), epsilon, max_relative)
}
}

94
tests/geometry/dual_quaternion.rs
View File

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