M-Labs
/
nalgebra
3
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Store DQ as real and dual Quat

This commit is contained in:
Chinedu Francis Nwafili 2020-12-18 10:09:56 -05:00
parent 8036c56fda
commit 89134efc3b
No known key found for this signature in database
GPG Key ID: 3C85D8F6538D8AD9
3 changed files with 38 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
src/geometry/dual_quaternion.rs
View File

@ -5,7 +5,7 @@ use crate::{Quaternion, SimdRealField};
/// # Indexing
///
/// DualQuaternions are stored as \[..real, ..dual\].
/// Both of the quaternion components are laid out in `w, i, j, k` order.
/// Both of the quaternion components are laid out in `i, j, k, w` order.
///
/// ```
/// # use nalgebra::{DualQuaternion, Quaternion};
@ -14,9 +14,11 @@ use crate::{Quaternion, SimdRealField};
/// 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);
/// 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:
@ -27,46 +29,10 @@ use crate::{Quaternion, SimdRealField};
#[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])
}
/// 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>
@ -85,14 +51,14 @@ where
///
/// let dq_normalized = dq.normalize();
///
/// relative_eq!(dq_normalized.real().norm(), 1.0);
/// 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();
let real_norm = self.real.norm();
Self::from_real_and_dual(self.real() / real_norm, self.dual() / real_norm)
Self::from_real_and_dual(self.real / real_norm, self.dual / real_norm)
}
/// Normalizes this quaternion.
@ -107,7 +73,7 @@ where
///
/// dq.normalize_mut();
///
/// relative_eq!(dq.real().norm(), 1.0);
/// relative_eq!(dq.real.norm(), 1.0);
/// ```
#[inline]
pub fn normalize_mut(&mut self) {

8
src/geometry/dual_quaternion_construction.rs
View File

@ -10,15 +10,11 @@ impl<N: SimdRealField> DualQuaternion<N> {
/// 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);
/// 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,
],
}
Self { real, dual }
}
}

29
src/geometry/dual_quaternion_ops.rs
View File

@ -25,21 +25,36 @@
use crate::base::allocator::Allocator;
use crate::{DefaultAllocator, DualQuaternion, SimdRealField, U1, U4};
use simba::simd::SimdValue;
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.dq[i]
&self.as_ref()[i]
}
}
impl<N: SimdRealField> IndexMut<usize> for DualQuaternion<N> {
#[inline]
fn index_mut(&mut self, i: usize) -> &mut N {
&mut self.dq[i]
&mut self.as_mut()[i]
}
}
@ -52,8 +67,8 @@ where
fn mul(self, rhs: Self) -> Self::Output {
Self::from_real_and_dual(
self.real() * rhs.real(),
self.real() * rhs.dual() + self.dual() * rhs.real(),
self.real * rhs.real,
self.real * rhs.dual + self.dual * rhs.real,
)
}
}
@ -66,7 +81,7 @@ where
type Output = DualQuaternion<N>;
fn mul(self, rhs: N) -> Self::Output {
Self::from_real_and_dual(self.real() * rhs, self.dual() * rhs)
Self::from_real_and_dual(self.real * rhs, self.dual * rhs)
}
}
@ -78,7 +93,7 @@ where
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())
Self::from_real_and_dual(self.real + rhs.real, self.dual + rhs.dual)
}
}
@ -90,6 +105,6 @@ where
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())
Self::from_real_and_dual(self.real - rhs.real, self.dual - rhs.dual)
}
}