Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
This commit introduces the `DualQuaternion` type, in line with the plan laid out in [#487]. [#487]: #487
- Loading branch information
Showing
6 changed files
with
268 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,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<N: SimdRealField> { | ||
// [real(w, i, j, k), dual(w, i, j, k)] | ||
pub(crate) dq: [N; 8], | ||
} | ||
|
||
impl<N: SimdRealField> DualQuaternion<N> { | ||
/// 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<N> { | ||
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<N> { | ||
Quaternion::new(self[4], self[5], self[6], self[7]) | ||
} | ||
} | ||
|
||
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,49 @@ | ||
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 { | ||
dq: [ | ||
real.w, real.i, real.j, real.k, dual.w, dual.i, dual.j, dual.k, | ||
], | ||
} | ||
} | ||
} | ||
|
||
impl<N: SimdRealField> DualQuaternion<N> { | ||
/// 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,95 @@ | ||
/* | ||
* 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::base::allocator::Allocator; | ||
use crate::{DefaultAllocator, DualQuaternion, SimdRealField, U1, U4}; | ||
use simba::simd::SimdValue; | ||
use std::ops::{Add, Index, IndexMut, Mul, Sub}; | ||
|
||
impl<N: SimdRealField> Index<usize> for DualQuaternion<N> { | ||
type Output = N; | ||
|
||
#[inline] | ||
fn index(&self, i: usize) -> &Self::Output { | ||
&self.dq[i] | ||
} | ||
} | ||
|
||
impl<N: SimdRealField> IndexMut<usize> for DualQuaternion<N> { | ||
#[inline] | ||
fn index_mut(&mut self, i: usize) -> &mut N { | ||
&mut self.dq[i] | ||
} | ||
} | ||
|
||
impl<N: SimdRealField> Mul<DualQuaternion<N>> for DualQuaternion<N> | ||
where | ||
N::Element: SimdRealField, | ||
DefaultAllocator: Allocator<N, U4, U1> + Allocator<N, U4, U1>, | ||
{ | ||
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 + SimdValue, | ||
DefaultAllocator: Allocator<N, U4, U1> + Allocator<N, U4, U1>, | ||
{ | ||
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, | ||
DefaultAllocator: Allocator<N, U4, U1> + Allocator<N, U4, U1>, | ||
{ | ||
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, | ||
DefaultAllocator: Allocator<N, U4, U1> + Allocator<N, U4, U1>, | ||
{ | ||
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