From 10da458ec1c3d86f4604614af0179b7d2754cd77 Mon Sep 17 00:00:00 2001
From: Chinedu Francis Nwafili <frankie.nwafili@gmail.com>
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              | 48 +++++++++++++++++++
 src/geometry/dual_quaternion_construction.rs | 50 ++++++++++++++++++++
 src/geometry/dual_quaternion_ops.rs          | 29 ++++++++++++
 src/geometry/mod.rs                          |  6 +++
 4 files changed, 133 insertions(+)
 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..dc6a4e5c1
--- /dev/null
+++ b/src/geometry/dual_quaternion.rs
@@ -0,0 +1,48 @@
+
+use crate::{Quaternion, Scalar, RealField, 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<N: Scalar + SimdValue> {
+    /// The rotation part of the dual quaternion.
+    pub rot: Quaternion<N>,
+    /// The translation part of the dual quaternion.
+    pub trans: Quaternion<N>
+}
+
+impl<N: RealField> Default for DualQuaternion<N> {
+    fn default() -> Self {
+        Self::identity()
+    }
+}
+
+impl<N: SimdRealField + Eq> Eq for DualQuaternion<N> where N::Element: SimdRealField {}
+
+impl<N: SimdRealField> PartialEq for DualQuaternion<N>
+    where
+        N::Element: SimdRealField,
+{
+    fn eq(&self, rhs: &Self) -> bool {
+        self.rot  == rhs.rot && self.trans == rhs.trans
+    }
+}
+
+
+impl<N: Scalar + Copy + SimdValue> Copy for DualQuaternion<N> {}
+
+impl<N: Scalar + SimdValue> Clone for DualQuaternion<N> {
+    #[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..97a02c220
--- /dev/null
+++ b/src/geometry/dual_quaternion_construction.rs
@@ -0,0 +1,50 @@
+use crate::{SimdRealField, DualQuaternion, Quaternion, Scalar};
+use simba::simd::SimdValue;
+
+impl<N: Scalar + SimdValue> DualQuaternion<N> {
+    /// 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<N>, trans: Quaternion<N>) -> Self {
+        Self {
+            rot,
+            trans,
+        }
+    }
+}
+
+impl<N: SimdRealField> DualQuaternion<N> {
+    /// 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..040da165c
--- /dev/null
+++ b/src/geometry/dual_quaternion_ops.rs
@@ -0,0 +1,29 @@
+/*
+ * 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 std::ops::{Mul};
+use crate::{DualQuaternion, Scalar, DefaultAllocator, U4, U1, SimdRealField};
+use simba::simd::SimdValue;
+use crate::base::allocator::Allocator;
+
+impl <N: Scalar + SimdValue + SimdRealField> Mul<DualQuaternion<N>> for DualQuaternion<N>
+    where N::Element: Scalar + SimdValue + SimdRealField,
+          DefaultAllocator: Allocator<N, U4, U1> + Allocator<N, U4, U1>
+{
+    type Output = DualQuaternion<N>;
+
+    fn mul(self, rhs: Self) -> Self::Output {
+        Self {
+            rot: self.rot * rhs.rot,
+            trans: self.rot * rhs.trans + rhs.rot * self.trans
+        }
+    }
+}
\ No newline at end of file
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::*;