Merge pull request #810 from chinedufn/dual-quaternion

Introduce DualQuaternion type
This commit is contained in:
Sébastien Crozet 2020-12-18 16:52:09 +01:00 committed by GitHub
commit d8fa3ff241
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 236 additions and 4 deletions

View File

@ -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();
}
}

View File

@ -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()),
)
}
}

View File

@ -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)
}
}

View File

@ -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::*;

View File

@ -1,7 +1,6 @@
use approx::{AbsDiffEq, RelativeEq, UlpsEq};
use num::Zero;
use std::fmt;
use std::hash;
#[cfg(feature = "abomonation-serialize")]
use std::io::{Result as IOResult, Write};
@ -14,7 +13,7 @@ use serde::{Deserialize, Deserializer, Serialize, Serializer};
use abomonation::Abomonation;
use simba::scalar::{ClosedNeg, RealField};
use simba::simd::{SimdBool, SimdOption, SimdRealField, SimdValue};
use simba::simd::{SimdBool, SimdOption, SimdRealField};
use crate::base::dimension::{U1, U3, U4};
use crate::base::storage::{CStride, RStride};
@ -23,7 +22,6 @@ use crate::base::{
};
use crate::geometry::{Point3, Rotation};
use std::ops::Neg;
/// A quaternion. See the type alias `UnitQuaternion = Unit<Quaternion>` for a quaternion
/// that may be used as a rotation.

View File

@ -10,7 +10,7 @@ use rand::distributions::{Distribution, OpenClosed01, Standard};
use rand::Rng;
use simba::scalar::RealField;
use simba::simd::{SimdBool, SimdValue};
use simba::simd::SimdBool;
use crate::base::dimension::U3;
use crate::base::storage::Storage;