From 8036c56fdafcbfc2e2e4e64ba7a23f2944cdb0a2 Mon Sep 17 00:00:00 2001 From: Chinedu Francis Nwafili Date: Wed, 16 Dec 2020 09:02:02 -0500 Subject: [PATCH] Introduce DualQuaternion type This commit introduces the `DualQuaternion` type, in line with the plan laid out in [#487]. [#487]: https://github.com/dimforge/nalgebra/issues/487 --- src/geometry/dual_quaternion.rs | 116 +++++++++++++++++++ src/geometry/dual_quaternion_construction.rs | 49 ++++++++ src/geometry/dual_quaternion_ops.rs | 95 +++++++++++++++ src/geometry/mod.rs | 6 + src/geometry/quaternion.rs | 4 +- src/geometry/quaternion_construction.rs | 2 +- 6 files changed, 268 insertions(+), 4 deletions(-) create mode 100644 src/geometry/dual_quaternion.rs create mode 100644 src/geometry/dual_quaternion_construction.rs create mode 100644 src/geometry/dual_quaternion_ops.rs diff --git a/src/geometry/dual_quaternion.rs b/src/geometry/dual_quaternion.rs new file mode 100644 index 000000000..9c5ad27c5 --- /dev/null +++ b/src/geometry/dual_quaternion.rs @@ -0,0 +1,116 @@ +use crate::{Quaternion, SimdRealField}; + +/// A dual quaternion. +/// +/// # Indexing +/// +/// DualQuaternions are stored as \[..real, ..dual\]. +/// Both of the quaternion components are laid out in `w, i, j, k` order. +/// +/// ``` +/// # 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); +/// assert_eq!(dq[0], 1.0); +/// assert_eq!(dq[4], 5.0); +/// assert_eq!(dq[6], 7.0); +/// ``` +/// +/// NOTE: +/// As of December 2020, dual quaternion support is a work in progress. +/// 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)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct DualQuaternion { + // [real(w, i, j, k), dual(w, i, j, k)] + pub(crate) dq: [N; 8], +} + +impl DualQuaternion { + /// Get the first quaternion component. + /// + /// # 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); + /// relative_eq!(dq.real(), real); + /// ``` + #[inline] + pub fn real(&self) -> Quaternion { + Quaternion::new(self[0], self[1], self[2], self[3]) + } + + /// Get the second quaternion component. + /// + /// # 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); + /// relative_eq!(dq.dual(), dual); + /// ``` + #[inline] + pub fn dual(&self) -> Quaternion { + Quaternion::new(self[4], self[5], self[6], self[7]) + } +} + +impl DualQuaternion +where + N::Element: SimdRealField, +{ + /// Normalizes this quaternion. + /// + /// # 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 dq_normalized = dq.normalize(); + /// + /// relative_eq!(dq_normalized.real().norm(), 1.0); + /// ``` + #[inline] + #[must_use = "Did you mean to use normalize_mut()?"] + pub fn normalize(&self) -> Self { + let real_norm = self.real().norm(); + + Self::from_real_and_dual(self.real() / real_norm, self.dual() / real_norm) + } + + /// Normalizes this quaternion. + /// + /// # 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 mut dq = DualQuaternion::from_real_and_dual(real, dual); + /// + /// dq.normalize_mut(); + /// + /// relative_eq!(dq.real().norm(), 1.0); + /// ``` + #[inline] + pub fn normalize_mut(&mut self) { + *self = self.normalize(); + } +} diff --git a/src/geometry/dual_quaternion_construction.rs b/src/geometry/dual_quaternion_construction.rs new file mode 100644 index 000000000..beb398ad3 --- /dev/null +++ b/src/geometry/dual_quaternion_construction.rs @@ -0,0 +1,49 @@ +use crate::{DualQuaternion, Quaternion, SimdRealField}; + +impl DualQuaternion { + /// Creates a dual quaternion from its rotation and translation components. + /// + /// # Example + /// ``` + /// # use nalgebra::{DualQuaternion, Quaternion}; + /// let rot = Quaternion::new(1.0, 2.0, 3.0, 4.0); + /// let trans = Quaternion::new(5.0, 6.0, 7.0, 8.0); + /// + /// let dq = DualQuaternion::from_real_and_dual(rot, trans); + /// assert_eq!(dq.real().w, 1.0); + /// ``` + #[inline] + pub fn from_real_and_dual(real: Quaternion, dual: Quaternion) -> Self { + Self { + dq: [ + real.w, real.i, real.j, real.k, dual.w, dual.i, dual.j, dual.k, + ], + } + } +} + +impl DualQuaternion { + /// The dual quaternion multiplicative identity + /// + /// # Example + /// + /// ``` + /// # use nalgebra::{DualQuaternion, Quaternion}; + /// + /// let dq1 = DualQuaternion::identity(); + /// let dq2 = DualQuaternion::from_real_and_dual( + /// Quaternion::new(1.,2.,3.,4.), + /// Quaternion::new(5.,6.,7.,8.) + /// ); + /// + /// assert_eq!(dq1 * dq2, dq2); + /// assert_eq!(dq2 * dq1, dq2); + /// ``` + #[inline] + pub fn identity() -> Self { + Self::from_real_and_dual( + Quaternion::from_real(N::one()), + Quaternion::from_real(N::zero()), + ) + } +} diff --git a/src/geometry/dual_quaternion_ops.rs b/src/geometry/dual_quaternion_ops.rs new file mode 100644 index 000000000..dc46faf22 --- /dev/null +++ b/src/geometry/dual_quaternion_ops.rs @@ -0,0 +1,95 @@ +/* + * This file provides: + * + * NOTE: Work in progress https://github.com/dimforge/nalgebra/issues/487 + * + * (Dual Quaternion) + * + * Index + * IndexMut + * + * (Assignment Operators) + * + * DualQuaternion × Scalar + * DualQuaternion × DualQuaternion + * DualQuaternion + DualQuaternion + * DualQuaternion - DualQuaternion + * + * --- + * + * References: + * Multiplication: + * - https://cs.gmu.edu/~jmlien/teaching/cs451/uploads/Main/dual-quaternion.pdf + */ + +use crate::base::allocator::Allocator; +use crate::{DefaultAllocator, DualQuaternion, SimdRealField, U1, U4}; +use simba::simd::SimdValue; +use std::ops::{Add, Index, IndexMut, Mul, Sub}; + +impl Index for DualQuaternion { + type Output = N; + + #[inline] + fn index(&self, i: usize) -> &Self::Output { + &self.dq[i] + } +} + +impl IndexMut for DualQuaternion { + #[inline] + fn index_mut(&mut self, i: usize) -> &mut N { + &mut self.dq[i] + } +} + +impl Mul> for DualQuaternion +where + N::Element: SimdRealField, + DefaultAllocator: Allocator + Allocator, +{ + 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(), + ) + } +} + +impl Mul for DualQuaternion +where + N::Element: SimdRealField + SimdValue, + DefaultAllocator: Allocator + Allocator, +{ + type Output = DualQuaternion; + + fn mul(self, rhs: N) -> Self::Output { + Self::from_real_and_dual(self.real() * rhs, self.dual() * rhs) + } +} + +impl Add> for DualQuaternion +where + N::Element: SimdRealField, + DefaultAllocator: Allocator + Allocator, +{ + type Output = DualQuaternion; + + fn add(self, rhs: DualQuaternion) -> Self::Output { + Self::from_real_and_dual(self.real() + rhs.real(), self.dual() + rhs.dual()) + } +} + +impl Sub> for DualQuaternion +where + N::Element: SimdRealField, + DefaultAllocator: Allocator + Allocator, +{ + type Output = DualQuaternion; + + fn sub(self, rhs: DualQuaternion) -> Self::Output { + Self::from_real_and_dual(self.real() - rhs.real(), self.dual() - rhs.dual()) + } +} diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index d3e2236ae..19313b65c 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -35,6 +35,10 @@ mod quaternion_coordinates; mod quaternion_ops; mod quaternion_simba; +mod dual_quaternion; +mod dual_quaternion_construction; +mod dual_quaternion_ops; + mod unit_complex; #[cfg(feature = "alga")] mod unit_complex_alga; @@ -98,6 +102,8 @@ pub use self::rotation_alias::*; pub use self::quaternion::*; +pub use self::dual_quaternion::*; + pub use self::unit_complex::*; pub use self::translation::*; diff --git a/src/geometry/quaternion.rs b/src/geometry/quaternion.rs index c8fd80bed..a5db1c697 100755 --- a/src/geometry/quaternion.rs +++ b/src/geometry/quaternion.rs @@ -1,7 +1,6 @@ use approx::{AbsDiffEq, RelativeEq, UlpsEq}; use num::Zero; use std::fmt; -use std::hash; #[cfg(feature = "abomonation-serialize")] use std::io::{Result as IOResult, Write}; @@ -14,7 +13,7 @@ use serde::{Deserialize, Deserializer, Serialize, Serializer}; use abomonation::Abomonation; use simba::scalar::{ClosedNeg, RealField}; -use simba::simd::{SimdBool, SimdOption, SimdRealField, SimdValue}; +use simba::simd::{SimdBool, SimdOption, SimdRealField}; use crate::base::dimension::{U1, U3, U4}; use crate::base::storage::{CStride, RStride}; @@ -23,7 +22,6 @@ use crate::base::{ }; use crate::geometry::{Point3, Rotation}; -use std::ops::Neg; /// A quaternion. See the type alias `UnitQuaternion = Unit` for a quaternion /// that may be used as a rotation. diff --git a/src/geometry/quaternion_construction.rs b/src/geometry/quaternion_construction.rs index 7d0cca8cf..03d87d391 100644 --- a/src/geometry/quaternion_construction.rs +++ b/src/geometry/quaternion_construction.rs @@ -10,7 +10,7 @@ use rand::distributions::{Distribution, OpenClosed01, Standard}; use rand::Rng; use simba::scalar::RealField; -use simba::simd::{SimdBool, SimdValue}; +use simba::simd::SimdBool; use crate::base::dimension::U3; use crate::base::storage::Storage;