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

Merge pull request #1016 from tpdickso/fix-dual-quaternion-sclerp

Browse Source 
Don't panic ScLERPing `UnitDualQuaternion` with equal rotation
This commit is contained in:
Sébastien Crozet 2021-11-21 17:57:34 +01:00 committed by GitHub
commit 10150ec783
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 119 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
src/geometry/dual_quaternion.rs
View File

@ -357,7 +357,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 dual quaternion. 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> {
@ -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(),

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(