New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Introduce DualQuaternion type #810
Merged
Merged
Changes from 1 commit
Commits
Show all changes
3 commits
Select commit
Hold shift + click to select a range
File filter
Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
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 { | ||
chinedufn marked this conversation as resolved.
Show resolved
Hide resolved
|
||
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> { | ||
chinedufn marked this conversation as resolved.
Show resolved
Hide resolved
|
||
/// 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>, | ||
chinedufn marked this conversation as resolved.
Show resolved
Hide resolved
|
||
{ | ||
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>, | ||
chinedufn marked this conversation as resolved.
Show resolved
Hide resolved
|
||
{ | ||
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>, | ||
chinedufn marked this conversation as resolved.
Show resolved
Hide resolved
|
||
{ | ||
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>, | ||
chinedufn marked this conversation as resolved.
Show resolved
Hide resolved
|
||
{ | ||
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
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Changed to store internally as an array (instead of storing real and dual part separately) so that we don't need an
if
statement when indexing.There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I think what you did before was better since it made it more explicit that it is composed of two quaternions. And these two quaternions could be public fields for easier access.
Regarding indexing, you can simply:
#[repr(C)]
toDualQuaternion
.AsRef
andAsMut
. Something like:and use these in the implementation of
Index
andIndexMut
.There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Ah I like your suggestion better, thanks will change.
Question though - why are quaternions stored internally at [i,j,k,w]? I would've expected [w,i,j,k] given that
Quaternion::new(w,i,j,k)
.No big deal, just curious.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
By similarity with the Eigen library, mostly.