diff --git a/src/geometry/dual_quaternion.rs b/src/geometry/dual_quaternion.rs index 8bbf11f1..e597cbf5 100644 --- a/src/geometry/dual_quaternion.rs +++ b/src/geometry/dual_quaternion.rs @@ -357,7 +357,8 @@ impl> UlpsEq for DualQuaternion { } } -/// 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 = Unit>; impl PartialEq for UnitDualQuaternion { @@ -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(), 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(