diff --git a/src/geometry/dual_quaternion.rs b/src/geometry/dual_quaternion.rs index ce9f7284..b11e7364 100644 --- a/src/geometry/dual_quaternion.rs +++ b/src/geometry/dual_quaternion.rs @@ -1,6 +1,13 @@ -use crate::{Quaternion, SimdRealField}; +use crate::{ + Isometry3, Matrix4, Normed, Point3, Quaternion, Scalar, SimdRealField, Translation3, Unit, + UnitQuaternion, Vector3, VectorN, Zero, U8, +}; +use approx::{AbsDiffEq, RelativeEq, UlpsEq}; #[cfg(feature = "serde-serialize")] use serde::{Deserialize, Deserializer, Serialize, Serializer}; +use std::fmt; + +use simba::scalar::{ClosedNeg, RealField}; /// A dual quaternion. /// @@ -28,14 +35,23 @@ use serde::{Deserialize, Deserializer, Serialize, Serializer}; /// 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, @@ -77,8 +93,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 +269,669 @@ 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 followed by a translation. +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 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 { + 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 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. + /// + /// # 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..f6ee9e10 --- /dev/null +++ b/src/geometry/dual_quaternion_alga.rs @@ -0,0 +1,324 @@ +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::{ + DualQuaternion, Point3, Quaternion, Translation3, UnitDualQuaternion, UnitQuaternion, +}; + +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..b0ae8036 100644 --- a/src/geometry/dual_quaternion_construction.rs +++ b/src/geometry/dual_quaternion_construction.rs @@ -1,6 +1,12 @@ -use crate::{DualQuaternion, Quaternion, SimdRealField}; +use crate::{ + 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 @@ -16,7 +22,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 /// @@ -33,10 +40,183 @@ 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()), ) } } + +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() + } +} + +#[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. + /// + /// ``` + /// # 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() + } +} + +#[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_conversion.rs b/src/geometry/dual_quaternion_conversion.rs new file mode 100644 index 00000000..20c6895a --- /dev/null +++ b/src/geometry/dual_quaternion_conversion.rs @@ -0,0 +1,188 @@ +use simba::scalar::{RealField, SubsetOf, SupersetOf}; +use simba::simd::SimdRealField; + +use crate::base::dimension::U3; +use crate::base::{Matrix4, Vector4}; +use crate::geometry::{ + DualQuaternion, Isometry3, Similarity3, SuperTCategoryOf, TAffine, Transform, Translation3, + UnitDualQuaternion, UnitQuaternion, +}; + +/* + * 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..44d36b97 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 * * --- * @@ -22,9 +44,16 @@ * - https://cs.gmu.edu/~jmlien/teaching/cs451/uploads/Main/dual-quaternion.pdf */ -use crate::{DualQuaternion, SimdRealField}; +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, 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 +85,1175 @@ 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; ); + +// 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; + (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 × 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; + (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 ÷ 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; + (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 ÷ Isometry3 +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;); + +// 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; + (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 ×= 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_op_assign_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_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); + 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; + (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..0c89958a 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..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, + TAffine, Transform, Translation, UnitDualQuaternion, UnitQuaternion, }; /* @@ -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..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, + TAffine, Transform, Translation, UnitComplex, UnitDualQuaternion, UnitQuaternion, }; /* @@ -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..9e915073 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, diff --git a/tests/geometry/dual_quaternion.rs b/tests/geometry/dual_quaternion.rs new file mode 100644 index 00000000..2884f7f4 --- /dev/null +++ b/tests/geometry/dual_quaternion.rs @@ -0,0 +1,204 @@ +#![cfg(feature = "arbitrary")] +#![allow(non_snake_case)] + +use na::{ + DualQuaternion, Isometry3, Point3, Translation3, UnitDualQuaternion, UnitQuaternion, 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) + } + + #[cfg_attr(rustfmt, rustfmt_skip)] + 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: DualQuaternion, + udq: UnitDualQuaternion, + uq: UnitQuaternion, + s: f64, + t: Translation3, + v: Vector3, + p: Point3 + ) -> bool { + let dqMs: DualQuaternion<_> = dq * s; + + let dqMdq: DualQuaternion<_> = dq * dq; + let dqMudq: DualQuaternion<_> = dq * udq; + let udqMdq: DualQuaternion<_> = udq * dq; + + let iMi: UnitDualQuaternion<_> = udq * udq; + let iMuq: UnitDualQuaternion<_> = udq * uq; + let iDi: UnitDualQuaternion<_> = udq / udq; + let iDuq: UnitDualQuaternion<_> = udq / uq; + + let iMp: Point3<_> = udq * p; + let iMv: Vector3<_> = udq * v; + + let iMt: UnitDualQuaternion<_> = udq * t; + let tMi: UnitDualQuaternion<_> = t * udq; + + let uqMi: UnitDualQuaternion<_> = uq * udq; + let uqDi: UnitDualQuaternion<_> = uq / udq; + + let mut dqMs1 = dq; + + let mut dqMdq1 = dq; + let mut dqMdq2 = dq; + + let mut dqMudq1 = dq; + let mut dqMudq2 = dq; + + let mut iMt1 = udq; + let mut iMt2 = udq; + + 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 *= udq; + iMi2 *= &udq; + + iMuq1 *= uq; + iMuq2 *= &uq; + + iDi1 /= udq; + iDi2 /= &udq; + + iDuq1 /= uq; + iDuq2 /= &uq; + + dqMs == dqMs1 + && dqMdq == dqMdq1 + && dqMdq == dqMdq2 + && dqMudq == dqMudq1 + && dqMudq == dqMudq2 + && iMt == iMt1 + && iMt == iMt2 + && iMi == iMi1 + && iMi == iMi2 + && iMuq == iMuq1 + && iMuq == iMuq2 + && iDi == iDi1 + && iDi == iDi2 + && iDuq == iDuq1 + && iDuq == iDuq2 + && 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 + } +); diff --git a/tests/geometry/mod.rs b/tests/geometry/mod.rs index ec9755a0..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;