Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #810 from chinedufn/dual-quaternion
Introduce DualQuaternion type
- Loading branch information
Showing
6 changed files
with
236 additions
and
4 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,82 @@ | ||
use crate::{Quaternion, SimdRealField}; | ||
|
||
/// A dual quaternion. | ||
/// | ||
/// # Indexing | ||
/// | ||
/// DualQuaternions are stored as \[..real, ..dual\]. | ||
/// Both of the quaternion components are laid out in `i, j, k, w` 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], 2.0); | ||
/// assert_eq!(dq[1], 3.0); | ||
/// | ||
/// assert_eq!(dq[4], 6.0); | ||
/// assert_eq!(dq[7], 5.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<N: SimdRealField> { | ||
/// The real component of the quaternion | ||
pub real: Quaternion<N>, | ||
/// The dual component of the quaternion | ||
pub dual: Quaternion<N>, | ||
} | ||
|
||
impl<N: SimdRealField> DualQuaternion<N> | ||
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(); | ||
} | ||
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,42 @@ | ||
use crate::{DualQuaternion, Quaternion, SimdRealField}; | ||
|
||
impl<N: SimdRealField> 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::from_real_and_dual(rot, trans); | ||
/// assert_eq!(dq.real.w, 1.0); | ||
/// ``` | ||
#[inline] | ||
pub fn from_real_and_dual(real: Quaternion<N>, dual: Quaternion<N>) -> Self { | ||
Self { real, dual } | ||
} | ||
/// 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()), | ||
) | ||
} | ||
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,104 @@ | ||
/* | ||
* This file provides: | ||
* | ||
* NOTE: Work in progress https://github.com/dimforge/nalgebra/issues/487 | ||
* | ||
* (Dual Quaternion) | ||
* | ||
* Index<usize> | ||
* IndexMut<usize> | ||
* | ||
* (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::{DualQuaternion, SimdRealField}; | ||
use std::mem; | ||
use std::ops::{Add, Index, IndexMut, Mul, Sub}; | ||
|
||
impl<N: SimdRealField> AsRef<[N; 8]> for DualQuaternion<N> { | ||
#[inline] | ||
fn as_ref(&self) -> &[N; 8] { | ||
unsafe { mem::transmute(self) } | ||
} | ||
} | ||
|
||
impl<N: SimdRealField> AsMut<[N; 8]> for DualQuaternion<N> { | ||
#[inline] | ||
fn as_mut(&mut self) -> &mut [N; 8] { | ||
unsafe { mem::transmute(self) } | ||
} | ||
} | ||
|
||
impl<N: SimdRealField> Index<usize> for DualQuaternion<N> { | ||
type Output = N; | ||
|
||
#[inline] | ||
fn index(&self, i: usize) -> &Self::Output { | ||
&self.as_ref()[i] | ||
} | ||
} | ||
|
||
impl<N: SimdRealField> IndexMut<usize> for DualQuaternion<N> { | ||
#[inline] | ||
fn index_mut(&mut self, i: usize) -> &mut N { | ||
&mut self.as_mut()[i] | ||
} | ||
} | ||
|
||
impl<N: SimdRealField> Mul<DualQuaternion<N>> for DualQuaternion<N> | ||
where | ||
N::Element: SimdRealField, | ||
{ | ||
type Output = DualQuaternion<N>; | ||
|
||
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<N: SimdRealField> Mul<N> for DualQuaternion<N> | ||
where | ||
N::Element: SimdRealField, | ||
{ | ||
type Output = DualQuaternion<N>; | ||
|
||
fn mul(self, rhs: N) -> Self::Output { | ||
Self::from_real_and_dual(self.real * rhs, self.dual * rhs) | ||
} | ||
} | ||
|
||
impl<N: SimdRealField> Add<DualQuaternion<N>> for DualQuaternion<N> | ||
where | ||
N::Element: SimdRealField, | ||
{ | ||
type Output = DualQuaternion<N>; | ||
|
||
fn add(self, rhs: DualQuaternion<N>) -> Self::Output { | ||
Self::from_real_and_dual(self.real + rhs.real, self.dual + rhs.dual) | ||
} | ||
} | ||
|
||
impl<N: SimdRealField> Sub<DualQuaternion<N>> for DualQuaternion<N> | ||
where | ||
N::Element: SimdRealField, | ||
{ | ||
type Output = DualQuaternion<N>; | ||
|
||
fn sub(self, rhs: DualQuaternion<N>) -> Self::Output { | ||
Self::from_real_and_dual(self.real - rhs.real, self.dual - rhs.dual) | ||
} | ||
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters