From f811accccbafce91c218109f40e0a22ab3868e82 Mon Sep 17 00:00:00 2001 From: Michael Stevens Date: Fri, 13 Mar 2020 11:05:44 +0100 Subject: [PATCH 01/10] Unnecessary Trait bound DimSub in impl Cholesky --- src/linalg/cholesky.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/linalg/cholesky.rs b/src/linalg/cholesky.rs index 67baefb1..23ebdea5 100644 --- a/src/linalg/cholesky.rs +++ b/src/linalg/cholesky.rs @@ -37,7 +37,7 @@ where { } -impl> Cholesky +impl Cholesky where DefaultAllocator: Allocator, { From 12c259f0b4d7c2e885586891240642b0958e9c01 Mon Sep 17 00:00:00 2001 From: Terence Date: Thu, 28 Jan 2021 17:25:32 -0500 Subject: [PATCH 02/10] Implement additional `DualQuaternion` ops and `UnitDualQuaternion` This implements `UnitDualQuaternion` as an alternative to `Isometry3` for representing 3D isometries, which also provides the `sclerp` operation which can be used to perform screw-linear interpolation between two unit dual quaternions. --- src/geometry/dual_quaternion.rs | 815 ++++++++++++++++++- src/geometry/dual_quaternion_alga.rs | 316 +++++++ src/geometry/dual_quaternion_construction.rs | 159 +++- src/geometry/dual_quaternion_conversion.rs | 186 +++++ src/geometry/dual_quaternion_ops.rs | 786 +++++++++++++++++- src/geometry/isometry_conversion.rs | 28 +- src/geometry/mod.rs | 3 + src/geometry/quaternion_conversion.rs | 25 +- src/geometry/rotation_conversion.rs | 28 +- src/geometry/translation_conversion.rs | 26 + 10 files changed, 2329 insertions(+), 43 deletions(-) create mode 100644 src/geometry/dual_quaternion_alga.rs create mode 100644 src/geometry/dual_quaternion_conversion.rs diff --git a/src/geometry/dual_quaternion.rs b/src/geometry/dual_quaternion.rs index ce9f7284..63c5fa03 100644 --- a/src/geometry/dual_quaternion.rs +++ b/src/geometry/dual_quaternion.rs @@ -1,7 +1,14 @@ -use crate::{Quaternion, SimdRealField}; +use std::fmt; +use approx::{AbsDiffEq, RelativeEq, UlpsEq}; +use crate::{ + Quaternion, SimdRealField, VectorN, U8, Vector3, Point3, Isometry3, Unit, Matrix4, + Translation3, UnitQuaternion, Scalar, Normed +}; #[cfg(feature = "serde-serialize")] use serde::{Deserialize, Deserializer, Serialize, Serializer}; +use simba::scalar::{ClosedNeg, RealField}; + /// A dual quaternion. /// /// # Indexing @@ -77,8 +84,147 @@ where /// relative_eq!(dq.real.norm(), 1.0); /// ``` #[inline] - pub fn normalize_mut(&mut self) { - *self = self.normalize(); + pub fn normalize_mut(&mut self) -> N { + let real_norm = self.real.norm(); + self.real /= real_norm; + self.dual /= real_norm; + real_norm + } + + /// The conjugate of this dual quaternion, containing the conjugate of + /// the real and imaginary parts.. + /// + /// # Example + /// ``` + /// # use nalgebra::{DualQuaternion, Quaternion}; + /// let real = Quaternion::new(1.0, 2.0, 3.0, 4.0); + /// let dual = Quaternion::new(5.0, 6.0, 7.0, 8.0); + /// let dq = DualQuaternion::from_real_and_dual(real, dual); + /// + /// let conj = dq.conjugate(); + /// assert!(conj.real.i == -2.0 && conj.real.j == -3.0 && conj.real.k == -4.0); + /// assert!(conj.real.w == 1.0); + /// assert!(conj.dual.i == -6.0 && conj.dual.j == -7.0 && conj.dual.k == -8.0); + /// assert!(conj.dual.w == 5.0); + /// ``` + #[inline] + #[must_use = "Did you mean to use conjugate_mut()?"] + pub fn conjugate(&self) -> Self { + Self::from_real_and_dual(self.real.conjugate(), self.dual.conjugate()) + } + + /// Replaces this quaternion by its conjugate. + /// + /// # Example + /// ``` + /// # use nalgebra::{DualQuaternion, Quaternion}; + /// let real = Quaternion::new(1.0, 2.0, 3.0, 4.0); + /// let dual = Quaternion::new(5.0, 6.0, 7.0, 8.0); + /// let mut dq = DualQuaternion::from_real_and_dual(real, dual); + /// + /// dq.conjugate_mut(); + /// assert!(dq.real.i == -2.0 && dq.real.j == -3.0 && dq.real.k == -4.0); + /// assert!(dq.real.w == 1.0); + /// assert!(dq.dual.i == -6.0 && dq.dual.j == -7.0 && dq.dual.k == -8.0); + /// assert!(dq.dual.w == 5.0); + /// ``` + #[inline] + pub fn conjugate_mut(&mut self) { + self.real.conjugate_mut(); + self.dual.conjugate_mut(); + } + + /// Inverts this dual quaternion if it is not zero. + /// + /// # Example + /// ``` + /// # #[macro_use] extern crate approx; + /// # use nalgebra::{DualQuaternion, Quaternion}; + /// let real = Quaternion::new(1.0, 2.0, 3.0, 4.0); + /// let dual = Quaternion::new(5.0, 6.0, 7.0, 8.0); + /// let dq = DualQuaternion::from_real_and_dual(real, dual); + /// let inverse = dq.try_inverse(); + /// + /// assert!(inverse.is_some()); + /// assert_relative_eq!(inverse.unwrap() * dq, DualQuaternion::identity()); + /// + /// //Non-invertible case + /// let zero = Quaternion::new(0.0, 0.0, 0.0, 0.0); + /// let dq = DualQuaternion::from_real_and_dual(zero, zero); + /// let inverse = dq.try_inverse(); + /// + /// assert!(inverse.is_none()); + /// ``` + #[inline] + #[must_use = "Did you mean to use try_inverse_mut()?"] + pub fn try_inverse(&self) -> Option + where + N: RealField, + { + let mut res = *self; + if res.try_inverse_mut() { + Some(res) + } else { + None + } + } + + /// Inverts this dual quaternion in-place if it is not zero. + /// + /// # Example + /// ``` + /// # #[macro_use] extern crate approx; + /// # use nalgebra::{DualQuaternion, Quaternion}; + /// let real = Quaternion::new(1.0, 2.0, 3.0, 4.0); + /// let dual = Quaternion::new(5.0, 6.0, 7.0, 8.0); + /// let dq = DualQuaternion::from_real_and_dual(real, dual); + /// let mut dq_inverse = dq; + /// dq_inverse.try_inverse_mut(); + /// + /// assert_relative_eq!(dq_inverse * dq, DualQuaternion::identity()); + /// + /// //Non-invertible case + /// let zero = Quaternion::new(0.0, 0.0, 0.0, 0.0); + /// let mut dq = DualQuaternion::from_real_and_dual(zero, zero); + /// assert!(!dq.try_inverse_mut()); + /// ``` + #[inline] + pub fn try_inverse_mut(&mut self) -> bool + where + N: RealField, + { + let inverted = self.real.try_inverse_mut(); + if inverted { + self.dual = -self.real * self.dual * self.real; + true + } else { + false + } + } + + /// Linear interpolation between two dual quaternions. + /// + /// Computes `self * (1 - t) + other * t`. + /// + /// # Example + /// ``` + /// # use nalgebra::{DualQuaternion, Quaternion}; + /// let dq1 = DualQuaternion::from_real_and_dual( + /// Quaternion::new(1.0, 0.0, 0.0, 4.0), + /// Quaternion::new(0.0, 2.0, 0.0, 0.0) + /// ); + /// let dq2 = DualQuaternion::from_real_and_dual( + /// Quaternion::new(2.0, 0.0, 1.0, 0.0), + /// Quaternion::new(0.0, 2.0, 0.0, 0.0) + /// ); + /// assert_eq!(dq1.lerp(&dq2, 0.25), DualQuaternion::from_real_and_dual( + /// Quaternion::new(1.25, 0.0, 0.25, 3.0), + /// Quaternion::new(0.0, 2.0, 0.0, 0.0) + /// )); + /// ``` + #[inline] + pub fn lerp(&self, other: &Self, t: N) -> Self { + self * (N::one() - t) + other * t } } @@ -114,3 +260,666 @@ where }) } } + +impl DualQuaternion { + fn to_vector(&self) -> VectorN { + self.as_ref().clone().into() + } +} + +impl> AbsDiffEq for DualQuaternion { + type Epsilon = N; + + #[inline] + fn default_epsilon() -> Self::Epsilon { + N::default_epsilon() + } + + #[inline] + fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool { + self.to_vector().abs_diff_eq(&other.to_vector(), epsilon) || + // Account for the double-covering of S², i.e. q = -q + self.to_vector().iter().zip(other.to_vector().iter()).all(|(a, b)| a.abs_diff_eq(&-*b, epsilon)) + } +} + +impl> RelativeEq for DualQuaternion { + #[inline] + fn default_max_relative() -> Self::Epsilon { + N::default_max_relative() + } + + #[inline] + fn relative_eq( + &self, + other: &Self, + epsilon: Self::Epsilon, + max_relative: Self::Epsilon, + ) -> bool { + self.to_vector().relative_eq(&other.to_vector(), epsilon, max_relative) || + // Account for the double-covering of S², i.e. q = -q + self.to_vector().iter().zip(other.to_vector().iter()).all(|(a, b)| a.relative_eq(&-*b, epsilon, max_relative)) + } +} + +impl> UlpsEq for DualQuaternion { + #[inline] + fn default_max_ulps() -> u32 { + N::default_max_ulps() + } + + #[inline] + fn ulps_eq(&self, other: &Self, epsilon: Self::Epsilon, max_ulps: u32) -> bool { + self.to_vector().ulps_eq(&other.to_vector(), epsilon, max_ulps) || + // Account for the double-covering of S², i.e. q = -q. + self.to_vector().iter().zip(other.to_vector().iter()).all(|(a, b)| a.ulps_eq(&-*b, epsilon, max_ulps)) + } +} + +/// A unit quaternions. May be used to represent a rotation. +pub type UnitDualQuaternion = Unit>; + +impl PartialEq for UnitDualQuaternion { + #[inline] + fn eq(&self, rhs: &Self) -> bool { + self.as_ref().eq(rhs.as_ref()) + } +} + +impl Eq for UnitDualQuaternion {} + +impl Normed for DualQuaternion { + type Norm = N::SimdRealField; + + #[inline] + fn norm(&self) -> N::SimdRealField { + self.real.norm() + } + + #[inline] + fn norm_squared(&self) -> N::SimdRealField { + self.real.norm_squared() + } + + #[inline] + fn scale_mut(&mut self, n: Self::Norm) { + self.real.scale_mut(n); + self.dual.scale_mut(n); + } + + #[inline] + fn unscale_mut(&mut self, n: Self::Norm) { + self.real.unscale_mut(n); + self.dual.unscale_mut(n); + } +} + +impl UnitDualQuaternion +where + N::Element: SimdRealField, +{ + /// The underlying dual quaternion. + /// + /// Same as `self.as_ref()`. + /// + /// # Example + /// ``` + /// # use nalgebra::{DualQuaternion, UnitDualQuaternion, Quaternion}; + /// let id = UnitDualQuaternion::identity(); + /// assert_eq!(*id.dual_quaternion(), DualQuaternion::from_real_and_dual( + /// Quaternion::new(1.0, 0.0, 0.0, 0.0), + /// Quaternion::new(0.0, 0.0, 0.0, 0.0) + /// )); + /// ``` + #[inline] + pub fn dual_quaternion(&self) -> &DualQuaternion { + self.as_ref() + } + + /// Compute the conjugate of this unit quaternion. + /// + /// # Example + /// ``` + /// # use nalgebra::{UnitDualQuaternion, DualQuaternion, Quaternion}; + /// let qr = Quaternion::new(1.0, 2.0, 3.0, 4.0); + /// let qd = Quaternion::new(5.0, 6.0, 7.0, 8.0); + /// let unit = UnitDualQuaternion::new_normalize( + /// DualQuaternion::from_real_and_dual(qr, qd) + /// ); + /// let conj = unit.conjugate(); + /// assert_eq!(conj.real, unit.real.conjugate()); + /// assert_eq!(conj.dual, unit.dual.conjugate()); + /// ``` + #[inline] + #[must_use = "Did you mean to use conjugate_mut()?"] + pub fn conjugate(&self) -> Self { + Self::new_unchecked(self.as_ref().conjugate()) + } + + /// Compute the conjugate of this unit quaternion in-place. + /// + /// # Example + /// ``` + /// # use nalgebra::{UnitDualQuaternion, DualQuaternion, Quaternion}; + /// let qr = Quaternion::new(1.0, 2.0, 3.0, 4.0); + /// let qd = Quaternion::new(5.0, 6.0, 7.0, 8.0); + /// let unit = UnitDualQuaternion::new_normalize( + /// DualQuaternion::from_real_and_dual(qr, qd) + /// ); + /// let mut conj = unit.clone(); + /// conj.conjugate_mut(); + /// assert_eq!(conj.as_ref().real, unit.as_ref().real.conjugate()); + /// assert_eq!(conj.as_ref().dual, unit.as_ref().dual.conjugate()); + /// ``` + #[inline] + pub fn conjugate_mut(&mut self) { + self.as_mut_unchecked().conjugate_mut() + } + + /// Inverts this dual quaternion if it is not zero. + /// + /// # Example + /// ``` + /// # #[macro_use] extern crate approx; + /// # use nalgebra::{UnitDualQuaternion, Quaternion, DualQuaternion}; + /// let qr = Quaternion::new(1.0, 2.0, 3.0, 4.0); + /// let qd = Quaternion::new(5.0, 6.0, 7.0, 8.0); + /// let unit = UnitDualQuaternion::new_normalize(DualQuaternion::from_real_and_dual(qr, qd)); + /// let inv = unit.inverse(); + /// assert_relative_eq!(unit * inv, UnitDualQuaternion::identity(), epsilon = 1.0e-6); + /// assert_relative_eq!(inv * unit, UnitDualQuaternion::identity(), epsilon = 1.0e-6); + /// ``` + #[inline] + #[must_use = "Did you mean to use inverse_mut()?"] + pub fn inverse(&self) -> Self { + let real = Unit::new_unchecked(self.as_ref().real).inverse().into_inner(); + let dual = -real * self.as_ref().dual * real; + UnitDualQuaternion::new_unchecked(DualQuaternion { real, dual }) + } + + /// Inverts this dual quaternion in place if it is not zero. + /// + /// # Example + /// ``` + /// # #[macro_use] extern crate approx; + /// # use nalgebra::{UnitDualQuaternion, Quaternion, DualQuaternion}; + /// let qr = Quaternion::new(1.0, 2.0, 3.0, 4.0); + /// let qd = Quaternion::new(5.0, 6.0, 7.0, 8.0); + /// let unit = UnitDualQuaternion::new_normalize(DualQuaternion::from_real_and_dual(qr, qd)); + /// let mut inv = unit.clone(); + /// inv.inverse_mut(); + /// assert_relative_eq!(unit * inv, UnitDualQuaternion::identity(), epsilon = 1.0e-6); + /// assert_relative_eq!(inv * unit, UnitDualQuaternion::identity(), epsilon = 1.0e-6); + /// ``` + #[inline] + #[must_use = "Did you mean to use inverse_mut()?"] + pub fn inverse_mut(&mut self) { + let quat = self.as_mut_unchecked(); + quat.real = Unit::new_unchecked(quat.real).inverse().into_inner(); + quat.dual = -quat.real * quat.dual * quat.real; + } + + /// The unit dual quaternion needed to make `self` and `other` coincide. + /// + /// The result is such that: `self.isometry_to(other) * self == other`. + /// + /// # Example + /// ``` + /// # #[macro_use] extern crate approx; + /// # use nalgebra::{UnitDualQuaternion, DualQuaternion, Quaternion}; + /// let qr = Quaternion::new(1.0, 2.0, 3.0, 4.0); + /// let qd = Quaternion::new(5.0, 6.0, 7.0, 8.0); + /// let iso1 = UnitDualQuaternion::new_normalize(DualQuaternion::from_real_and_dual(qr, qd)); + /// let iso2 = UnitDualQuaternion::new_normalize(DualQuaternion::from_real_and_dual(qd, qr)); + /// let iso_to = iso1.isometry_to(&iso2); + /// assert_relative_eq!(iso_to * iso1, iso2, epsilon = 1.0e-6); + /// ``` + #[inline] + pub fn isometry_to(&self, other: &Self) -> Self { + other / self + } + + /// Linear interpolation between two unit dual quaternions. + /// + /// The result is not normalized. + /// + /// # Example + /// ``` + /// # #[macro_use] extern crate approx; + /// # use nalgebra::{UnitDualQuaternion, DualQuaternion, Quaternion}; + /// let dq1 = UnitDualQuaternion::new_normalize(DualQuaternion::from_real_and_dual( + /// Quaternion::new(0.5, 0.0, 0.5, 0.0), + /// Quaternion::new(0.0, 0.5, 0.0, 0.5) + /// )); + /// let dq2 = UnitDualQuaternion::new_normalize(DualQuaternion::from_real_and_dual( + /// Quaternion::new(0.5, 0.0, 0.0, 0.5), + /// Quaternion::new(0.5, 0.0, 0.5, 0.0) + /// )); + /// assert_relative_eq!( + /// UnitDualQuaternion::new_normalize(dq1.lerp(&dq2, 0.5)), + /// UnitDualQuaternion::new_normalize( + /// DualQuaternion::from_real_and_dual( + /// Quaternion::new(0.5, 0.0, 0.25, 0.25), + /// Quaternion::new(0.25, 0.25, 0.25, 0.25) + /// ) + /// ), + /// epsilon = 1.0e-6 + /// ); + /// ``` + #[inline] + pub fn lerp(&self, other: &Self, t: N) -> DualQuaternion { + self.as_ref().lerp(other.as_ref(), t) + } + + /// Normalized linear interpolation between two unit quaternions. + /// + /// This is the same as `self.lerp` except that the result is normalized. + /// + /// # Example + /// ``` + /// # #[macro_use] extern crate approx; + /// # use nalgebra::{UnitDualQuaternion, DualQuaternion, Quaternion}; + /// let dq1 = UnitDualQuaternion::new_normalize(DualQuaternion::from_real_and_dual( + /// Quaternion::new(0.5, 0.0, 0.5, 0.0), + /// Quaternion::new(0.0, 0.5, 0.0, 0.5) + /// )); + /// let dq2 = UnitDualQuaternion::new_normalize(DualQuaternion::from_real_and_dual( + /// Quaternion::new(0.5, 0.0, 0.0, 0.5), + /// Quaternion::new(0.5, 0.0, 0.5, 0.0) + /// )); + /// assert_relative_eq!(dq1.nlerp(&dq2, 0.2), UnitDualQuaternion::new_normalize( + /// DualQuaternion::from_real_and_dual( + /// Quaternion::new(0.5, 0.0, 0.4, 0.1), + /// Quaternion::new(0.1, 0.4, 0.1, 0.4) + /// ) + /// ), epsilon = 1.0e-6); + /// ``` + #[inline] + pub fn nlerp(&self, other: &Self, t: N) -> Self { + let mut res = self.lerp(other, t); + let _ = res.normalize_mut(); + + Self::new_unchecked(res) + } + + /// Screw linear interpolation between two unit quaternions. This creates a + /// smooth arc from one isometry 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. + /// + /// # Example + /// ``` + /// # #[macro_use] extern crate approx; + /// # use nalgebra::{UnitDualQuaternion, DualQuaternion, UnitQuaternion, Vector3}; + /// + /// let dq1 = UnitDualQuaternion::from_parts( + /// Vector3::new(0.0, 3.0, 0.0).into(), + /// UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_4, 0.0, 0.0), + /// ); + /// + /// let dq2 = UnitDualQuaternion::from_parts( + /// Vector3::new(0.0, 0.0, 3.0).into(), + /// UnitQuaternion::from_euler_angles(-std::f32::consts::PI, 0.0, 0.0), + /// ); + /// + /// let dq = dq1.sclerp(&dq2, 1.0 / 3.0); + /// + /// assert_relative_eq!( + /// dq.rotation().euler_angles().0, std::f32::consts::FRAC_PI_2, epsilon = 1.0e-6 + /// ); + /// assert_relative_eq!(dq.translation().vector.y, 3.0, epsilon = 1.0e-6); + #[inline] + pub fn sclerp(&self, other: &Self, t: N) -> Self + where + N: RealField, + { + self.try_sclerp(other, t, N::default_epsilon()) + .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). + /// + /// # 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 + /// must be to return `None`. + #[inline] + pub fn try_sclerp(&self, other: &Self, t: N, epsilon: N) -> Option + where + N: RealField, + { + let two = N::one() + N::one(); + let half = N::one() / two; + + // Invert one of the quaternions if we've got a longest-path + // interpolation. + let other = { + let dot_product = self.as_ref().real.coords.dot(&other.as_ref().real.coords); + if dot_product < N::zero() { + -other.clone() + } else { + other.clone() + } + }; + + let difference = self.as_ref().conjugate() * other.as_ref(); + let norm_squared = difference.real.vector().norm_squared(); + if relative_eq!(norm_squared, N::zero(), epsilon = epsilon) { + return None; + } + + let inverse_norm_squared = N::one() / norm_squared; + let inverse_norm = inverse_norm_squared.sqrt(); + + let mut angle = two * difference.real.scalar().acos(); + let mut pitch = -two * difference.dual.scalar() * inverse_norm; + let direction = difference.real.vector() * inverse_norm; + let moment = (difference.dual.vector() + - direction * (pitch * difference.real.scalar() * half)) + * inverse_norm; + + angle *= t; + pitch *= t; + + let sin = (half * angle).sin(); + let cos = (half * angle).cos(); + let real = Quaternion::from_parts(cos, direction * sin); + let dual = Quaternion::from_parts( + -pitch * half * sin, + moment * sin + direction * (pitch * half * cos), + ); + + Some(self * UnitDualQuaternion::new_unchecked( + DualQuaternion::from_real_and_dual(real, dual) + )) + } + + /// Return the rotation part of this unit dual quaternion. + /// + /// ``` + /// # #[macro_use] extern crate approx; + /// # use nalgebra::{UnitDualQuaternion, UnitQuaternion, Vector3}; + /// let dq = UnitDualQuaternion::from_parts( + /// Vector3::new(0.0, 3.0, 0.0).into(), + /// UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_4, 0.0, 0.0) + /// ); + /// + /// assert_relative_eq!( + /// dq.rotation().angle(), std::f32::consts::FRAC_PI_4, epsilon = 1.0e-6 + /// ); + /// ``` + #[inline] + pub fn rotation(&self) -> UnitQuaternion { + Unit::new_unchecked(self.as_ref().real) + } + + /// Return the translation part of this unit dual quaternion. + /// + /// ``` + /// # #[macro_use] extern crate approx; + /// # use nalgebra::{UnitDualQuaternion, UnitQuaternion, Vector3}; + /// let dq = UnitDualQuaternion::from_parts( + /// Vector3::new(0.0, 3.0, 0.0).into(), + /// UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_4, 0.0, 0.0) + /// ); + /// + /// assert_relative_eq!( + /// dq.translation().vector, Vector3::new(0.0, 3.0, 0.0), epsilon = 1.0e-6 + /// ); + /// ``` + #[inline] + pub fn translation(&self) -> Translation3 { + let two = N::one() + N::one(); + Translation3::from( + ((self.as_ref().dual * self.as_ref().real.conjugate()) * two) + .vector() + .into_owned(), + ) + } + + /// Builds an isometry from this unit dual quaternion. + /// + /// ``` + /// # #[macro_use] extern crate approx; + /// # use nalgebra::{UnitDualQuaternion, UnitQuaternion, Vector3}; + /// let rotation = UnitQuaternion::from_euler_angles(std::f32::consts::PI, 0.0, 0.0); + /// let translation = Vector3::new(1.0, 3.0, 2.5); + /// let dq = UnitDualQuaternion::from_parts( + /// translation.into(), + /// rotation + /// ); + /// let iso = dq.to_isometry(); + /// + /// assert_relative_eq!(iso.rotation.angle(), std::f32::consts::PI, epsilon = 1.0e-6); + /// assert_relative_eq!(iso.translation.vector, translation, epsilon = 1.0e-6); + /// ``` + #[inline] + pub fn to_isometry(&self) -> Isometry3 { + Isometry3::from_parts(self.translation(), self.rotation()) + } + + /// Rotate and translate a point by this unit dual quaternion interpreted + /// as an isometry. + /// + /// This is the same as the multiplication `self * pt`. + /// + /// ``` + /// # #[macro_use] extern crate approx; + /// # use nalgebra::{UnitDualQuaternion, UnitQuaternion, Vector3, Point3}; + /// let dq = UnitDualQuaternion::from_parts( + /// Vector3::new(0.0, 3.0, 0.0).into(), + /// UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_2, 0.0, 0.0) + /// ); + /// let point = Point3::new(1.0, 2.0, 3.0); + /// + /// assert_relative_eq!( + /// dq.transform_point(&point), Point3::new(1.0, 0.0, 2.0), epsilon = 1.0e-6 + /// ); + /// ``` + #[inline] + pub fn transform_point(&self, pt: &Point3) -> Point3 { + self * pt + } + + /// Rotate a vector by this unit dual quaternion, ignoring the translational + /// component. + /// + /// This is the same as the multiplication `self * v`. + /// + /// ``` + /// # #[macro_use] extern crate approx; + /// # use nalgebra::{UnitDualQuaternion, UnitQuaternion, Vector3}; + /// let dq = UnitDualQuaternion::from_parts( + /// Vector3::new(0.0, 3.0, 0.0).into(), + /// UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_2, 0.0, 0.0) + /// ); + /// let vector = Vector3::new(1.0, 2.0, 3.0); + /// + /// assert_relative_eq!( + /// dq.transform_vector(&vector), Vector3::new(1.0, -3.0, 2.0), epsilon = 1.0e-6 + /// ); + /// ``` + #[inline] + pub fn transform_vector(&self, v: &Vector3) -> Vector3 { + self * v + } + + /// Rotate and translate a point by the inverse of this unit quaternion. + /// This may be + /// cheaper than inverting the unit dual quaternion and transforming the + /// point. + /// + /// ``` + /// # #[macro_use] extern crate approx; + /// # use nalgebra::{UnitDualQuaternion, UnitQuaternion, Vector3, Point3}; + /// let dq = UnitDualQuaternion::from_parts( + /// Vector3::new(0.0, 3.0, 0.0).into(), + /// UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_2, 0.0, 0.0) + /// ); + /// let point = Point3::new(1.0, 2.0, 3.0); + /// + /// assert_relative_eq!( + /// dq.inverse_transform_point(&point), Point3::new(1.0, 3.0, 1.0), epsilon = 1.0e-6 + /// ); + /// ``` + #[inline] + pub fn inverse_transform_point(&self, pt: &Point3) -> Point3 { + self.inverse() * pt + } + + /// Rotate a vector by the inverse of this unit quaternion, ignoring the + /// translational component + /// This may be + /// cheaper than inverting the unit dual quaternion and transforming the + /// vector. + /// + /// ``` + /// # #[macro_use] extern crate approx; + /// # use nalgebra::{UnitDualQuaternion, UnitQuaternion, Vector3}; + /// let dq = UnitDualQuaternion::from_parts( + /// Vector3::new(0.0, 3.0, 0.0).into(), + /// UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_2, 0.0, 0.0) + /// ); + /// let vector = Vector3::new(1.0, 2.0, 3.0); + /// + /// assert_relative_eq!( + /// dq.inverse_transform_vector(&vector), Vector3::new(1.0, 3.0, -2.0), epsilon = 1.0e-6 + /// ); + /// ``` + #[inline] + pub fn inverse_transform_vector(&self, v: &Vector3) -> Vector3 { + self.inverse() * v + } + + /// Rotate a unit vector by the inverse of this unit quaternion, ignoring + /// the translational component. This may be + /// cheaper than inverting the unit dual quaternion and transforming the + /// vector. + /// + /// ``` + /// # #[macro_use] extern crate approx; + /// # use nalgebra::{UnitDualQuaternion, UnitQuaternion, Unit, Vector3}; + /// let dq = UnitDualQuaternion::from_parts( + /// Vector3::new(0.0, 3.0, 0.0).into(), + /// UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_2, 0.0, 0.0) + /// ); + /// let vector = Unit::new_unchecked(Vector3::new(0.0, 1.0, 0.0)); + /// + /// assert_relative_eq!( + /// dq.inverse_transform_unit_vector(&vector), + /// Unit::new_unchecked(Vector3::new(0.0, 0.0, -1.0)), + /// epsilon = 1.0e-6 + /// ); + /// ``` + #[inline] + pub fn inverse_transform_unit_vector(&self, v: &Unit>) -> Unit> { + self.inverse() * v + } +} + +impl UnitDualQuaternion +where + N::Element: SimdRealField, +{ + /// Converts this unit dual quaternion interpreted as an isometry + /// into its equivalent homogeneous transformation matrix. + /// + /// ``` + /// # #[macro_use] extern crate approx; + /// # use nalgebra::{Matrix4, UnitDualQuaternion, UnitQuaternion, Vector3}; + /// let dq = UnitDualQuaternion::from_parts( + /// Vector3::new(1.0, 3.0, 2.0).into(), + /// UnitQuaternion::from_axis_angle(&Vector3::z_axis(), std::f32::consts::FRAC_PI_6) + /// ); + /// let expected = Matrix4::new(0.8660254, -0.5, 0.0, 1.0, + /// 0.5, 0.8660254, 0.0, 3.0, + /// 0.0, 0.0, 1.0, 2.0, + /// 0.0, 0.0, 0.0, 1.0); + /// + /// assert_relative_eq!(dq.to_homogeneous(), expected, epsilon = 1.0e-6); + /// ``` + #[inline] + pub fn to_homogeneous(&self) -> Matrix4 + { + self.to_isometry().to_homogeneous() + } +} + +impl Default for UnitDualQuaternion { + fn default() -> Self { + Self::identity() + } +} + +impl fmt::Display for UnitDualQuaternion { + fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { + if let Some(axis) = self.rotation().axis() { + let axis = axis.into_inner(); + write!( + f, + "UnitDualQuaternion translation: {} − angle: {} − axis: ({}, {}, {})", + self.translation().vector, + self.rotation().angle(), + axis[0], + axis[1], + axis[2] + ) + } else { + write!( + f, + "UnitDualQuaternion translation: {} − angle: {} − axis: (undefined)", + self.translation().vector, + self.rotation().angle() + ) + } + } +} + +impl> AbsDiffEq for UnitDualQuaternion { + type Epsilon = N; + + #[inline] + fn default_epsilon() -> Self::Epsilon { + N::default_epsilon() + } + + #[inline] + fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool { + self.as_ref().abs_diff_eq(other.as_ref(), epsilon) + } +} + +impl> RelativeEq for UnitDualQuaternion { + #[inline] + fn default_max_relative() -> Self::Epsilon { + N::default_max_relative() + } + + #[inline] + fn relative_eq( + &self, + other: &Self, + epsilon: Self::Epsilon, + max_relative: Self::Epsilon, + ) -> bool { + self.as_ref() + .relative_eq(other.as_ref(), epsilon, max_relative) + } +} + +impl> UlpsEq for UnitDualQuaternion { + #[inline] + fn default_max_ulps() -> u32 { + N::default_max_ulps() + } + + #[inline] + fn ulps_eq(&self, other: &Self, epsilon: Self::Epsilon, max_ulps: u32) -> bool { + self.as_ref().ulps_eq(other.as_ref(), epsilon, max_ulps) + } +} diff --git a/src/geometry/dual_quaternion_alga.rs b/src/geometry/dual_quaternion_alga.rs new file mode 100644 index 00000000..d0dfdb24 --- /dev/null +++ b/src/geometry/dual_quaternion_alga.rs @@ -0,0 +1,316 @@ +use num::Zero; + +use alga::general::{ + AbstractGroup, AbstractGroupAbelian, AbstractLoop, AbstractMagma, AbstractModule, + AbstractMonoid, AbstractQuasigroup, AbstractSemigroup, Additive, Id, Identity, Module, + Multiplicative, RealField, TwoSidedInverse, +}; +use alga::linear::{ + AffineTransformation, DirectIsometry, FiniteDimVectorSpace, Isometry, NormedSpace, + ProjectiveTransformation, Similarity, Transformation, + VectorSpace, +}; + +use crate::base::Vector3; +use crate::geometry::{ + Point3, Quaternion, UnitQuaternion, DualQuaternion, UnitDualQuaternion, Translation3 +}; + +impl Identity for DualQuaternion { + #[inline] + fn identity() -> Self { + Self::identity() + } +} + +impl Identity for DualQuaternion { + #[inline] + fn identity() -> Self { + Self::zero() + } +} + +impl AbstractMagma for DualQuaternion { + #[inline] + fn operate(&self, rhs: &Self) -> Self { + self * rhs + } +} + +impl AbstractMagma for DualQuaternion { + #[inline] + fn operate(&self, rhs: &Self) -> Self { + self + rhs + } +} + +impl TwoSidedInverse for DualQuaternion { + #[inline] + fn two_sided_inverse(&self) -> Self { + -self + } +} + +macro_rules! impl_structures( + ($DualQuaternion: ident; $($marker: ident<$operator: ident>),* $(,)*) => {$( + impl $marker<$operator> for $DualQuaternion { } + )*} +); + +impl_structures!( + DualQuaternion; + AbstractSemigroup, + AbstractMonoid, + + AbstractSemigroup, + AbstractQuasigroup, + AbstractMonoid, + AbstractLoop, + AbstractGroup, + AbstractGroupAbelian +); + +/* + * + * Vector space. + * + */ +impl AbstractModule for DualQuaternion { + type AbstractRing = N; + + #[inline] + fn multiply_by(&self, n: N) -> Self { + self * n + } +} + +impl Module for DualQuaternion { + type Ring = N; +} + +impl VectorSpace for DualQuaternion { + type Field = N; +} + +impl FiniteDimVectorSpace for DualQuaternion { + #[inline] + fn dimension() -> usize { + 8 + } + + #[inline] + fn canonical_basis_element(i: usize) -> Self { + if i < 4 { + DualQuaternion::from_real_and_dual( + Quaternion::canonical_basis_element(i), + Quaternion::zero() + ) + } else { + DualQuaternion::from_real_and_dual( + Quaternion::zero(), + Quaternion::canonical_basis_element(i - 4) + ) + } + } + + #[inline] + fn dot(&self, other: &Self) -> N { + self.real.dot(&other.real) + self.dual.dot(&other.dual) + } + + #[inline] + unsafe fn component_unchecked(&self, i: usize) -> &N { + self.as_ref().get_unchecked(i) + } + + #[inline] + unsafe fn component_unchecked_mut(&mut self, i: usize) -> &mut N { + self.as_mut().get_unchecked_mut(i) + } +} + +impl NormedSpace for DualQuaternion { + type RealField = N; + type ComplexField = N; + + #[inline] + fn norm_squared(&self) -> N { + self.real.norm_squared() + } + + #[inline] + fn norm(&self) -> N { + self.real.norm() + } + + #[inline] + fn normalize(&self) -> Self { + self.normalize() + } + + #[inline] + fn normalize_mut(&mut self) -> N { + self.normalize_mut() + } + + #[inline] + fn try_normalize(&self, min_norm: N) -> Option { + let real_norm = self.real.norm(); + if real_norm > min_norm { + Some(Self::from_real_and_dual(self.real / real_norm, self.dual / real_norm)) + } else { + None + } + } + + #[inline] + fn try_normalize_mut(&mut self, min_norm: N) -> Option { + let real_norm = self.real.norm(); + if real_norm > min_norm { + self.real /= real_norm; + self.dual /= real_norm; + Some(real_norm) + } else { + None + } + } +} + +/* + * + * Implementations for UnitDualQuaternion. + * + */ +impl Identity for UnitDualQuaternion { + #[inline] + fn identity() -> Self { + Self::identity() + } +} + +impl AbstractMagma for UnitDualQuaternion { + #[inline] + fn operate(&self, rhs: &Self) -> Self { + self * rhs + } +} + +impl TwoSidedInverse + for UnitDualQuaternion +{ + #[inline] + fn two_sided_inverse(&self) -> Self { + self.inverse() + } + + #[inline] + fn two_sided_inverse_mut(&mut self) { + self.inverse_mut() + } +} + +impl_structures!( + UnitDualQuaternion; + AbstractSemigroup, + AbstractQuasigroup, + AbstractMonoid, + AbstractLoop, + AbstractGroup +); + +impl Transformation> for UnitDualQuaternion { + #[inline] + fn transform_point(&self, pt: &Point3) -> Point3 { + self.transform_point(pt) + } + + #[inline] + fn transform_vector(&self, v: &Vector3) -> Vector3 { + self.transform_vector(v) + } +} + +impl ProjectiveTransformation> + for UnitDualQuaternion +{ + #[inline] + fn inverse_transform_point(&self, pt: &Point3) -> Point3 { + self.inverse_transform_point(pt) + } + + #[inline] + fn inverse_transform_vector(&self, v: &Vector3) -> Vector3 { + self.inverse_transform_vector(v) + } +} + +impl AffineTransformation> + for UnitDualQuaternion +{ + type Rotation = UnitQuaternion; + type NonUniformScaling = Id; + type Translation = Translation3; + + #[inline] + fn decompose(&self) -> (Self::Translation, Self::Rotation, Id, Self::Rotation) { + (self.translation(), self.rotation(), Id::new(), UnitQuaternion::identity()) + } + + #[inline] + fn append_translation(&self, translation: &Self::Translation) -> Self { + self * Self::from_parts(translation.clone(), UnitQuaternion::identity()) + } + + #[inline] + fn prepend_translation(&self, translation: &Self::Translation) -> Self { + Self::from_parts(translation.clone(), UnitQuaternion::identity()) * self + } + + #[inline] + fn append_rotation(&self, r: &Self::Rotation) -> Self { + r * self + } + + #[inline] + fn prepend_rotation(&self, r: &Self::Rotation) -> Self { + self * r + } + + #[inline] + fn append_scaling(&self, _: &Self::NonUniformScaling) -> Self { + self.clone() + } + + #[inline] + fn prepend_scaling(&self, _: &Self::NonUniformScaling) -> Self { + self.clone() + } +} + +impl Similarity> for UnitDualQuaternion { + type Scaling = Id; + + #[inline] + fn translation(&self) -> Translation3 { + self.translation() + } + + #[inline] + fn rotation(&self) -> UnitQuaternion { + self.rotation() + } + + #[inline] + fn scaling(&self) -> Id { + Id::new() + } +} + +macro_rules! marker_impl( + ($($Trait: ident),*) => {$( + impl $Trait> for UnitDualQuaternion { } + )*} +); + +marker_impl!(Isometry, DirectIsometry); + diff --git a/src/geometry/dual_quaternion_construction.rs b/src/geometry/dual_quaternion_construction.rs index 25a979f7..2ec3d6b5 100644 --- a/src/geometry/dual_quaternion_construction.rs +++ b/src/geometry/dual_quaternion_construction.rs @@ -1,4 +1,8 @@ -use crate::{DualQuaternion, Quaternion, SimdRealField}; +use crate::{ + DualQuaternion, Quaternion, UnitDualQuaternion, SimdRealField, Isometry3, + Translation3, UnitQuaternion +}; +use num::{One, Zero}; impl DualQuaternion { /// Creates a dual quaternion from its rotation and translation components. @@ -16,7 +20,8 @@ impl DualQuaternion { pub fn from_real_and_dual(real: Quaternion, dual: Quaternion) -> Self { Self { real, dual } } - /// The dual quaternion multiplicative identity + + /// The dual quaternion multiplicative identity. /// /// # Example /// @@ -40,3 +45,153 @@ impl DualQuaternion { ) } } + +impl DualQuaternion +where + N::Element: SimdRealField +{ + + /// Creates a dual quaternion from only its real part, with no translation + /// component. + /// + /// # Example + /// ``` + /// # use nalgebra::{DualQuaternion, Quaternion}; + /// let rot = Quaternion::new(1.0, 2.0, 3.0, 4.0); + /// + /// let dq = DualQuaternion::from_real(rot); + /// assert_eq!(dq.real.w, 1.0); + /// assert_eq!(dq.dual.w, 0.0); + /// ``` + #[inline] + pub fn from_real(real: Quaternion) -> Self { + Self { real, dual: Quaternion::zero() } + } +} + +impl One for DualQuaternion +where + N::Element: SimdRealField, +{ + #[inline] + fn one() -> Self { + Self::identity() + } +} + +impl Zero for DualQuaternion +where + N::Element: SimdRealField, +{ + #[inline] + fn zero() -> Self { + DualQuaternion::from_real_and_dual( + Quaternion::zero(), + Quaternion::zero() + ) + } + + #[inline] + fn is_zero(&self) -> bool { + self.real.is_zero() && self.dual.is_zero() + } +} + +impl UnitDualQuaternion { + /// The unit dual quaternion multiplicative identity, which also represents + /// the identity transformation as an isometry. + /// + /// ``` + /// # use nalgebra::{UnitDualQuaternion, UnitQuaternion, Vector3, Point3}; + /// let ident = UnitDualQuaternion::identity(); + /// let point = Point3::new(1.0, -4.3, 3.33); + /// + /// assert_eq!(ident * point, point); + /// assert_eq!(ident, ident.inverse()); + /// ``` + #[inline] + pub fn identity() -> Self { + Self::new_unchecked(DualQuaternion::identity()) + } +} + +impl UnitDualQuaternion +where + N::Element: SimdRealField, +{ + /// Return a dual quaternion representing the translation and orientation + /// given by the provided rotation quaternion and translation vector. + /// + /// ``` + /// # #[macro_use] extern crate approx; + /// # use nalgebra::{UnitDualQuaternion, UnitQuaternion, Vector3, Point3}; + /// let dq = UnitDualQuaternion::from_parts( + /// Vector3::new(0.0, 3.0, 0.0).into(), + /// UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_2, 0.0, 0.0) + /// ); + /// let point = Point3::new(1.0, 2.0, 3.0); + /// + /// assert_relative_eq!(dq * point, Point3::new(1.0, 0.0, 2.0), epsilon = 1.0e-6); + /// ``` + #[inline] + pub fn from_parts( + translation: Translation3, + rotation: UnitQuaternion + ) -> Self { + let half: N = crate::convert(0.5f64); + UnitDualQuaternion::new_unchecked(DualQuaternion { + real: rotation.clone().into_inner(), + dual: Quaternion::from_parts(N::zero(), translation.vector) + * rotation.clone().into_inner() + * half, + }) + } + + /// Return a unit dual quaternion representing the translation and orientation + /// given by the provided isometry. + /// + /// ``` + /// # #[macro_use] extern crate approx; + /// # use nalgebra::{Isometry3, UnitDualQuaternion, UnitQuaternion, Vector3, Point3}; + /// let iso = Isometry3::from_parts( + /// Vector3::new(0.0, 3.0, 0.0).into(), + /// UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_2, 0.0, 0.0) + /// ); + /// let dq = UnitDualQuaternion::from_isometry(&iso); + /// let point = Point3::new(1.0, 2.0, 3.0); + /// + /// assert_relative_eq!(dq * point, iso * point, epsilon = 1.0e-6); + /// ``` + #[inline] + pub fn from_isometry(isometry: &Isometry3) -> Self { + UnitDualQuaternion::from_parts(isometry.translation, isometry.rotation) + } + + /// Creates a dual quaternion from a unit quaternion rotation. + /// + /// # Example + /// ``` + /// # #[macro_use] extern crate approx; + /// # use nalgebra::{UnitQuaternion, UnitDualQuaternion, Quaternion}; + /// let q = Quaternion::new(1.0, 2.0, 3.0, 4.0); + /// let rot = UnitQuaternion::new_normalize(q); + /// + /// let dq = UnitDualQuaternion::from_rotation(rot); + /// assert_relative_eq!(dq.as_ref().real.norm(), 1.0, epsilon = 1.0e-6); + /// assert_eq!(dq.as_ref().dual.norm(), 0.0); + /// ``` + #[inline] + pub fn from_rotation(rotation: UnitQuaternion) -> Self { + Self::new_unchecked(DualQuaternion::from_real(rotation.into_inner())) + } +} + +impl One for UnitDualQuaternion +where + N::Element: SimdRealField, +{ + #[inline] + fn one() -> Self { + Self::identity() + } +} diff --git a/src/geometry/dual_quaternion_conversion.rs b/src/geometry/dual_quaternion_conversion.rs new file mode 100644 index 00000000..41548391 --- /dev/null +++ b/src/geometry/dual_quaternion_conversion.rs @@ -0,0 +1,186 @@ +use simba::scalar::{RealField, SubsetOf, SupersetOf}; +use simba::simd::SimdRealField; + +use crate::base::dimension::U3; +use crate::base::{Matrix4, Vector4}; +use crate::geometry::{ + Isometry3, DualQuaternion, Similarity3, SuperTCategoryOf, + TAffine, Transform, Translation3, UnitQuaternion, UnitDualQuaternion +}; + +/* + * This file provides the following conversions: + * ============================================= + * + * DualQuaternion -> DualQuaternion + * UnitDualQuaternion -> UnitDualQuaternion + * UnitDualQuaternion -> Isometry + * UnitDualQuaternion -> Similarity + * UnitDualQuaternion -> Transform + * UnitDualQuaternion -> Matrix (homogeneous) + * + * NOTE: + * UnitDualQuaternion -> DualQuaternion is already provided by: Unit -> T + */ + +impl SubsetOf> for DualQuaternion +where + N1: SimdRealField, + N2: SimdRealField + SupersetOf, +{ + #[inline] + fn to_superset(&self) -> DualQuaternion { + DualQuaternion::from_real_and_dual(self.real.to_superset(), self.dual.to_superset()) + } + + #[inline] + fn is_in_subset(dq: &DualQuaternion) -> bool { + crate::is_convertible::<_, Vector4>(&dq.real.coords) && + crate::is_convertible::<_, Vector4>(&dq.dual.coords) + } + + #[inline] + fn from_superset_unchecked(dq: &DualQuaternion) -> Self { + DualQuaternion::from_real_and_dual( + dq.real.to_subset_unchecked(), dq.dual.to_subset_unchecked() + ) + } +} + +impl SubsetOf> for UnitDualQuaternion +where + N1: SimdRealField, + N2: SimdRealField + SupersetOf, +{ + #[inline] + fn to_superset(&self) -> UnitDualQuaternion { + UnitDualQuaternion::new_unchecked(self.as_ref().to_superset()) + } + + #[inline] + fn is_in_subset(dq: &UnitDualQuaternion) -> bool { + crate::is_convertible::<_, DualQuaternion>(dq.as_ref()) + } + + #[inline] + fn from_superset_unchecked(dq: &UnitDualQuaternion) -> Self { + Self::new_unchecked(crate::convert_ref_unchecked(dq.as_ref())) + } +} + +impl SubsetOf> for UnitDualQuaternion +where + N1: RealField, + N2: RealField + SupersetOf +{ + #[inline] + fn to_superset(&self) -> Isometry3 { + let dq: UnitDualQuaternion = self.to_superset(); + let iso = dq.to_isometry(); + crate::convert_unchecked(iso) + } + + #[inline] + fn is_in_subset(iso: &Isometry3) -> bool { + crate::is_convertible::<_, UnitQuaternion>(&iso.rotation) && + crate::is_convertible::<_, Translation3>(&iso.translation) + } + + #[inline] + fn from_superset_unchecked(iso: &Isometry3) -> Self { + let dq = UnitDualQuaternion::::from_isometry(iso); + crate::convert_unchecked(dq) + } +} + +impl SubsetOf> for UnitDualQuaternion +where + N1: RealField, + N2: RealField + SupersetOf +{ + #[inline] + fn to_superset(&self) -> Similarity3 { + Similarity3::from_isometry(crate::convert_ref(self), N2::one()) + } + + #[inline] + fn is_in_subset(sim: &Similarity3) -> bool { + sim.scaling() == N2::one() + } + + #[inline] + fn from_superset_unchecked(sim: &Similarity3) -> Self { + crate::convert_ref_unchecked(&sim.isometry) + } +} + +impl SubsetOf> for UnitDualQuaternion +where + N1: RealField, + N2: RealField + SupersetOf, + C: SuperTCategoryOf, +{ + #[inline] + fn to_superset(&self) -> Transform { + Transform::from_matrix_unchecked(self.to_homogeneous().to_superset()) + } + + #[inline] + fn is_in_subset(t: &Transform) -> bool { + >::is_in_subset(t.matrix()) + } + + #[inline] + fn from_superset_unchecked(t: &Transform) -> Self { + Self::from_superset_unchecked(t.matrix()) + } +} + +impl> SubsetOf> for UnitDualQuaternion { + #[inline] + fn to_superset(&self) -> Matrix4 { + self.to_homogeneous().to_superset() + } + + #[inline] + fn is_in_subset(m: &Matrix4) -> bool { + crate::is_convertible::<_, Isometry3>(m) + } + + #[inline] + fn from_superset_unchecked(m: &Matrix4) -> Self { + let iso: Isometry3 = crate::convert_ref_unchecked(m); + Self::from_isometry(&iso) + } +} + +impl From> for Matrix4 +where + N::Element: SimdRealField, +{ + #[inline] + fn from(dq: UnitDualQuaternion) -> Self { + dq.to_homogeneous() + } +} + +impl From> for Isometry3 +where + N::Element: SimdRealField, +{ + #[inline] + fn from(dq: UnitDualQuaternion) -> Self { + dq.to_isometry() + } +} + +impl From> for UnitDualQuaternion +where + N::Element: SimdRealField, +{ + #[inline] + fn from(iso: Isometry3) -> Self { + Self::from_isometry(&iso) + } +} + diff --git a/src/geometry/dual_quaternion_ops.rs b/src/geometry/dual_quaternion_ops.rs index 0c9f78f4..95991c63 100644 --- a/src/geometry/dual_quaternion_ops.rs +++ b/src/geometry/dual_quaternion_ops.rs @@ -22,9 +22,16 @@ * - https://cs.gmu.edu/~jmlien/teaching/cs451/uploads/Main/dual-quaternion.pdf */ -use crate::{DualQuaternion, SimdRealField}; +use crate::{ + DualQuaternion, SimdRealField, Point3, Point, Vector3, Isometry3, Quaternion, + UnitDualQuaternion, UnitQuaternion, U1, U3, U4, Unit, Allocator, + DefaultAllocator, Vector +}; +use crate::base::storage::Storage; use std::mem; -use std::ops::{Add, Index, IndexMut, Mul, Sub}; +use std::ops::{ + Add, AddAssign, Div, DivAssign, Index, IndexMut, Mul, MulAssign, Neg, Sub, SubAssign +}; impl AsRef<[N; 8]> for DualQuaternion { #[inline] @@ -56,49 +63,758 @@ impl IndexMut for DualQuaternion { } } -impl Mul> for DualQuaternion +impl Neg for DualQuaternion where N::Element: SimdRealField, { type Output = DualQuaternion; - fn mul(self, rhs: Self) -> Self::Output { - Self::from_real_and_dual( - self.real * rhs.real, - self.real * rhs.dual + self.dual * rhs.real, + #[inline] + fn neg(self) -> Self::Output { + DualQuaternion::from_real_and_dual(-self.real, -self.dual) + } +} + +impl<'a, N: SimdRealField> Neg for &'a DualQuaternion +where + N::Element: SimdRealField, +{ + type Output = DualQuaternion; + + #[inline] + fn neg(self) -> Self::Output { + DualQuaternion::from_real_and_dual(-&self.real, -&self.dual) + } +} + +impl Neg for UnitDualQuaternion +where + N::Element: SimdRealField, +{ + type Output = UnitDualQuaternion; + + #[inline] + fn neg(self) -> Self::Output { + UnitDualQuaternion::new_unchecked(-self.into_inner()) + } +} + +impl<'a, N: SimdRealField> Neg for &'a UnitDualQuaternion +where + N::Element: SimdRealField, +{ + type Output = UnitDualQuaternion; + + #[inline] + fn neg(self) -> Self::Output { + UnitDualQuaternion::new_unchecked(-self.as_ref()) + } +} + +macro_rules! dual_quaternion_op_impl( + ($Op: ident, $op: ident; + ($LhsRDim: ident, $LhsCDim: ident), ($RhsRDim: ident, $RhsCDim: ident) + $(for $Storage: ident: $StoragesBound: ident $(<$($BoundParam: ty),*>)*),*; + $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Result: ty $(=> $VDimA: ty, $VDimB: ty)*; + $action: expr; $($lives: tt),*) => { + impl<$($lives ,)* N: SimdRealField $(, $Storage: $StoragesBound $(<$($BoundParam),*>)*)*> $Op<$Rhs> for $Lhs + where N::Element: SimdRealField, + DefaultAllocator: Allocator + + Allocator { + type Output = $Result; + + #[inline] + fn $op($lhs, $rhs: $Rhs) -> Self::Output { + $action + } + } + } +); + +// DualQuaternion + DualQuaternion +dual_quaternion_op_impl!( + Add, add; + (U4, U1), (U4, U1); + self: &'a DualQuaternion, rhs: &'b DualQuaternion, Output = DualQuaternion; + DualQuaternion::from_real_and_dual( + &self.real + &rhs.real, + &self.dual + &rhs.dual, + ); + 'a, 'b); + +dual_quaternion_op_impl!( + Add, add; + (U4, U1), (U4, U1); + self: &'a DualQuaternion, rhs: DualQuaternion, Output = DualQuaternion; + DualQuaternion::from_real_and_dual( + &self.real + rhs.real, + &self.dual + rhs.dual, + ); + 'a); + +dual_quaternion_op_impl!( + Add, add; + (U4, U1), (U4, U1); + self: DualQuaternion, rhs: &'b DualQuaternion, Output = DualQuaternion; + DualQuaternion::from_real_and_dual( + self.real + &rhs.real, + self.dual + &rhs.dual, + ); + 'b); + +dual_quaternion_op_impl!( + Add, add; + (U4, U1), (U4, U1); + self: DualQuaternion, rhs: DualQuaternion, Output = DualQuaternion; + DualQuaternion::from_real_and_dual( + self.real + rhs.real, + self.dual + rhs.dual, + ); ); + +// DualQuaternion - DualQuaternion +dual_quaternion_op_impl!( + Sub, sub; + (U4, U1), (U4, U1); + self: &'a DualQuaternion, rhs: &'b DualQuaternion, Output = DualQuaternion; + DualQuaternion::from_real_and_dual( + &self.real - &rhs.real, + &self.dual - &rhs.dual, + ); + 'a, 'b); + +dual_quaternion_op_impl!( + Sub, sub; + (U4, U1), (U4, U1); + self: &'a DualQuaternion, rhs: DualQuaternion, Output = DualQuaternion; + DualQuaternion::from_real_and_dual( + &self.real - rhs.real, + &self.dual - rhs.dual, + ); + 'a); + +dual_quaternion_op_impl!( + Sub, sub; + (U4, U1), (U4, U1); + self: DualQuaternion, rhs: &'b DualQuaternion, Output = DualQuaternion; + DualQuaternion::from_real_and_dual( + self.real - &rhs.real, + self.dual - &rhs.dual, + ); + 'b); + +dual_quaternion_op_impl!( + Sub, sub; + (U4, U1), (U4, U1); + self: DualQuaternion, rhs: DualQuaternion, Output = DualQuaternion; + DualQuaternion::from_real_and_dual( + self.real - rhs.real, + self.dual - rhs.dual, + ); ); + +// DualQuaternion × DualQuaternion +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U4, U1); + self: &'a DualQuaternion, rhs: &'b DualQuaternion, Output = DualQuaternion; + DualQuaternion::from_real_and_dual( + &self.real * &rhs.real, + &self.real * &rhs.dual + &self.dual * &rhs.real, + ); + 'a, 'b); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U4, U1); + self: &'a DualQuaternion, rhs: DualQuaternion, Output = DualQuaternion; + self * &rhs; + 'a); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U4, U1); + self: DualQuaternion, rhs: &'b DualQuaternion, Output = DualQuaternion; + &self * rhs; + 'b); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U4, U1); + self: DualQuaternion, rhs: DualQuaternion, Output = DualQuaternion; + &self * &rhs; ); + +// UnitDualQuaternion × UnitDualQuaternion +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U4, U1); + self: &'a UnitDualQuaternion, rhs: &'b UnitDualQuaternion, Output = UnitDualQuaternion; + UnitDualQuaternion::new_unchecked(self.as_ref() * rhs.as_ref()); + 'a, 'b); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U4, U1); + self: &'a UnitDualQuaternion, rhs: UnitDualQuaternion, Output = UnitDualQuaternion; + self * &rhs; + 'a); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U4, U1); + self: UnitDualQuaternion, rhs: &'b UnitDualQuaternion, Output = UnitDualQuaternion; + &self * rhs; + 'b); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U4, U1); + self: UnitDualQuaternion, rhs: UnitDualQuaternion, Output = UnitDualQuaternion; + &self * &rhs; ); + +// UnitDualQuaternion ÷ UnitDualQuaternion +dual_quaternion_op_impl!( + Div, div; + (U4, U1), (U4, U1); + self: &'a UnitDualQuaternion, rhs: &'b UnitDualQuaternion, Output = UnitDualQuaternion; + #[allow(clippy::suspicious_arithmetic_impl)] { self * rhs.inverse() }; + 'a, 'b); + +dual_quaternion_op_impl!( + Div, div; + (U4, U1), (U4, U1); + self: &'a UnitDualQuaternion, rhs: UnitDualQuaternion, Output = UnitDualQuaternion; + self / &rhs; + 'a); + +dual_quaternion_op_impl!( + Div, div; + (U4, U1), (U4, U1); + self: UnitDualQuaternion, rhs: &'b UnitDualQuaternion, Output = UnitDualQuaternion; + &self / rhs; + 'b); + +dual_quaternion_op_impl!( + Div, div; + (U4, U1), (U4, U1); + self: UnitDualQuaternion, rhs: UnitDualQuaternion, Output = UnitDualQuaternion; + &self / &rhs; ); + +// UnitDualQuaternion × UnitQuaternion +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U4, U1); + self: &'a UnitDualQuaternion, rhs: &'b UnitQuaternion, + Output = UnitDualQuaternion => U1, U4; + self * UnitDualQuaternion::::new_unchecked(DualQuaternion::from_real(rhs.into_inner())); + 'a, 'b); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U4, U1); + self: &'a UnitDualQuaternion, rhs: UnitQuaternion, + Output = UnitDualQuaternion => U3, U3; + self * UnitDualQuaternion::::new_unchecked(DualQuaternion::from_real(rhs.into_inner())); + 'a); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U4, U1); + self: UnitDualQuaternion, rhs: &'b UnitQuaternion, + Output = UnitDualQuaternion => U3, U3; + self * UnitDualQuaternion::::new_unchecked(DualQuaternion::from_real(rhs.into_inner())); + 'b); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U4, U1); + self: UnitDualQuaternion, rhs: UnitQuaternion, + Output = UnitDualQuaternion => U3, U3; + self * UnitDualQuaternion::::new_unchecked(DualQuaternion::from_real(rhs.into_inner()));); + +// UnitQuaternion × UnitDualQuaternion +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U4, U1); + self: &'a UnitQuaternion, rhs: &'b UnitDualQuaternion, + Output = UnitDualQuaternion => U1, U4; + UnitDualQuaternion::::new_unchecked(DualQuaternion::from_real(self.into_inner())) * rhs; + 'a, 'b); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U4, U1); + self: &'a UnitQuaternion, rhs: UnitDualQuaternion, + Output = UnitDualQuaternion => U3, U3; + UnitDualQuaternion::::new_unchecked(DualQuaternion::from_real(self.into_inner())) * rhs; + 'a); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U4, U1); + self: UnitQuaternion, rhs: &'b UnitDualQuaternion, + Output = UnitDualQuaternion => U3, U3; + UnitDualQuaternion::::new_unchecked(DualQuaternion::from_real(self.into_inner())) * rhs; + 'b); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U4, U1); + self: UnitQuaternion, rhs: UnitDualQuaternion, + Output = UnitDualQuaternion => U3, U3; + UnitDualQuaternion::::new_unchecked(DualQuaternion::from_real(self.into_inner())) * rhs;); + +// UnitDualQuaternion × Isometry3 +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U3, U1); + self: &'a UnitDualQuaternion, rhs: &'b Isometry3, + Output = UnitDualQuaternion => U3, U1; + self * UnitDualQuaternion::::from_isometry(rhs); + 'a, 'b); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U3, U3); + self: &'a UnitDualQuaternion, rhs: Isometry3, + Output = UnitDualQuaternion => U3, U1; + self * UnitDualQuaternion::::from_isometry(&rhs); + 'a); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U3, U3); + self: UnitDualQuaternion, rhs: &'b Isometry3, + Output = UnitDualQuaternion => U3, U1; + self * UnitDualQuaternion::::from_isometry(rhs); + 'b); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U3, U3); + self: UnitDualQuaternion, rhs: Isometry3, + Output = UnitDualQuaternion => U3, U1; + self * UnitDualQuaternion::::from_isometry(&rhs); ); + +// UnitDualQuaternion ÷ Isometry +dual_quaternion_op_impl!( + Div, div; + (U4, U1), (U3, U1); + self: &'a UnitDualQuaternion, rhs: &'b Isometry3, + Output = UnitDualQuaternion => U3, U1; + // TODO: can we avoid the conversion to a rotation matrix? + self / UnitDualQuaternion::::from_isometry(rhs); + 'a, 'b); + +dual_quaternion_op_impl!( + Div, div; + (U4, U1), (U3, U3); + self: &'a UnitDualQuaternion, rhs: Isometry3, + Output = UnitDualQuaternion => U3, U1; + self / UnitDualQuaternion::::from_isometry(&rhs); + 'a); + +dual_quaternion_op_impl!( + Div, div; + (U4, U1), (U3, U3); + self: UnitDualQuaternion, rhs: &'b Isometry3, + Output = UnitDualQuaternion => U3, U1; + self / UnitDualQuaternion::::from_isometry(rhs); + 'b); + +dual_quaternion_op_impl!( + Div, div; + (U4, U1), (U3, U3); + self: UnitDualQuaternion, rhs: Isometry3, + Output = UnitDualQuaternion => U3, U1; + self / UnitDualQuaternion::::from_isometry(&rhs); ); + +// Isometry × UnitDualQuaternion +dual_quaternion_op_impl!( + Mul, mul; + (U3, U1), (U4, U1); + self: &'a Isometry3, rhs: &'b UnitDualQuaternion, + Output = UnitDualQuaternion => U3, U1; + UnitDualQuaternion::::from_isometry(self) * rhs; + 'a, 'b); + +dual_quaternion_op_impl!( + Mul, mul; + (U3, U1), (U4, U1); + self: &'a Isometry3, rhs: UnitDualQuaternion, + Output = UnitDualQuaternion => U3, U1; + UnitDualQuaternion::::from_isometry(self) * rhs; + 'a); + +dual_quaternion_op_impl!( + Mul, mul; + (U3, U1), (U4, U1); + self: Isometry3, rhs: &'b UnitDualQuaternion, + Output = UnitDualQuaternion => U3, U1; + UnitDualQuaternion::::from_isometry(&self) * rhs; + 'b); + +dual_quaternion_op_impl!( + Mul, mul; + (U3, U1), (U4, U1); + self: Isometry3, rhs: UnitDualQuaternion, + Output = UnitDualQuaternion => U3, U1; + UnitDualQuaternion::::from_isometry(&self) * rhs; ); + +// Isometry ÷ UnitDualQuaternion +dual_quaternion_op_impl!( + Div, div; + (U3, U1), (U4, U1); + self: &'a Isometry3, rhs: &'b UnitDualQuaternion, + Output = UnitDualQuaternion => U3, U1; + // TODO: can we avoid the conversion from a rotation matrix? + UnitDualQuaternion::::from_isometry(self) / rhs; + 'a, 'b); + +dual_quaternion_op_impl!( + Div, div; + (U3, U1), (U4, U1); + self: &'a Isometry3, rhs: UnitDualQuaternion, + Output = UnitDualQuaternion => U3, U1; + UnitDualQuaternion::::from_isometry(self) / rhs; + 'a); + +dual_quaternion_op_impl!( + Div, div; + (U3, U1), (U4, U1); + self: Isometry3, rhs: &'b UnitDualQuaternion, + Output = UnitDualQuaternion => U3, U1; + UnitDualQuaternion::::from_isometry(&self) / rhs; + 'b); + +dual_quaternion_op_impl!( + Div, div; + (U3, U1), (U4, U1); + self: Isometry3, rhs: UnitDualQuaternion, + Output = UnitDualQuaternion => U3, U1; + UnitDualQuaternion::::from_isometry(&self) / rhs; ); + +// UnitDualQuaternion × Vector +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U3, U1) for SB: Storage ; + self: &'a UnitDualQuaternion, rhs: &'b Vector, + Output = Vector3 => U3, U1; + Unit::new_unchecked(self.as_ref().real) * rhs; + 'a, 'b); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U3, U1) for SB: Storage ; + self: &'a UnitDualQuaternion, rhs: Vector, + Output = Vector3 => U3, U1; + self * &rhs; + 'a); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U3, U1) for SB: Storage ; + self: UnitDualQuaternion, rhs: &'b Vector, + Output = Vector3 => U3, U1; + &self * rhs; + 'b); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U3, U1) for SB: Storage ; + self: UnitDualQuaternion, rhs: Vector, + Output = Vector3 => U3, U1; + &self * &rhs; ); + +// UnitDualQuaternion × Point +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U3, U1); + self: &'a UnitDualQuaternion, rhs: &'b Point3, + Output = Point3 => U3, U1; + { + let two: N = crate::convert(2.0f64); + let q_point = Quaternion::from_parts(N::zero(), rhs.coords.clone()); + Point::from( + ((self.as_ref().real * q_point + self.as_ref().dual * two) * self.as_ref().real.conjugate()) + .vector() + .into_owned(), ) + }; + 'a, 'b); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U3, U1); + self: &'a UnitDualQuaternion, rhs: Point3, + Output = Point3 => U3, U1; + self * &rhs; + 'a); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U3, U1); + self: UnitDualQuaternion, rhs: &'b Point3, + Output = Point3 => U3, U1; + &self * rhs; + 'b); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U3, U1); + self: UnitDualQuaternion, rhs: Point3, + Output = Point3 => U3, U1; + &self * &rhs; ); + +// UnitDualQuaternion × Unit +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U3, U1) for SB: Storage ; + self: &'a UnitDualQuaternion, rhs: &'b Unit>, + Output = Unit> => U3, U4; + Unit::new_unchecked(self * rhs.as_ref()); + 'a, 'b); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U3, U1) for SB: Storage ; + self: &'a UnitDualQuaternion, rhs: Unit>, + Output = Unit> => U3, U4; + Unit::new_unchecked(self * rhs.into_inner()); + 'a); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U3, U1) for SB: Storage ; + self: UnitDualQuaternion, rhs: &'b Unit>, + Output = Unit> => U3, U4; + Unit::new_unchecked(self * rhs.as_ref()); + 'b); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U3, U1) for SB: Storage ; + self: UnitDualQuaternion, rhs: Unit>, + Output = Unit> => U3, U4; + Unit::new_unchecked(self * rhs.into_inner()); ); + +macro_rules! left_scalar_mul_impl( + ($($T: ty),* $(,)*) => {$( + impl Mul> for $T { + type Output = DualQuaternion<$T>; + + #[inline] + fn mul(self, right: DualQuaternion<$T>) -> Self::Output { + DualQuaternion::from_real_and_dual( + self * right.real, + self * right.dual + ) + } + } + + impl<'b> Mul<&'b DualQuaternion<$T>> for $T { + type Output = DualQuaternion<$T>; + + #[inline] + fn mul(self, right: &'b DualQuaternion<$T>) -> Self::Output { + DualQuaternion::from_real_and_dual( + self * &right.real, + self * &right.dual + ) + } + } + )*} +); + +left_scalar_mul_impl!(f32, f64); + +macro_rules! dual_quaternion_op_impl( + ($OpAssign: ident, $op_assign: ident; + ($LhsRDim: ident, $LhsCDim: ident), ($RhsRDim: ident, $RhsCDim: ident); + $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty $(=> $VDimA: ty, $VDimB: ty)*; + $action: expr; $($lives: tt),*) => { + impl<$($lives ,)* N: SimdRealField> $OpAssign<$Rhs> for $Lhs + where N::Element: SimdRealField, + DefaultAllocator: Allocator + + Allocator { + + #[inline] + fn $op_assign(&mut $lhs, $rhs: $Rhs) { + $action + } + } } -} +); -impl Mul for DualQuaternion -where - N::Element: SimdRealField, -{ - type Output = DualQuaternion; +// DualQuaternion += DualQuaternion +dual_quaternion_op_impl!( + AddAssign, add_assign; + (U4, U1), (U4, U1); + self: DualQuaternion, rhs: &'b DualQuaternion; + { + self.real += &rhs.real; + self.dual += &rhs.dual; + }; + 'b); - fn mul(self, rhs: N) -> Self::Output { - Self::from_real_and_dual(self.real * rhs, self.dual * rhs) - } -} +dual_quaternion_op_impl!( + AddAssign, add_assign; + (U4, U1), (U4, U1); + self: DualQuaternion, rhs: DualQuaternion; + { + self.real += rhs.real; + self.dual += rhs.dual; + };); -impl Add> for DualQuaternion -where - N::Element: SimdRealField, -{ - type Output = DualQuaternion; +// DualQuaternion -= DualQuaternion +dual_quaternion_op_impl!( + SubAssign, sub_assign; + (U4, U1), (U4, U1); + self: DualQuaternion, rhs: &'b DualQuaternion; + { + self.real -= &rhs.real; + self.dual -= &rhs.dual; + }; + 'b); - fn add(self, rhs: DualQuaternion) -> Self::Output { - Self::from_real_and_dual(self.real + rhs.real, self.dual + rhs.dual) - } -} +dual_quaternion_op_impl!( + SubAssign, sub_assign; + (U4, U1), (U4, U1); + self: DualQuaternion, rhs: DualQuaternion; + { + self.real -= rhs.real; + self.dual -= rhs.dual; + };); -impl Sub> for DualQuaternion -where - N::Element: SimdRealField, -{ - type Output = DualQuaternion; +// DualQuaternion ×= DualQuaternion +dual_quaternion_op_impl!( + MulAssign, mul_assign; + (U4, U1), (U4, U1); + self: DualQuaternion, rhs: &'b DualQuaternion; + { + let res = &*self * rhs; + self.real.coords.copy_from(&res.real.coords); + self.dual.coords.copy_from(&res.dual.coords); + }; + 'b); - fn sub(self, rhs: DualQuaternion) -> Self::Output { - Self::from_real_and_dual(self.real - rhs.real, self.dual - rhs.dual) - } -} +dual_quaternion_op_impl!( + MulAssign, mul_assign; + (U4, U1), (U4, U1); + self: DualQuaternion, rhs: DualQuaternion; + *self *= &rhs; ); + +// UnitDualQuaternion ×= UnitDualQuaternion +dual_quaternion_op_impl!( + MulAssign, mul_assign; + (U4, U1), (U4, U1); + self: UnitDualQuaternion, rhs: &'b UnitDualQuaternion; + { + let res = &*self * 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); + }; + 'b); + +dual_quaternion_op_impl!( + MulAssign, mul_assign; + (U4, U1), (U4, U1); + self: UnitDualQuaternion, rhs: UnitDualQuaternion; + *self *= &rhs; ); + +// UnitDualQuaternion ÷= UnitDualQuaternion +dual_quaternion_op_impl!( + DivAssign, div_assign; + (U4, U1), (U4, U1); + self: UnitDualQuaternion, rhs: &'b UnitDualQuaternion; + { + let res = &*self / 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); + }; + 'b); + +dual_quaternion_op_impl!( + DivAssign, div_assign; + (U4, U1), (U4, U1); + self: UnitDualQuaternion, rhs: UnitDualQuaternion; + *self /= &rhs; ); + +// UnitDualQuaternion ×= Isometry3 +dual_quaternion_op_impl!( + MulAssign, mul_assign; + (U4, U1), (U3, U1); + self: UnitDualQuaternion, rhs: &'b Isometry3 => U3, U1; + { + let res = &*self * 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); + }; + 'b); + +dual_quaternion_op_impl!( + MulAssign, mul_assign; + (U4, U1), (U3, U1); + self: UnitDualQuaternion, rhs: Isometry3 => U3, U1; + *self *= &rhs; ); + +// UnitDualQuaternion ÷= Isometry3 +dual_quaternion_op_impl!( + DivAssign, div_assign; + (U4, U1), (U3, U1); + self: UnitDualQuaternion, rhs: &'b Isometry3 => U3, U1; + { + let res = &*self / 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); + }; + 'b); + +dual_quaternion_op_impl!( + DivAssign, div_assign; + (U4, U1), (U3, U1); + self: UnitDualQuaternion, rhs: Isometry3 => U3, U1; + *self /= &rhs; ); + +macro_rules! scalar_op_impl( + ($($Op: ident, $op: ident, $OpAssign: ident, $op_assign: ident);* $(;)*) => {$( + impl $Op for DualQuaternion + where N::Element: SimdRealField { + type Output = DualQuaternion; + + #[inline] + fn $op(self, n: N) -> Self::Output { + DualQuaternion::from_real_and_dual( + self.real.$op(n), + self.dual.$op(n) + ) + } + } + + impl<'a, N: SimdRealField> $Op for &'a DualQuaternion + where N::Element: SimdRealField { + type Output = DualQuaternion; + + #[inline] + fn $op(self, n: N) -> Self::Output { + DualQuaternion::from_real_and_dual( + self.real.$op(n), + self.dual.$op(n) + ) + } + } + + impl $OpAssign for DualQuaternion + where N::Element: SimdRealField { + + #[inline] + fn $op_assign(&mut self, n: N) { + self.real.$op_assign(n); + self.dual.$op_assign(n); + } + } + )*} +); + +scalar_op_impl!( + Mul, mul, MulAssign, mul_assign; + Div, div, DivAssign, div_assign; +); diff --git a/src/geometry/isometry_conversion.rs b/src/geometry/isometry_conversion.rs index e3416cac..5939c2c1 100644 --- a/src/geometry/isometry_conversion.rs +++ b/src/geometry/isometry_conversion.rs @@ -6,7 +6,8 @@ use crate::base::dimension::{DimMin, DimName, DimNameAdd, DimNameSum, U1}; use crate::base::{DefaultAllocator, MatrixN, Scalar}; use crate::geometry::{ - AbstractRotation, Isometry, Similarity, SuperTCategoryOf, TAffine, Transform, Translation, + AbstractRotation, Isometry, Isometry3, Similarity, SuperTCategoryOf, TAffine, Transform, + Translation, UnitDualQuaternion, UnitQuaternion }; /* @@ -14,6 +15,7 @@ use crate::geometry::{ * ============================================= * * Isometry -> Isometry + * Isometry3 -> UnitDualQuaternion * Isometry -> Similarity * Isometry -> Transform * Isometry -> Matrix (homogeneous) @@ -47,6 +49,30 @@ where } } +impl SubsetOf> for Isometry3 +where + N1: RealField, + N2: RealField + SupersetOf, +{ + #[inline] + fn to_superset(&self) -> UnitDualQuaternion { + let dq = UnitDualQuaternion::::from_isometry(self); + dq.to_superset() + } + + #[inline] + fn is_in_subset(dq: &UnitDualQuaternion) -> bool { + crate::is_convertible::<_, UnitQuaternion>(&dq.rotation()) && + crate::is_convertible::<_, Translation>(&dq.translation()) + } + + #[inline] + fn from_superset_unchecked(dq: &UnitDualQuaternion) -> Self { + let dq: UnitDualQuaternion = crate::convert_ref_unchecked(dq); + dq.to_isometry() + } +} + impl SubsetOf> for Isometry where N1: RealField, diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index 19313b65..5fa8c094 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -36,7 +36,10 @@ mod quaternion_ops; mod quaternion_simba; mod dual_quaternion; +#[cfg(feature = "alga")] +mod dual_quaternion_alga; mod dual_quaternion_construction; +mod dual_quaternion_conversion; mod dual_quaternion_ops; mod unit_complex; diff --git a/src/geometry/quaternion_conversion.rs b/src/geometry/quaternion_conversion.rs index b57cc52b..ff2ab92a 100644 --- a/src/geometry/quaternion_conversion.rs +++ b/src/geometry/quaternion_conversion.rs @@ -10,7 +10,7 @@ use crate::base::dimension::U3; use crate::base::{Matrix3, Matrix4, Scalar, Vector4}; use crate::geometry::{ AbstractRotation, Isometry, Quaternion, Rotation, Rotation3, Similarity, SuperTCategoryOf, - TAffine, Transform, Translation, UnitQuaternion, + TAffine, Transform, Translation, UnitQuaternion, UnitDualQuaternion }; /* @@ -21,6 +21,7 @@ use crate::geometry::{ * UnitQuaternion -> UnitQuaternion * UnitQuaternion -> Rotation * UnitQuaternion -> Isometry + * UnitQuaternion -> UnitDualQuaternion * UnitQuaternion -> Similarity * UnitQuaternion -> Transform * UnitQuaternion -> Matrix (homogeneous) @@ -121,6 +122,28 @@ where } } +impl SubsetOf> for UnitQuaternion +where + N1: RealField, + N2: RealField + SupersetOf +{ + #[inline] + fn to_superset(&self) -> UnitDualQuaternion { + let q: UnitQuaternion = crate::convert_ref(self); + UnitDualQuaternion::from_rotation(q) + } + + #[inline] + fn is_in_subset(dq: &UnitDualQuaternion) -> bool { + dq.translation().vector.is_zero() + } + + #[inline] + fn from_superset_unchecked(dq: &UnitDualQuaternion) -> Self { + crate::convert_unchecked(dq.rotation()) + } +} + impl SubsetOf> for UnitQuaternion where N1: RealField, diff --git a/src/geometry/rotation_conversion.rs b/src/geometry/rotation_conversion.rs index c49b92c4..5e2c6014 100644 --- a/src/geometry/rotation_conversion.rs +++ b/src/geometry/rotation_conversion.rs @@ -12,7 +12,7 @@ use crate::base::{DefaultAllocator, Matrix2, Matrix3, Matrix4, MatrixN, Scalar}; use crate::geometry::{ AbstractRotation, Isometry, Rotation, Rotation2, Rotation3, Similarity, SuperTCategoryOf, - TAffine, Transform, Translation, UnitComplex, UnitQuaternion, + TAffine, Transform, Translation, UnitComplex, UnitQuaternion, UnitDualQuaternion, }; /* @@ -21,6 +21,7 @@ use crate::geometry::{ * * Rotation -> Rotation * Rotation3 -> UnitQuaternion + * Rotation3 -> UnitDualQuaternion * Rotation2 -> UnitComplex * Rotation -> Isometry * Rotation -> Similarity @@ -75,6 +76,31 @@ where } } +impl SubsetOf> for Rotation3 +where + N1: RealField, + N2: RealField + SupersetOf, +{ + #[inline] + fn to_superset(&self) -> UnitDualQuaternion { + let q = UnitQuaternion::::from_rotation_matrix(self); + let dq = UnitDualQuaternion::from_rotation(q); + dq.to_superset() + } + + #[inline] + fn is_in_subset(dq: &UnitDualQuaternion) -> bool { + crate::is_convertible::<_, UnitQuaternion>(&dq.rotation()) && + dq.translation().vector.is_zero() + } + + #[inline] + fn from_superset_unchecked(dq: &UnitDualQuaternion) -> Self { + let dq: UnitDualQuaternion = crate::convert_ref_unchecked(dq); + dq.rotation().to_rotation_matrix() + } +} + impl SubsetOf> for Rotation2 where N1: RealField, diff --git a/src/geometry/translation_conversion.rs b/src/geometry/translation_conversion.rs index 0754a678..f275297e 100644 --- a/src/geometry/translation_conversion.rs +++ b/src/geometry/translation_conversion.rs @@ -9,6 +9,7 @@ use crate::base::{DefaultAllocator, MatrixN, Scalar, VectorN}; use crate::geometry::{ AbstractRotation, Isometry, Similarity, SuperTCategoryOf, TAffine, Transform, Translation, + Translation3, UnitDualQuaternion, UnitQuaternion }; /* @@ -17,6 +18,7 @@ use crate::geometry::{ * * Translation -> Translation * Translation -> Isometry + * Translation3 -> UnitDualQuaternion * Translation -> Similarity * Translation -> Transform * Translation -> Matrix (homogeneous) @@ -69,6 +71,30 @@ where } } +impl SubsetOf> for Translation3 +where + N1: RealField, + N2: RealField + SupersetOf, +{ + #[inline] + fn to_superset(&self) -> UnitDualQuaternion { + let dq = UnitDualQuaternion::::from_parts(self.clone(), UnitQuaternion::identity()); + dq.to_superset() + } + + #[inline] + fn is_in_subset(dq: &UnitDualQuaternion) -> bool { + crate::is_convertible::<_, Translation>(&dq.translation()) && + dq.rotation() == UnitQuaternion::identity() + } + + #[inline] + fn from_superset_unchecked(dq: &UnitDualQuaternion) -> Self { + let dq: UnitDualQuaternion = crate::convert_ref_unchecked(dq); + dq.translation() + } +} + impl SubsetOf> for Translation where N1: RealField, From 6be0365203de7d09f418d0a42dba400a2839578f Mon Sep 17 00:00:00 2001 From: Terence Date: Thu, 28 Jan 2021 18:45:34 -0500 Subject: [PATCH 03/10] add integration test --- src/geometry/dual_quaternion_construction.rs | 29 ++ src/geometry/dual_quaternion_ops.rs | 291 ++++++++++++++++++- tests/geometry/dual_quaternion.rs | 172 +++++++++++ tests/geometry/mod.rs | 1 + 4 files changed, 492 insertions(+), 1 deletion(-) create mode 100644 tests/geometry/dual_quaternion.rs 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; From 388b77108eaa8dc8e99da7c1537048eeb5077e41 Mon Sep 17 00:00:00 2001 From: Terence Date: Thu, 28 Jan 2021 18:46:14 -0500 Subject: [PATCH 04/10] rustfmt --- src/geometry/dual_quaternion.rs | 23 ++++++++------- src/geometry/dual_quaternion_alga.rs | 26 ++++++++++------ src/geometry/dual_quaternion_construction.rs | 31 ++++++++------------ src/geometry/dual_quaternion_conversion.rs | 24 ++++++++------- src/geometry/dual_quaternion_ops.rs | 12 ++++---- src/geometry/isometry_conversion.rs | 6 ++-- src/geometry/quaternion_conversion.rs | 4 +-- src/geometry/rotation_conversion.rs | 6 ++-- src/geometry/translation_conversion.rs | 6 ++-- tests/geometry/dual_quaternion.rs | 8 ++--- tests/geometry/mod.rs | 2 +- 11 files changed, 77 insertions(+), 71 deletions(-) diff --git a/src/geometry/dual_quaternion.rs b/src/geometry/dual_quaternion.rs index 63c5fa03..dac11725 100644 --- a/src/geometry/dual_quaternion.rs +++ b/src/geometry/dual_quaternion.rs @@ -1,11 +1,11 @@ -use std::fmt; -use approx::{AbsDiffEq, RelativeEq, UlpsEq}; use crate::{ - Quaternion, SimdRealField, VectorN, U8, Vector3, Point3, Isometry3, Unit, Matrix4, - Translation3, UnitQuaternion, Scalar, Normed + Isometry3, Matrix4, Normed, Point3, Quaternion, Scalar, SimdRealField, Translation3, Unit, + UnitQuaternion, Vector3, VectorN, U8, }; +use approx::{AbsDiffEq, RelativeEq, UlpsEq}; #[cfg(feature = "serde-serialize")] use serde::{Deserialize, Deserializer, Serialize, Serializer}; +use std::fmt; use simba::scalar::{ClosedNeg, RealField}; @@ -432,7 +432,9 @@ 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).inverse().into_inner(); + let real = Unit::new_unchecked(self.as_ref().real) + .inverse() + .into_inner(); let dual = -real * self.as_ref().dual * real; UnitDualQuaternion::new_unchecked(DualQuaternion { real, dual }) } @@ -634,9 +636,11 @@ where moment * sin + direction * (pitch * half * cos), ); - Some(self * UnitDualQuaternion::new_unchecked( - DualQuaternion::from_real_and_dual(real, dual) - )) + Some( + self * UnitDualQuaternion::new_unchecked(DualQuaternion::from_real_and_dual( + real, dual, + )), + ) } /// Return the rotation part of this unit dual quaternion. @@ -844,8 +848,7 @@ where /// assert_relative_eq!(dq.to_homogeneous(), expected, epsilon = 1.0e-6); /// ``` #[inline] - pub fn to_homogeneous(&self) -> Matrix4 - { + pub fn to_homogeneous(&self) -> Matrix4 { self.to_isometry().to_homogeneous() } } diff --git a/src/geometry/dual_quaternion_alga.rs b/src/geometry/dual_quaternion_alga.rs index d0dfdb24..f6ee9e10 100644 --- a/src/geometry/dual_quaternion_alga.rs +++ b/src/geometry/dual_quaternion_alga.rs @@ -7,13 +7,12 @@ use alga::general::{ }; use alga::linear::{ AffineTransformation, DirectIsometry, FiniteDimVectorSpace, Isometry, NormedSpace, - ProjectiveTransformation, Similarity, Transformation, - VectorSpace, + ProjectiveTransformation, Similarity, Transformation, VectorSpace, }; use crate::base::Vector3; use crate::geometry::{ - Point3, Quaternion, UnitQuaternion, DualQuaternion, UnitDualQuaternion, Translation3 + DualQuaternion, Point3, Quaternion, Translation3, UnitDualQuaternion, UnitQuaternion, }; impl Identity for DualQuaternion { @@ -103,12 +102,12 @@ impl FiniteDimVectorSpace for DualQuate if i < 4 { DualQuaternion::from_real_and_dual( Quaternion::canonical_basis_element(i), - Quaternion::zero() + Quaternion::zero(), ) } else { DualQuaternion::from_real_and_dual( Quaternion::zero(), - Quaternion::canonical_basis_element(i - 4) + Quaternion::canonical_basis_element(i - 4), ) } } @@ -157,7 +156,10 @@ impl NormedSpace for DualQuaternion fn try_normalize(&self, min_norm: N) -> Option { let real_norm = self.real.norm(); if real_norm > min_norm { - Some(Self::from_real_and_dual(self.real / real_norm, self.dual / real_norm)) + Some(Self::from_real_and_dual( + self.real / real_norm, + self.dual / real_norm, + )) } else { None } @@ -188,7 +190,9 @@ impl Identity for UnitD } } -impl AbstractMagma for UnitDualQuaternion { +impl AbstractMagma + for UnitDualQuaternion +{ #[inline] fn operate(&self, rhs: &Self) -> Self { self * rhs @@ -253,7 +257,12 @@ impl AffineTransformation> #[inline] fn decompose(&self) -> (Self::Translation, Self::Rotation, Id, Self::Rotation) { - (self.translation(), self.rotation(), Id::new(), UnitQuaternion::identity()) + ( + self.translation(), + self.rotation(), + Id::new(), + UnitQuaternion::identity(), + ) } #[inline] @@ -313,4 +322,3 @@ macro_rules! marker_impl( ); marker_impl!(Isometry, DirectIsometry); - diff --git a/src/geometry/dual_quaternion_construction.rs b/src/geometry/dual_quaternion_construction.rs index daedc239..95c208ce 100644 --- a/src/geometry/dual_quaternion_construction.rs +++ b/src/geometry/dual_quaternion_construction.rs @@ -1,10 +1,10 @@ -#[cfg(feature = "arbitrary")] -use quickcheck::{Arbitrary, Gen}; use crate::{ - DualQuaternion, Quaternion, UnitDualQuaternion, SimdRealField, Isometry3, - Translation3, UnitQuaternion + DualQuaternion, Isometry3, Quaternion, SimdRealField, Translation3, UnitDualQuaternion, + UnitQuaternion, }; use num::{One, Zero}; +#[cfg(feature = "arbitrary")] +use quickcheck::{Arbitrary, Gen}; impl DualQuaternion { /// Creates a dual quaternion from its rotation and translation components. @@ -50,9 +50,8 @@ impl DualQuaternion { impl DualQuaternion where - N::Element: SimdRealField + N::Element: SimdRealField, { - /// Creates a dual quaternion from only its real part, with no translation /// component. /// @@ -67,7 +66,10 @@ where /// ``` #[inline] pub fn from_real(real: Quaternion) -> Self { - Self { real, dual: Quaternion::zero() } + Self { + real, + dual: Quaternion::zero(), + } } } @@ -87,10 +89,7 @@ where { #[inline] fn zero() -> Self { - DualQuaternion::from_real_and_dual( - Quaternion::zero(), - Quaternion::zero() - ) + DualQuaternion::from_real_and_dual(Quaternion::zero(), Quaternion::zero()) } #[inline] @@ -107,10 +106,7 @@ where { #[inline] fn arbitrary(rng: &mut G) -> Self { - Self::from_real_and_dual( - Arbitrary::arbitrary(rng), - Arbitrary::arbitrary(rng) - ) + Self::from_real_and_dual(Arbitrary::arbitrary(rng), Arbitrary::arbitrary(rng)) } } @@ -151,10 +147,7 @@ where /// assert_relative_eq!(dq * point, Point3::new(1.0, 0.0, 2.0), epsilon = 1.0e-6); /// ``` #[inline] - pub fn from_parts( - translation: Translation3, - rotation: UnitQuaternion - ) -> Self { + pub fn from_parts(translation: Translation3, rotation: UnitQuaternion) -> Self { let half: N = crate::convert(0.5f64); UnitDualQuaternion::new_unchecked(DualQuaternion { real: rotation.clone().into_inner(), diff --git a/src/geometry/dual_quaternion_conversion.rs b/src/geometry/dual_quaternion_conversion.rs index 41548391..20c6895a 100644 --- a/src/geometry/dual_quaternion_conversion.rs +++ b/src/geometry/dual_quaternion_conversion.rs @@ -4,8 +4,8 @@ use simba::simd::SimdRealField; use crate::base::dimension::U3; use crate::base::{Matrix4, Vector4}; use crate::geometry::{ - Isometry3, DualQuaternion, Similarity3, SuperTCategoryOf, - TAffine, Transform, Translation3, UnitQuaternion, UnitDualQuaternion + DualQuaternion, Isometry3, Similarity3, SuperTCategoryOf, TAffine, Transform, Translation3, + UnitDualQuaternion, UnitQuaternion, }; /* @@ -35,14 +35,15 @@ where #[inline] fn is_in_subset(dq: &DualQuaternion) -> bool { - crate::is_convertible::<_, Vector4>(&dq.real.coords) && - crate::is_convertible::<_, Vector4>(&dq.dual.coords) + crate::is_convertible::<_, Vector4>(&dq.real.coords) + && crate::is_convertible::<_, Vector4>(&dq.dual.coords) } #[inline] fn from_superset_unchecked(dq: &DualQuaternion) -> Self { DualQuaternion::from_real_and_dual( - dq.real.to_subset_unchecked(), dq.dual.to_subset_unchecked() + dq.real.to_subset_unchecked(), + dq.dual.to_subset_unchecked(), ) } } @@ -71,7 +72,7 @@ where impl SubsetOf> for UnitDualQuaternion where N1: RealField, - N2: RealField + SupersetOf + N2: RealField + SupersetOf, { #[inline] fn to_superset(&self) -> Isometry3 { @@ -82,8 +83,8 @@ where #[inline] fn is_in_subset(iso: &Isometry3) -> bool { - crate::is_convertible::<_, UnitQuaternion>(&iso.rotation) && - crate::is_convertible::<_, Translation3>(&iso.translation) + crate::is_convertible::<_, UnitQuaternion>(&iso.rotation) + && crate::is_convertible::<_, Translation3>(&iso.translation) } #[inline] @@ -96,7 +97,7 @@ where impl SubsetOf> for UnitDualQuaternion where N1: RealField, - N2: RealField + SupersetOf + N2: RealField + SupersetOf, { #[inline] fn to_superset(&self) -> Similarity3 { @@ -136,7 +137,9 @@ where } } -impl> SubsetOf> for UnitDualQuaternion { +impl> SubsetOf> + for UnitDualQuaternion +{ #[inline] fn to_superset(&self) -> Matrix4 { self.to_homogeneous().to_superset() @@ -183,4 +186,3 @@ where Self::from_isometry(&iso) } } - diff --git a/src/geometry/dual_quaternion_ops.rs b/src/geometry/dual_quaternion_ops.rs index 098fde6b..17644302 100644 --- a/src/geometry/dual_quaternion_ops.rs +++ b/src/geometry/dual_quaternion_ops.rs @@ -22,15 +22,15 @@ * - https://cs.gmu.edu/~jmlien/teaching/cs451/uploads/Main/dual-quaternion.pdf */ -use crate::{ - DualQuaternion, SimdRealField, Point3, Point, Vector3, Isometry3, Quaternion, - UnitDualQuaternion, UnitQuaternion, U1, U3, U4, Unit, Allocator, - DefaultAllocator, Vector, Translation3 -}; use crate::base::storage::Storage; +use crate::{ + Allocator, DefaultAllocator, DualQuaternion, Isometry3, Point, Point3, Quaternion, + SimdRealField, Translation3, Unit, UnitDualQuaternion, UnitQuaternion, Vector, Vector3, U1, U3, + U4, +}; use std::mem; use std::ops::{ - Add, AddAssign, Div, DivAssign, Index, IndexMut, Mul, MulAssign, Neg, Sub, SubAssign + Add, AddAssign, Div, DivAssign, Index, IndexMut, Mul, MulAssign, Neg, Sub, SubAssign, }; impl AsRef<[N; 8]> for DualQuaternion { diff --git a/src/geometry/isometry_conversion.rs b/src/geometry/isometry_conversion.rs index 5939c2c1..0c89958a 100644 --- a/src/geometry/isometry_conversion.rs +++ b/src/geometry/isometry_conversion.rs @@ -7,7 +7,7 @@ use crate::base::{DefaultAllocator, MatrixN, Scalar}; use crate::geometry::{ AbstractRotation, Isometry, Isometry3, Similarity, SuperTCategoryOf, TAffine, Transform, - Translation, UnitDualQuaternion, UnitQuaternion + Translation, UnitDualQuaternion, UnitQuaternion, }; /* @@ -62,8 +62,8 @@ where #[inline] fn is_in_subset(dq: &UnitDualQuaternion) -> bool { - crate::is_convertible::<_, UnitQuaternion>(&dq.rotation()) && - crate::is_convertible::<_, Translation>(&dq.translation()) + crate::is_convertible::<_, UnitQuaternion>(&dq.rotation()) + && crate::is_convertible::<_, Translation>(&dq.translation()) } #[inline] diff --git a/src/geometry/quaternion_conversion.rs b/src/geometry/quaternion_conversion.rs index ff2ab92a..2707419e 100644 --- a/src/geometry/quaternion_conversion.rs +++ b/src/geometry/quaternion_conversion.rs @@ -10,7 +10,7 @@ use crate::base::dimension::U3; use crate::base::{Matrix3, Matrix4, Scalar, Vector4}; use crate::geometry::{ AbstractRotation, Isometry, Quaternion, Rotation, Rotation3, Similarity, SuperTCategoryOf, - TAffine, Transform, Translation, UnitQuaternion, UnitDualQuaternion + TAffine, Transform, Translation, UnitDualQuaternion, UnitQuaternion, }; /* @@ -125,7 +125,7 @@ where impl SubsetOf> for UnitQuaternion where N1: RealField, - N2: RealField + SupersetOf + N2: RealField + SupersetOf, { #[inline] fn to_superset(&self) -> UnitDualQuaternion { diff --git a/src/geometry/rotation_conversion.rs b/src/geometry/rotation_conversion.rs index 5e2c6014..accf9a41 100644 --- a/src/geometry/rotation_conversion.rs +++ b/src/geometry/rotation_conversion.rs @@ -12,7 +12,7 @@ use crate::base::{DefaultAllocator, Matrix2, Matrix3, Matrix4, MatrixN, Scalar}; use crate::geometry::{ AbstractRotation, Isometry, Rotation, Rotation2, Rotation3, Similarity, SuperTCategoryOf, - TAffine, Transform, Translation, UnitComplex, UnitQuaternion, UnitDualQuaternion, + TAffine, Transform, Translation, UnitComplex, UnitDualQuaternion, UnitQuaternion, }; /* @@ -90,8 +90,8 @@ where #[inline] fn is_in_subset(dq: &UnitDualQuaternion) -> bool { - crate::is_convertible::<_, UnitQuaternion>(&dq.rotation()) && - dq.translation().vector.is_zero() + crate::is_convertible::<_, UnitQuaternion>(&dq.rotation()) + && dq.translation().vector.is_zero() } #[inline] diff --git a/src/geometry/translation_conversion.rs b/src/geometry/translation_conversion.rs index f275297e..9e915073 100644 --- a/src/geometry/translation_conversion.rs +++ b/src/geometry/translation_conversion.rs @@ -9,7 +9,7 @@ use crate::base::{DefaultAllocator, MatrixN, Scalar, VectorN}; use crate::geometry::{ AbstractRotation, Isometry, Similarity, SuperTCategoryOf, TAffine, Transform, Translation, - Translation3, UnitDualQuaternion, UnitQuaternion + Translation3, UnitDualQuaternion, UnitQuaternion, }; /* @@ -84,8 +84,8 @@ where #[inline] fn is_in_subset(dq: &UnitDualQuaternion) -> bool { - crate::is_convertible::<_, Translation>(&dq.translation()) && - dq.rotation() == UnitQuaternion::identity() + crate::is_convertible::<_, Translation>(&dq.translation()) + && dq.rotation() == UnitQuaternion::identity() } #[inline] diff --git a/tests/geometry/dual_quaternion.rs b/tests/geometry/dual_quaternion.rs index 1baced29..9cfca879 100644 --- a/tests/geometry/dual_quaternion.rs +++ b/tests/geometry/dual_quaternion.rs @@ -1,9 +1,7 @@ #![cfg(feature = "arbitrary")] #![allow(non_snake_case)] -use na::{ - Isometry3, Point3, Translation3, UnitQuaternion, UnitDualQuaternion, Vector3, -}; +use na::{Isometry3, Point3, Translation3, UnitDualQuaternion, UnitQuaternion, Vector3}; quickcheck!( fn isometry_equivalence(iso: Isometry3, p: Point3, v: Vector3) -> bool { @@ -25,7 +23,9 @@ quickcheck!( } fn multiply_equals_alga_transform( - dq: UnitDualQuaternion, v: Vector3, p: Point3 + dq: UnitDualQuaternion, + v: Vector3, + p: Point3, ) -> bool { dq * v == dq.transform_vector(&v) && dq * p == dq.transform_point(&p) diff --git a/tests/geometry/mod.rs b/tests/geometry/mod.rs index 68d1bd6e..2363d411 100644 --- a/tests/geometry/mod.rs +++ b/tests/geometry/mod.rs @@ -1,3 +1,4 @@ +mod dual_quaternion; mod isometry; mod point; mod projection; @@ -5,4 +6,3 @@ mod quaternion; mod rotation; mod similarity; mod unit_complex; -mod dual_quaternion; From ecda74f6b2f2ec0473accd6e3723abf80245215c Mon Sep 17 00:00:00 2001 From: Terence Date: Thu, 28 Jan 2021 18:50:34 -0500 Subject: [PATCH 05/10] clippify --- src/geometry/dual_quaternion_ops.rs | 4 ++-- tests/geometry/dual_quaternion.rs | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/geometry/dual_quaternion_ops.rs b/src/geometry/dual_quaternion_ops.rs index 17644302..d6cb35b6 100644 --- a/src/geometry/dual_quaternion_ops.rs +++ b/src/geometry/dual_quaternion_ops.rs @@ -977,7 +977,7 @@ dual_quaternion_op_impl!( DivAssign, div_assign; (U4, U1), (U4, U1); self: UnitDualQuaternion, rhs: &'b UnitQuaternion; - #[allow(clippy::suspicious_arithmetic_impl)] + #[allow(clippy::suspicious_op_assign_impl)] { let res = &*self * UnitDualQuaternion::from_rotation(rhs.inverse()); self.as_mut_unchecked().real.coords.copy_from(&res.as_ref().real.coords); @@ -1013,7 +1013,7 @@ dual_quaternion_op_impl!( DivAssign, div_assign; (U4, U1), (U4, U1); self: UnitDualQuaternion, rhs: &'b Translation3; - #[allow(clippy::suspicious_arithmetic_impl)] + #[allow(clippy::suspicious_op_assign_impl)] { let res = &*self * UnitDualQuaternion::from_parts(rhs.inverse(), UnitQuaternion::identity()); self.as_mut_unchecked().real.coords.copy_from(&res.as_ref().real.coords); diff --git a/tests/geometry/dual_quaternion.rs b/tests/geometry/dual_quaternion.rs index 9cfca879..727e6aa1 100644 --- a/tests/geometry/dual_quaternion.rs +++ b/tests/geometry/dual_quaternion.rs @@ -25,7 +25,7 @@ quickcheck!( fn multiply_equals_alga_transform( dq: UnitDualQuaternion, v: Vector3, - p: Point3, + p: Point3 ) -> bool { dq * v == dq.transform_vector(&v) && dq * p == dq.transform_point(&p) From ac92e684862316e8c7df3eef7a37a8c120643ebe Mon Sep 17 00:00:00 2001 From: Terence Date: Thu, 28 Jan 2021 19:27:40 -0500 Subject: [PATCH 06/10] add new operators --- src/geometry/dual_quaternion_ops.rs | 128 ++++++++++++++++++++++ tests/geometry/dual_quaternion.rs | 159 ++++++++++++++++------------ 2 files changed, 222 insertions(+), 65 deletions(-) diff --git a/src/geometry/dual_quaternion_ops.rs b/src/geometry/dual_quaternion_ops.rs index d6cb35b6..2cf91b0f 100644 --- a/src/geometry/dual_quaternion_ops.rs +++ b/src/geometry/dual_quaternion_ops.rs @@ -242,6 +242,66 @@ dual_quaternion_op_impl!( self: DualQuaternion, rhs: DualQuaternion, Output = DualQuaternion; &self * &rhs; ); +// DualQuaternion × UnitDualQuaternion +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U4, U1); + self: &'a DualQuaternion, rhs: &'b UnitDualQuaternion, Output = DualQuaternion; + self * rhs.dual_quaternion(); + 'a, 'b); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U4, U1); + self: &'a DualQuaternion, rhs: UnitDualQuaternion, Output = DualQuaternion; + self * rhs.dual_quaternion(); + 'a); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U4, U1); + self: DualQuaternion, rhs: &'b UnitDualQuaternion, Output = DualQuaternion; + self * rhs.dual_quaternion(); + 'b); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U4, U1); + self: DualQuaternion, rhs: UnitDualQuaternion, Output = DualQuaternion; + self * rhs.dual_quaternion();); + +// DualQuaternion ÷ UnitDualQuaternion +dual_quaternion_op_impl!( + Div, div; + (U4, U1), (U4, U1); + self: &'a DualQuaternion, rhs: &'b UnitDualQuaternion, Output = DualQuaternion; + #[allow(clippy::suspicious_arithmetic_impl)] + { self * rhs.inverse().dual_quaternion() }; + 'a, 'b); + +dual_quaternion_op_impl!( + Div, div; + (U4, U1), (U4, U1); + self: &'a DualQuaternion, rhs: UnitDualQuaternion, Output = DualQuaternion; + #[allow(clippy::suspicious_arithmetic_impl)] + { self * rhs.inverse().dual_quaternion() }; + 'a); + +dual_quaternion_op_impl!( + Div, div; + (U4, U1), (U4, U1); + self: DualQuaternion, rhs: &'b UnitDualQuaternion, Output = DualQuaternion; + #[allow(clippy::suspicious_arithmetic_impl)] + { self * rhs.inverse().dual_quaternion() }; + 'b); + +dual_quaternion_op_impl!( + Div, div; + (U4, U1), (U4, U1); + self: DualQuaternion, rhs: UnitDualQuaternion, Output = DualQuaternion; + #[allow(clippy::suspicious_arithmetic_impl)] + { self * rhs.inverse().dual_quaternion() };); + // UnitDualQuaternion × UnitDualQuaternion dual_quaternion_op_impl!( Mul, mul; @@ -298,6 +358,38 @@ dual_quaternion_op_impl!( self: UnitDualQuaternion, rhs: UnitDualQuaternion, Output = UnitDualQuaternion; &self / &rhs; ); +// UnitDualQuaternion × DualQuaternion +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U4, U1); + self: &'a UnitDualQuaternion, rhs: &'b DualQuaternion, + Output = DualQuaternion => U1, U4; + self.dual_quaternion() * rhs; + 'a, 'b); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U4, U1); + self: &'a UnitDualQuaternion, rhs: DualQuaternion, + Output = DualQuaternion => U3, U3; + self.dual_quaternion() * rhs; + 'a); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U4, U1); + self: UnitDualQuaternion, rhs: &'b DualQuaternion, + Output = DualQuaternion => U3, U3; + self.dual_quaternion() * rhs; + 'b); + +dual_quaternion_op_impl!( + Mul, mul; + (U4, U1), (U4, U1); + self: UnitDualQuaternion, rhs: DualQuaternion, + Output = DualQuaternion => U3, U3; + self.dual_quaternion() * rhs;); + // UnitDualQuaternion × UnitQuaternion dual_quaternion_op_impl!( Mul, mul; @@ -917,8 +1009,44 @@ dual_quaternion_op_impl!( MulAssign, mul_assign; (U4, U1), (U4, U1); self: DualQuaternion, rhs: DualQuaternion; + *self *= &rhs;); + +// DualQuaternion ×= UnitDualQuaternion +dual_quaternion_op_impl!( + MulAssign, mul_assign; + (U4, U1), (U4, U1); + self: DualQuaternion, rhs: &'b UnitDualQuaternion; + { + let res = &*self * rhs; + self.real.coords.copy_from(&res.real.coords); + self.dual.coords.copy_from(&res.dual.coords); + }; + 'b); + +dual_quaternion_op_impl!( + MulAssign, mul_assign; + (U4, U1), (U4, U1); + self: DualQuaternion, rhs: UnitDualQuaternion; *self *= &rhs; ); +// DualQuaternion ÷= UnitDualQuaternion +dual_quaternion_op_impl!( + DivAssign, div_assign; + (U4, U1), (U4, U1); + self: DualQuaternion, rhs: &'b UnitDualQuaternion; + { + let res = &*self / rhs; + self.real.coords.copy_from(&res.real.coords); + self.dual.coords.copy_from(&res.dual.coords); + }; + 'b); + +dual_quaternion_op_impl!( + DivAssign, div_assign; + (U4, U1), (U4, U1); + self: DualQuaternion, rhs: UnitDualQuaternion; + *self /= &rhs; ); + // UnitDualQuaternion ×= UnitDualQuaternion dual_quaternion_op_impl!( MulAssign, mul_assign; diff --git a/tests/geometry/dual_quaternion.rs b/tests/geometry/dual_quaternion.rs index 727e6aa1..c3254ce6 100644 --- a/tests/geometry/dual_quaternion.rs +++ b/tests/geometry/dual_quaternion.rs @@ -1,7 +1,7 @@ #![cfg(feature = "arbitrary")] #![allow(non_snake_case)] -use na::{Isometry3, Point3, Translation3, UnitDualQuaternion, UnitQuaternion, Vector3}; +use na::{DualQuaternion, Isometry3, Point3, Translation3, UnitDualQuaternion, UnitQuaternion, Vector3}; quickcheck!( fn isometry_equivalence(iso: Isometry3, p: Point3, v: Vector3) -> bool { @@ -68,61 +68,86 @@ quickcheck!( #[cfg_attr(rustfmt, rustfmt_skip)] fn all_op_exist( - dq: UnitDualQuaternion, + dq: DualQuaternion, + udq: UnitDualQuaternion, uq: UnitQuaternion, + s: f64, t: Translation3, v: Vector3, p: Point3 ) -> bool { - let iMi = dq * dq; - let iMuq = dq * uq; - let iDi = dq / dq; - let iDuq = dq / uq; + let dqMs: DualQuaternion<_> = dq * s; - let iMp = dq * p; - let iMv = dq * v; + let dqMdq: DualQuaternion<_> = dq * dq; + let dqMudq: DualQuaternion<_> = dq * udq; + let udqMdq: DualQuaternion<_> = udq * dq; - let iMt = dq * t; - let tMi = t * dq; + let iMi: UnitDualQuaternion<_> = udq * udq; + let iMuq: UnitDualQuaternion<_> = udq * uq; + let iDi: UnitDualQuaternion<_> = udq / udq; + let iDuq: UnitDualQuaternion<_> = udq / uq; - let tMuq = t * uq; + let iMp: Point3<_> = udq * p; + let iMv: Vector3<_> = udq * v; - let uqMi = uq * dq; - let uqDi = uq / dq; + let iMt: UnitDualQuaternion<_> = udq * t; + let tMi: UnitDualQuaternion<_> = t * udq; - let uqMt = uq * t; + let uqMi: UnitDualQuaternion<_> = uq * udq; + let uqDi: UnitDualQuaternion<_> = uq / udq; - let mut iMt1 = dq; - let mut iMt2 = dq; + let mut dqMs1 = dq; - let mut iMi1 = dq; - let mut iMi2 = dq; + let mut dqMdq1 = dq; + let mut dqMdq2 = dq; - let mut iMuq1 = dq; - let mut iMuq2 = dq; + let mut dqMudq1 = dq; + let mut dqMudq2 = dq; - let mut iDi1 = dq; - let mut iDi2 = dq; + let mut iMt1 = udq; + let mut iMt2 = udq; - let mut iDuq1 = dq; - let mut iDuq2 = dq; + let mut iMi1 = udq; + let mut iMi2 = udq; + + let mut iMuq1 = udq; + let mut iMuq2 = udq; + + let mut iDi1 = udq; + let mut iDi2 = udq; + + let mut iDuq1 = udq; + let mut iDuq2 = udq; + + dqMs1 *= s; + + dqMdq1 *= dq; + dqMdq2 *= &dq; + + dqMudq1 *= udq; + dqMudq2 *= &udq; iMt1 *= t; iMt2 *= &t; - iMi1 *= dq; - iMi2 *= &dq; + iMi1 *= udq; + iMi2 *= &udq; iMuq1 *= uq; iMuq2 *= &uq; - iDi1 /= dq; - iDi2 /= &dq; + iDi1 /= udq; + iDi2 /= &udq; iDuq1 /= uq; iDuq2 /= &uq; - iMt == iMt1 + dqMs == dqMs1 + && dqMdq == dqMdq1 + && dqMdq == dqMdq2 + && dqMudq == dqMudq1 + && dqMudq == dqMudq2 + && iMt == iMt1 && iMt == iMt2 && iMi == iMi1 && iMi == iMi2 @@ -132,41 +157,45 @@ quickcheck!( && 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 + && dqMs == &dq * s + && dqMdq == &dq * &dq + && dqMdq == dq * &dq + && dqMdq == &dq * dq + && dqMudq == &dq * &udq + && dqMudq == dq * &udq + && dqMudq == &dq * udq + && udqMdq == &udq * &dq + && udqMdq == udq * &dq + && udqMdq == &udq * dq + && iMi == &udq * &udq + && iMi == udq * &udq + && iMi == &udq * udq + && iMuq == &udq * &uq + && iMuq == udq * &uq + && iMuq == &udq * uq + && iDi == &udq / &udq + && iDi == udq / &udq + && iDi == &udq / udq + && iDuq == &udq / &uq + && iDuq == udq / &uq + && iDuq == &udq / uq + && iMp == &udq * &p + && iMp == udq * &p + && iMp == &udq * p + && iMv == &udq * &v + && iMv == udq * &v + && iMv == &udq * v + && iMt == &udq * &t + && iMt == udq * &t + && iMt == &udq * t + && tMi == &t * &udq + && tMi == t * &udq + && tMi == &t * udq + && uqMi == &uq * &udq + && uqMi == uq * &udq + && uqMi == &uq * udq + && uqDi == &uq / &udq + && uqDi == uq / &udq + && uqDi == &uq / udq } ); From 8c52e4dfdcef04c164a568fc1d0b35243aeb4542 Mon Sep 17 00:00:00 2001 From: Terence Date: Thu, 28 Jan 2021 19:29:12 -0500 Subject: [PATCH 07/10] rustfmt --- tests/geometry/dual_quaternion.rs | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/tests/geometry/dual_quaternion.rs b/tests/geometry/dual_quaternion.rs index c3254ce6..2884f7f4 100644 --- a/tests/geometry/dual_quaternion.rs +++ b/tests/geometry/dual_quaternion.rs @@ -1,7 +1,9 @@ #![cfg(feature = "arbitrary")] #![allow(non_snake_case)] -use na::{DualQuaternion, Isometry3, Point3, Translation3, UnitDualQuaternion, UnitQuaternion, Vector3}; +use na::{ + DualQuaternion, Isometry3, Point3, Translation3, UnitDualQuaternion, UnitQuaternion, Vector3, +}; quickcheck!( fn isometry_equivalence(iso: Isometry3, p: Point3, v: Vector3) -> bool { @@ -22,6 +24,7 @@ quickcheck!( && relative_eq!((ii * i) * v, v, epsilon = 1.0e-7) } + #[cfg_attr(rustfmt, rustfmt_skip)] fn multiply_equals_alga_transform( dq: UnitDualQuaternion, v: Vector3, From c408e09e28471c8554a40f02583f52594c75cfa8 Mon Sep 17 00:00:00 2001 From: Terence Date: Thu, 28 Jan 2021 19:41:55 -0500 Subject: [PATCH 08/10] update header comment for operators --- src/geometry/dual_quaternion_ops.rs | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/src/geometry/dual_quaternion_ops.rs b/src/geometry/dual_quaternion_ops.rs index 2cf91b0f..a9e83764 100644 --- a/src/geometry/dual_quaternion_ops.rs +++ b/src/geometry/dual_quaternion_ops.rs @@ -10,10 +10,32 @@ * * (Assignment Operators) * + * -DualQuaternion * DualQuaternion × Scalar * DualQuaternion × DualQuaternion * DualQuaternion + DualQuaternion * DualQuaternion - DualQuaternion + * DualQuaternion × UnitDualQuaternion + * DualQuaternion ÷ UnitDualQuaternion + * -UnitDualQuaternion + * UnitDualQuaternion × DualQuaternion + * UnitDualQuaternion × UnitDualQuaternion + * UnitDualQuaternion ÷ UnitDualQuaternion + * UnitDualQuaternion × Translation3 + * UnitDualQuaternion ÷ Translation3 + * UnitDualQuaternion × UnitQuaternion + * UnitDualQuaternion ÷ UnitQuaternion + * Translation3 × UnitDualQuaternion + * Translation3 ÷ UnitDualQuaternion + * UnitQuaternion × UnitDualQuaternion + * UnitQuaternion ÷ UnitDualQuaternion + * UnitDualQuaternion × Isometry3 + * UnitDualQuaternion ÷ Isometry3 + * Isometry3 × UnitDualQuaternion + * Isometry3 ÷ UnitDualQuaternion + * UnitDualQuaternion × Point + * UnitDualQuaternion × Vector + * UnitDualQuaternion × Unit * * --- * @@ -703,7 +725,7 @@ dual_quaternion_op_impl!( Output = UnitDualQuaternion => U3, U1; self * UnitDualQuaternion::::from_isometry(&rhs); ); -// UnitDualQuaternion ÷ Isometry +// UnitDualQuaternion ÷ Isometry3 dual_quaternion_op_impl!( Div, div; (U4, U1), (U3, U1); From bf0f3163cecad4befcce08ea5e6e2e97e940111d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Thu, 25 Feb 2021 14:45:26 +0100 Subject: [PATCH 09/10] Rename some of the variables in dual-quaternion doc-tests. --- src/geometry/dual_quaternion.rs | 26 +++++++++++++------------- src/geometry/dual_quaternion_ops.rs | 2 +- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/src/geometry/dual_quaternion.rs b/src/geometry/dual_quaternion.rs index dac11725..cce0da2a 100644 --- a/src/geometry/dual_quaternion.rs +++ b/src/geometry/dual_quaternion.rs @@ -316,7 +316,7 @@ impl> UlpsEq for DualQuaternion { } } -/// A unit quaternions. May be used to represent a rotation. +/// A unit quaternions. May be used to represent a rotation followed by a translation. pub type UnitDualQuaternion = Unit>; impl PartialEq for UnitDualQuaternion { @@ -471,10 +471,10 @@ where /// # use nalgebra::{UnitDualQuaternion, DualQuaternion, Quaternion}; /// let qr = Quaternion::new(1.0, 2.0, 3.0, 4.0); /// let qd = Quaternion::new(5.0, 6.0, 7.0, 8.0); - /// let iso1 = UnitDualQuaternion::new_normalize(DualQuaternion::from_real_and_dual(qr, qd)); - /// let iso2 = UnitDualQuaternion::new_normalize(DualQuaternion::from_real_and_dual(qd, qr)); - /// let iso_to = iso1.isometry_to(&iso2); - /// assert_relative_eq!(iso_to * iso1, iso2, epsilon = 1.0e-6); + /// let dq1 = UnitDualQuaternion::new_normalize(DualQuaternion::from_real_and_dual(qr, qd)); + /// let dq2 = UnitDualQuaternion::new_normalize(DualQuaternion::from_real_and_dual(qd, qr)); + /// let dq_to = dq1.isometry_to(&dq2); + /// assert_relative_eq!(dq_to * dq1, dq2, epsilon = 1.0e-6); /// ``` #[inline] pub fn isometry_to(&self, other: &Self) -> Self { @@ -545,7 +545,7 @@ where } /// Screw linear interpolation between two unit quaternions. This creates a - /// smooth arc from one isometry to another. + /// 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. @@ -754,9 +754,9 @@ where } /// Rotate and translate a point by the inverse of this unit quaternion. - /// This may be - /// cheaper than inverting the unit dual quaternion and transforming the - /// point. + /// + /// This may be cheaper than inverting the unit dual quaternion and + /// transforming the point. /// /// ``` /// # #[macro_use] extern crate approx; @@ -777,10 +777,10 @@ where } /// Rotate a vector by the inverse of this unit quaternion, ignoring the - /// translational component - /// This may be - /// cheaper than inverting the unit dual quaternion and transforming the - /// vector. + /// translational component. + /// + /// This may be cheaper than inverting the unit dual quaternion and + /// transforming the vector. /// /// ``` /// # #[macro_use] extern crate approx; diff --git a/src/geometry/dual_quaternion_ops.rs b/src/geometry/dual_quaternion_ops.rs index a9e83764..44d36b97 100644 --- a/src/geometry/dual_quaternion_ops.rs +++ b/src/geometry/dual_quaternion_ops.rs @@ -796,7 +796,7 @@ dual_quaternion_op_impl!( (U3, U1), (U4, U1); self: &'a Isometry3, rhs: &'b UnitDualQuaternion, Output = UnitDualQuaternion => U3, U1; - // TODO: can we avoid the conversion from a rotation matrix? + // TODO: can we avoid the conversion from a rotation matrix? UnitDualQuaternion::::from_isometry(self) / rhs; 'a, 'b); From a32f41bd410a214a022d76adc60d09f67e52ad75 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Thu, 25 Feb 2021 15:01:53 +0100 Subject: [PATCH 10/10] Fix compilation when targetting no-std. --- src/geometry/dual_quaternion.rs | 15 ++++++++++++--- src/geometry/dual_quaternion_construction.rs | 9 ++++++--- 2 files changed, 18 insertions(+), 6 deletions(-) diff --git a/src/geometry/dual_quaternion.rs b/src/geometry/dual_quaternion.rs index cce0da2a..b11e7364 100644 --- a/src/geometry/dual_quaternion.rs +++ b/src/geometry/dual_quaternion.rs @@ -1,6 +1,6 @@ use crate::{ Isometry3, Matrix4, Normed, Point3, Quaternion, Scalar, SimdRealField, Translation3, Unit, - UnitQuaternion, Vector3, VectorN, U8, + UnitQuaternion, Vector3, VectorN, Zero, U8, }; use approx::{AbsDiffEq, RelativeEq, UlpsEq}; #[cfg(feature = "serde-serialize")] @@ -35,14 +35,23 @@ use simba::scalar::{ClosedNeg, RealField}; /// If a feature that you need is missing, feel free to open an issue or a PR. /// See https://github.com/dimforge/nalgebra/issues/487 #[repr(C)] -#[derive(Debug, Default, Eq, PartialEq, Copy, Clone)] -pub struct DualQuaternion { +#[derive(Debug, Eq, PartialEq, Copy, Clone)] +pub struct DualQuaternion { /// The real component of the quaternion pub real: Quaternion, /// The dual component of the quaternion pub dual: Quaternion, } +impl Default for DualQuaternion { + fn default() -> Self { + Self { + real: Quaternion::default(), + dual: Quaternion::default(), + } + } +} + impl DualQuaternion where N::Element: SimdRealField, diff --git a/src/geometry/dual_quaternion_construction.rs b/src/geometry/dual_quaternion_construction.rs index 95c208ce..b0ae8036 100644 --- a/src/geometry/dual_quaternion_construction.rs +++ b/src/geometry/dual_quaternion_construction.rs @@ -1,12 +1,12 @@ use crate::{ - DualQuaternion, Isometry3, Quaternion, SimdRealField, Translation3, UnitDualQuaternion, + DualQuaternion, Isometry3, Quaternion, Scalar, SimdRealField, Translation3, UnitDualQuaternion, UnitQuaternion, }; use num::{One, Zero}; #[cfg(feature = "arbitrary")] use quickcheck::{Arbitrary, Gen}; -impl DualQuaternion { +impl DualQuaternion { /// Creates a dual quaternion from its rotation and translation components. /// /// # Example @@ -40,7 +40,10 @@ impl DualQuaternion { /// assert_eq!(dq2 * dq1, dq2); /// ``` #[inline] - pub fn identity() -> Self { + pub fn identity() -> Self + where + N: SimdRealField, + { Self::from_real_and_dual( Quaternion::from_real(N::one()), Quaternion::from_real(N::zero()),