From dfb7b6af220c32cd317cd1debdc6055ee196ab2f Mon Sep 17 00:00:00 2001 From: Terence Date: Wed, 27 Oct 2021 23:59:27 -0400 Subject: [PATCH] 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. --- src/geometry/dual_quaternion.rs | 52 +++++++++-------- tests/geometry/dual_quaternion.rs | 94 ++++++++++++++++++++++++++++++- 2 files changed, 122 insertions(+), 24 deletions(-) diff --git a/src/geometry/dual_quaternion.rs b/src/geometry/dual_quaternion.rs index 8bbf11f1..72bc2c8b 100644 --- a/src/geometry/dual_quaternion.rs +++ b/src/geometry/dual_quaternion.rs @@ -57,10 +57,7 @@ impl PartialEq for DualQuaternion { impl Default for DualQuaternion { fn default() -> Self { - Self { - real: Quaternion::default(), - dual: Quaternion::default(), - } + Self { real: Quaternion::default(), dual: Quaternion::default() } } } @@ -357,7 +354,8 @@ impl> UlpsEq for DualQuaternion { } } -/// 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 = Unit>; impl PartialEq for UnitDualQuaternion { @@ -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> RelativeEq for UnitDualQuaternion bool { - self.as_ref() - .relative_eq(other.as_ref(), epsilon, max_relative) + self.as_ref().relative_eq(other.as_ref(), epsilon, max_relative) } } diff --git a/tests/geometry/dual_quaternion.rs b/tests/geometry/dual_quaternion.rs index 6cc975a5..1926fee9 100644 --- a/tests/geometry/dual_quaternion.rs +++ b/tests/geometry/dual_quaternion.rs @@ -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(