diff --git a/src/geometry/dual_quaternion.rs b/src/geometry/dual_quaternion.rs new file mode 100644 index 000000000..7098e101f --- /dev/null +++ b/src/geometry/dual_quaternion.rs @@ -0,0 +1,46 @@ +use crate::{Quaternion, RealField, Scalar, SimdRealField}; +use simba::simd::SimdValue; + +/// A dual quaternion. +/// +/// 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)] +pub struct DualQuaternion { + /// The rotation part of the dual quaternion. + pub rot: Quaternion, + /// The translation part of the dual quaternion. + pub trans: Quaternion, +} + +impl Default for DualQuaternion { + fn default() -> Self { + Self::identity() + } +} + +impl Eq for DualQuaternion where N::Element: SimdRealField {} + +impl PartialEq for DualQuaternion +where + N::Element: SimdRealField, +{ + fn eq(&self, rhs: &Self) -> bool { + self.rot == rhs.rot && self.trans == rhs.trans + } +} + +impl Copy for DualQuaternion {} + +impl Clone for DualQuaternion { + #[inline] + fn clone(&self) -> Self { + Self { + rot: self.rot.clone(), + trans: self.trans.clone(), + } + } +} diff --git a/src/geometry/dual_quaternion_construction.rs b/src/geometry/dual_quaternion_construction.rs new file mode 100644 index 000000000..b630ccfb5 --- /dev/null +++ b/src/geometry/dual_quaternion_construction.rs @@ -0,0 +1,46 @@ +use crate::{DualQuaternion, Quaternion, Scalar, SimdRealField}; +use simba::simd::SimdValue; + +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::new(rot, trans); + /// assert_eq!(dq.rot.w, 1.0); + /// ``` + #[inline] + pub fn new(rot: Quaternion, trans: Quaternion) -> Self { + Self { rot, trans } + } +} + +impl DualQuaternion { + /// The dual quaternion multiplicative identity + /// + /// # Example + /// + /// ``` + /// # use nalgebra::{DualQuaternion, Quaternion}; + /// + /// let dq1 = DualQuaternion::identity(); + /// let dq2 = DualQuaternion::new( + /// 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 { + rot: Quaternion::from_real(N::one()), + trans: 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..bb1cfb3e1 --- /dev/null +++ b/src/geometry/dual_quaternion_ops.rs @@ -0,0 +1,30 @@ +/* + * This file provides: + * NOTE: Work in progress https://github.com/dimforge/nalgebra/issues/487 + * + * References: + * Multiplication: + * - https://cs.gmu.edu/~jmlien/teaching/cs451/uploads/Main/dual-quaternion.pdf + * =================== + * + */ + +use crate::base::allocator::Allocator; +use crate::{DefaultAllocator, DualQuaternion, Scalar, SimdRealField, U1, U4}; +use simba::simd::SimdValue; +use std::ops::Mul; + +impl Mul> for DualQuaternion +where + N::Element: Scalar + SimdValue + SimdRealField, + DefaultAllocator: Allocator + Allocator, +{ + type Output = DualQuaternion; + + fn mul(self, rhs: Self) -> Self::Output { + Self { + rot: self.rot * rhs.rot, + trans: self.rot * rhs.trans + rhs.rot * self.trans, + } + } +} 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::*;