From 12c259f0b4d7c2e885586891240642b0958e9c01 Mon Sep 17 00:00:00 2001 From: Terence Date: Thu, 28 Jan 2021 17:25:32 -0500 Subject: [PATCH] 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,