diff --git a/src/geometry/dual_quaternion_construction.rs b/src/geometry/dual_quaternion_construction.rs index 2ec3d6b5..daedc239 100644 --- a/src/geometry/dual_quaternion_construction.rs +++ b/src/geometry/dual_quaternion_construction.rs @@ -1,3 +1,5 @@ +#[cfg(feature = "arbitrary")] +use quickcheck::{Arbitrary, Gen}; use crate::{ DualQuaternion, Quaternion, UnitDualQuaternion, SimdRealField, Isometry3, Translation3, UnitQuaternion @@ -97,6 +99,21 @@ where } } +#[cfg(feature = "arbitrary")] +impl Arbitrary for DualQuaternion +where + N: SimdRealField + Arbitrary + Send, + N::Element: SimdRealField, +{ + #[inline] + fn arbitrary(rng: &mut G) -> Self { + Self::from_real_and_dual( + Arbitrary::arbitrary(rng), + Arbitrary::arbitrary(rng) + ) + } +} + impl UnitDualQuaternion { /// The unit dual quaternion multiplicative identity, which also represents /// the identity transformation as an isometry. @@ -195,3 +212,15 @@ where Self::identity() } } + +#[cfg(feature = "arbitrary")] +impl Arbitrary for UnitDualQuaternion +where + N: SimdRealField + Arbitrary + Send, + N::Element: SimdRealField, +{ + #[inline] + fn arbitrary(rng: &mut G) -> Self { + Self::new_normalize(Arbitrary::arbitrary(rng)) + } +} diff --git a/src/geometry/dual_quaternion_ops.rs b/src/geometry/dual_quaternion_ops.rs index 95991c63..098fde6b 100644 --- a/src/geometry/dual_quaternion_ops.rs +++ b/src/geometry/dual_quaternion_ops.rs @@ -25,7 +25,7 @@ use crate::{ DualQuaternion, SimdRealField, Point3, Point, Vector3, Isometry3, Quaternion, UnitDualQuaternion, UnitQuaternion, U1, U3, U4, Unit, Allocator, - DefaultAllocator, Vector + DefaultAllocator, Vector, Translation3 }; use crate::base::storage::Storage; use std::mem; @@ -362,6 +362,223 @@ dual_quaternion_op_impl!( Output = UnitDualQuaternion => U3, U3; UnitDualQuaternion::::new_unchecked(DualQuaternion::from_real(self.into_inner())) * rhs;); +// UnitDualQuaternion ÷ UnitQuaternion +dual_quaternion_op_impl!( + Div, div; + (U4, U1), (U4, U1); + self: &'a UnitDualQuaternion, rhs: &'b UnitQuaternion, + Output = UnitDualQuaternion => U1, U4; + #[allow(clippy::suspicious_arithmetic_impl)] + { self * UnitDualQuaternion::::from_rotation(rhs.inverse()) }; + 'a, 'b); + +dual_quaternion_op_impl!( + Div, div; + (U4, U1), (U4, U1); + self: &'a UnitDualQuaternion, rhs: UnitQuaternion, + Output = UnitDualQuaternion => U3, U3; + #[allow(clippy::suspicious_arithmetic_impl)] + { self * UnitDualQuaternion::::from_rotation(rhs.inverse()) }; + 'a); + +dual_quaternion_op_impl!( + Div, div; + (U4, U1), (U4, U1); + self: UnitDualQuaternion, rhs: &'b UnitQuaternion, + Output = UnitDualQuaternion => U3, U3; + #[allow(clippy::suspicious_arithmetic_impl)] + { self * UnitDualQuaternion::::from_rotation(rhs.inverse()) }; + 'b); + +dual_quaternion_op_impl!( + Div, div; + (U4, U1), (U4, U1); + self: UnitDualQuaternion, rhs: UnitQuaternion, + Output = UnitDualQuaternion => U3, U3; + #[allow(clippy::suspicious_arithmetic_impl)] + { self * UnitDualQuaternion::::from_rotation(rhs.inverse()) };); + +// UnitQuaternion ÷ UnitDualQuaternion +dual_quaternion_op_impl!( + Div, div; + (U4, U1), (U4, U1); + self: &'a UnitQuaternion, rhs: &'b UnitDualQuaternion, + Output = UnitDualQuaternion => U1, U4; + #[allow(clippy::suspicious_arithmetic_impl)] + { + UnitDualQuaternion::::new_unchecked( + DualQuaternion::from_real(self.into_inner()) + ) * rhs.inverse() + }; 'a, 'b); + +dual_quaternion_op_impl!( + Div, div; + (U4, U1), (U4, U1); + self: &'a UnitQuaternion, rhs: UnitDualQuaternion, + Output = UnitDualQuaternion => U3, U3; + #[allow(clippy::suspicious_arithmetic_impl)] + { + UnitDualQuaternion::::new_unchecked( + DualQuaternion::from_real(self.into_inner()) + ) * rhs.inverse() + }; 'a); + +dual_quaternion_op_impl!( + Div, div; + (U4, U1), (U4, U1); + self: UnitQuaternion, rhs: &'b UnitDualQuaternion, + Output = UnitDualQuaternion => U3, U3; + #[allow(clippy::suspicious_arithmetic_impl)] + { + UnitDualQuaternion::::new_unchecked( + DualQuaternion::from_real(self.into_inner()) + ) * rhs.inverse() + }; 'b); + +dual_quaternion_op_impl!( + Div, div; + (U4, U1), (U4, U1); + self: UnitQuaternion, rhs: UnitDualQuaternion, + Output = UnitDualQuaternion => U3, U3; + #[allow(clippy::suspicious_arithmetic_impl)] + { + UnitDualQuaternion::::new_unchecked( + DualQuaternion::from_real(self.into_inner()) + ) * rhs.inverse() + };); + +// UnitDualQuaternion × Translation3 +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U3, U1); + self: &'a UnitDualQuaternion, rhs: &'b Translation3, + Output = UnitDualQuaternion => U3, U1; + self * UnitDualQuaternion::::from_parts(rhs.clone(), UnitQuaternion::identity()); + 'a, 'b); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U3, U3); + self: &'a UnitDualQuaternion, rhs: Translation3, + Output = UnitDualQuaternion => U3, U1; + self * UnitDualQuaternion::::from_parts(rhs, UnitQuaternion::identity()); + 'a); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U3, U3); + self: UnitDualQuaternion, rhs: &'b Translation3, + Output = UnitDualQuaternion => U3, U1; + self * UnitDualQuaternion::::from_parts(rhs.clone(), UnitQuaternion::identity()); + 'b); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U3, U3); + self: UnitDualQuaternion, rhs: Translation3, + Output = UnitDualQuaternion => U3, U1; + self * UnitDualQuaternion::::from_parts(rhs, UnitQuaternion::identity()); ); + +// UnitDualQuaternion ÷ Translation3 +dual_quaternion_op_impl!( + Div, div; + (U4, U1), (U3, U1); + self: &'a UnitDualQuaternion, rhs: &'b Translation3, + Output = UnitDualQuaternion => U3, U1; + #[allow(clippy::suspicious_arithmetic_impl)] + { self * UnitDualQuaternion::::from_parts(rhs.inverse(), UnitQuaternion::identity()) }; + 'a, 'b); + +dual_quaternion_op_impl!( + Div, div; + (U4, U1), (U3, U3); + self: &'a UnitDualQuaternion, rhs: Translation3, + Output = UnitDualQuaternion => U3, U1; + #[allow(clippy::suspicious_arithmetic_impl)] + { self * UnitDualQuaternion::::from_parts(rhs.inverse(), UnitQuaternion::identity()) }; + 'a); + +dual_quaternion_op_impl!( + Div, div; + (U4, U1), (U3, U3); + self: UnitDualQuaternion, rhs: &'b Translation3, + Output = UnitDualQuaternion => U3, U1; + #[allow(clippy::suspicious_arithmetic_impl)] + { self * UnitDualQuaternion::::from_parts(rhs.inverse(), UnitQuaternion::identity()) }; + 'b); + +dual_quaternion_op_impl!( + Div, div; + (U4, U1), (U3, U3); + self: UnitDualQuaternion, rhs: Translation3, + Output = UnitDualQuaternion => U3, U1; + #[allow(clippy::suspicious_arithmetic_impl)] + { self * UnitDualQuaternion::::from_parts(rhs.inverse(), UnitQuaternion::identity()) };); + +// Translation3 × UnitDualQuaternion +dual_quaternion_op_impl!( + Mul, mul; + (U3, U1), (U4, U1); + self: &'b Translation3, rhs: &'a UnitDualQuaternion, + Output = UnitDualQuaternion => U3, U1; + UnitDualQuaternion::::from_parts(self.clone(), UnitQuaternion::identity()) * rhs; + 'a, 'b); + +dual_quaternion_op_impl!( + Mul, mul; + (U3, U1), (U4, U1); + self: &'a Translation3, rhs: UnitDualQuaternion, + Output = UnitDualQuaternion => U3, U1; + UnitDualQuaternion::::from_parts(self.clone(), UnitQuaternion::identity()) * rhs; + 'a); + +dual_quaternion_op_impl!( + Mul, mul; + (U3, U1), (U4, U1); + self: Translation3, rhs: &'b UnitDualQuaternion, + Output = UnitDualQuaternion => U3, U1; + UnitDualQuaternion::::from_parts(self, UnitQuaternion::identity()) * rhs; + 'b); + +dual_quaternion_op_impl!( + Mul, mul; + (U3, U1), (U4, U1); + self: Translation3, rhs: UnitDualQuaternion, + Output = UnitDualQuaternion => U3, U1; + UnitDualQuaternion::::from_parts(self, UnitQuaternion::identity()) * rhs;); + +// Translation3 ÷ UnitDualQuaternion +dual_quaternion_op_impl!( + Div, div; + (U3, U1), (U4, U1); + self: &'b Translation3, rhs: &'a UnitDualQuaternion, + Output = UnitDualQuaternion => U3, U1; + UnitDualQuaternion::::from_parts(self.clone(), UnitQuaternion::identity()) / rhs; + 'a, 'b); + +dual_quaternion_op_impl!( + Div, div; + (U3, U1), (U4, U1); + self: &'a Translation3, rhs: UnitDualQuaternion, + Output = UnitDualQuaternion => U3, U1; + UnitDualQuaternion::::from_parts(self.clone(), UnitQuaternion::identity()) / rhs; + 'a); + +dual_quaternion_op_impl!( + Div, div; + (U3, U1), (U4, U1); + self: Translation3, rhs: &'b UnitDualQuaternion, + Output = UnitDualQuaternion => U3, U1; + UnitDualQuaternion::::from_parts(self, UnitQuaternion::identity()) / rhs; + 'b); + +dual_quaternion_op_impl!( + Div, div; + (U3, U1), (U4, U1); + self: Translation3, rhs: UnitDualQuaternion, + Output = UnitDualQuaternion => U3, U1; + UnitDualQuaternion::::from_parts(self, UnitQuaternion::identity()) / rhs;); + // UnitDualQuaternion × Isometry3 dual_quaternion_op_impl!( Mul, mul; @@ -738,6 +955,78 @@ dual_quaternion_op_impl!( self: UnitDualQuaternion, rhs: UnitDualQuaternion; *self /= &rhs; ); +// UnitDualQuaternion ×= UnitQuaternion +dual_quaternion_op_impl!( + MulAssign, mul_assign; + (U4, U1), (U4, U1); + self: UnitDualQuaternion, rhs: UnitQuaternion; + { + let res = &*self * UnitDualQuaternion::from_rotation(rhs); + self.as_mut_unchecked().real.coords.copy_from(&res.as_ref().real.coords); + self.as_mut_unchecked().dual.coords.copy_from(&res.as_ref().dual.coords); + };); + +dual_quaternion_op_impl!( + MulAssign, mul_assign; + (U4, U1), (U4, U1); + self: UnitDualQuaternion, rhs: &'b UnitQuaternion; + *self *= rhs.clone(); 'b); + +// UnitDualQuaternion ÷= UnitQuaternion +dual_quaternion_op_impl!( + DivAssign, div_assign; + (U4, U1), (U4, U1); + self: UnitDualQuaternion, rhs: &'b UnitQuaternion; + #[allow(clippy::suspicious_arithmetic_impl)] + { + let res = &*self * UnitDualQuaternion::from_rotation(rhs.inverse()); + self.as_mut_unchecked().real.coords.copy_from(&res.as_ref().real.coords); + self.as_mut_unchecked().dual.coords.copy_from(&res.as_ref().dual.coords); + }; + 'b); + +dual_quaternion_op_impl!( + DivAssign, div_assign; + (U4, U1), (U4, U1); + self: UnitDualQuaternion, rhs: UnitQuaternion; + *self /= &rhs; ); + +// UnitDualQuaternion ×= Translation3 +dual_quaternion_op_impl!( + MulAssign, mul_assign; + (U4, U1), (U4, U1); + self: UnitDualQuaternion, rhs: Translation3; + { + let res = &*self * UnitDualQuaternion::from_parts(rhs, UnitQuaternion::identity()); + self.as_mut_unchecked().real.coords.copy_from(&res.as_ref().real.coords); + self.as_mut_unchecked().dual.coords.copy_from(&res.as_ref().dual.coords); + };); + +dual_quaternion_op_impl!( + MulAssign, mul_assign; + (U4, U1), (U4, U1); + self: UnitDualQuaternion, rhs: &'b Translation3; + *self *= rhs.clone(); 'b); + +// UnitDualQuaternion ÷= Translation3 +dual_quaternion_op_impl!( + DivAssign, div_assign; + (U4, U1), (U4, U1); + self: UnitDualQuaternion, rhs: &'b Translation3; + #[allow(clippy::suspicious_arithmetic_impl)] + { + let res = &*self * UnitDualQuaternion::from_parts(rhs.inverse(), UnitQuaternion::identity()); + self.as_mut_unchecked().real.coords.copy_from(&res.as_ref().real.coords); + self.as_mut_unchecked().dual.coords.copy_from(&res.as_ref().dual.coords); + }; + 'b); + +dual_quaternion_op_impl!( + DivAssign, div_assign; + (U4, U1), (U4, U1); + self: UnitDualQuaternion, rhs: Translation3; + *self /= &rhs; ); + // UnitDualQuaternion ×= Isometry3 dual_quaternion_op_impl!( MulAssign, mul_assign; diff --git a/tests/geometry/dual_quaternion.rs b/tests/geometry/dual_quaternion.rs new file mode 100644 index 00000000..1baced29 --- /dev/null +++ b/tests/geometry/dual_quaternion.rs @@ -0,0 +1,172 @@ +#![cfg(feature = "arbitrary")] +#![allow(non_snake_case)] + +use na::{ + Isometry3, Point3, Translation3, UnitQuaternion, UnitDualQuaternion, Vector3, +}; + +quickcheck!( + fn isometry_equivalence(iso: Isometry3, p: Point3, v: Vector3) -> bool { + let dq = UnitDualQuaternion::from_isometry(&iso); + + relative_eq!(iso * p, dq * p, epsilon = 1.0e-7) + && relative_eq!(iso * v, dq * v, epsilon = 1.0e-7) + } + + fn inverse_is_identity(i: UnitDualQuaternion, p: Point3, v: Vector3) -> bool { + let ii = i.inverse(); + + relative_eq!(i * ii, UnitDualQuaternion::identity(), epsilon = 1.0e-7) + && relative_eq!(ii * i, UnitDualQuaternion::identity(), epsilon = 1.0e-7) + && relative_eq!((i * ii) * p, p, epsilon = 1.0e-7) + && relative_eq!((ii * i) * p, p, epsilon = 1.0e-7) + && relative_eq!((i * ii) * v, v, epsilon = 1.0e-7) + && relative_eq!((ii * i) * v, v, epsilon = 1.0e-7) + } + + fn multiply_equals_alga_transform( + dq: UnitDualQuaternion, v: Vector3, p: Point3 + ) -> bool { + dq * v == dq.transform_vector(&v) + && dq * p == dq.transform_point(&p) + && relative_eq!( + dq.inverse() * v, + dq.inverse_transform_vector(&v), + epsilon = 1.0e-7 + ) + && relative_eq!( + dq.inverse() * p, + dq.inverse_transform_point(&p), + epsilon = 1.0e-7 + ) + } + + #[cfg_attr(rustfmt, rustfmt_skip)] + fn composition( + dq: UnitDualQuaternion, + uq: UnitQuaternion, + t: Translation3, + v: Vector3, + p: Point3 + ) -> bool { + // (rotation × dual quaternion) * point = rotation × (dual quaternion * point) + relative_eq!((uq * dq) * v, uq * (dq * v), epsilon = 1.0e-7) && + relative_eq!((uq * dq) * p, uq * (dq * p), epsilon = 1.0e-7) && + + // (dual quaternion × rotation) * point = dual quaternion × (rotation * point) + relative_eq!((dq * uq) * v, dq * (uq * v), epsilon = 1.0e-7) && + relative_eq!((dq * uq) * p, dq * (uq * p), epsilon = 1.0e-7) && + + // (translation × dual quaternion) * point = translation × (dual quaternion * point) + relative_eq!((t * dq) * v, (dq * v), epsilon = 1.0e-7) && + relative_eq!((t * dq) * p, t * (dq * p), epsilon = 1.0e-7) && + + // (dual quaternion × translation) * point = dual quaternion × (translation * point) + relative_eq!((dq * t) * v, dq * v, epsilon = 1.0e-7) && + relative_eq!((dq * t) * p, dq * (t * p), epsilon = 1.0e-7) + } + + #[cfg_attr(rustfmt, rustfmt_skip)] + fn all_op_exist( + dq: UnitDualQuaternion, + uq: UnitQuaternion, + t: Translation3, + v: Vector3, + p: Point3 + ) -> bool { + let iMi = dq * dq; + let iMuq = dq * uq; + let iDi = dq / dq; + let iDuq = dq / uq; + + let iMp = dq * p; + let iMv = dq * v; + + let iMt = dq * t; + let tMi = t * dq; + + let tMuq = t * uq; + + let uqMi = uq * dq; + let uqDi = uq / dq; + + let uqMt = uq * t; + + let mut iMt1 = dq; + let mut iMt2 = dq; + + let mut iMi1 = dq; + let mut iMi2 = dq; + + let mut iMuq1 = dq; + let mut iMuq2 = dq; + + let mut iDi1 = dq; + let mut iDi2 = dq; + + let mut iDuq1 = dq; + let mut iDuq2 = dq; + + iMt1 *= t; + iMt2 *= &t; + + iMi1 *= dq; + iMi2 *= &dq; + + iMuq1 *= uq; + iMuq2 *= &uq; + + iDi1 /= dq; + iDi2 /= &dq; + + iDuq1 /= uq; + iDuq2 /= &uq; + + iMt == iMt1 + && iMt == iMt2 + && iMi == iMi1 + && iMi == iMi2 + && iMuq == iMuq1 + && iMuq == iMuq2 + && iDi == iDi1 + && iDi == iDi2 + && iDuq == iDuq1 + && iDuq == iDuq2 + && iMi == &dq * &dq + && iMi == dq * &dq + && iMi == &dq * dq + && iMuq == &dq * &uq + && iMuq == dq * &uq + && iMuq == &dq * uq + && iDi == &dq / &dq + && iDi == dq / &dq + && iDi == &dq / dq + && iDuq == &dq / &uq + && iDuq == dq / &uq + && iDuq == &dq / uq + && iMp == &dq * &p + && iMp == dq * &p + && iMp == &dq * p + && iMv == &dq * &v + && iMv == dq * &v + && iMv == &dq * v + && iMt == &dq * &t + && iMt == dq * &t + && iMt == &dq * t + && tMi == &t * &dq + && tMi == t * &dq + && tMi == &t * dq + && tMuq == &t * &uq + && tMuq == t * &uq + && tMuq == &t * uq + && uqMi == &uq * &dq + && uqMi == uq * &dq + && uqMi == &uq * dq + && uqDi == &uq / &dq + && uqDi == uq / &dq + && uqDi == &uq / dq + && uqMt == &uq * &t + && uqMt == uq * &t + && uqMt == &uq * t + } +); diff --git a/tests/geometry/mod.rs b/tests/geometry/mod.rs index ec9755a0..68d1bd6e 100644 --- a/tests/geometry/mod.rs +++ b/tests/geometry/mod.rs @@ -5,3 +5,4 @@ mod quaternion; mod rotation; mod similarity; mod unit_complex; +mod dual_quaternion;