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(