Merge pull request #810 from chinedufn/dual-quaternion
Introduce DualQuaternion type
This commit is contained in:
commit
d8fa3ff241
|
@ -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();
|
||||||
|
}
|
||||||
|
}
|
|
@ -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()),
|
||||||
|
)
|
||||||
|
}
|
||||||
|
}
|
|
@ -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)
|
||||||
|
}
|
||||||
|
}
|
|
@ -35,6 +35,10 @@ mod quaternion_coordinates;
|
||||||
mod quaternion_ops;
|
mod quaternion_ops;
|
||||||
mod quaternion_simba;
|
mod quaternion_simba;
|
||||||
|
|
||||||
|
mod dual_quaternion;
|
||||||
|
mod dual_quaternion_construction;
|
||||||
|
mod dual_quaternion_ops;
|
||||||
|
|
||||||
mod unit_complex;
|
mod unit_complex;
|
||||||
#[cfg(feature = "alga")]
|
#[cfg(feature = "alga")]
|
||||||
mod unit_complex_alga;
|
mod unit_complex_alga;
|
||||||
|
@ -98,6 +102,8 @@ pub use self::rotation_alias::*;
|
||||||
|
|
||||||
pub use self::quaternion::*;
|
pub use self::quaternion::*;
|
||||||
|
|
||||||
|
pub use self::dual_quaternion::*;
|
||||||
|
|
||||||
pub use self::unit_complex::*;
|
pub use self::unit_complex::*;
|
||||||
|
|
||||||
pub use self::translation::*;
|
pub use self::translation::*;
|
||||||
|
|
|
@ -1,7 +1,6 @@
|
||||||
use approx::{AbsDiffEq, RelativeEq, UlpsEq};
|
use approx::{AbsDiffEq, RelativeEq, UlpsEq};
|
||||||
use num::Zero;
|
use num::Zero;
|
||||||
use std::fmt;
|
use std::fmt;
|
||||||
use std::hash;
|
|
||||||
#[cfg(feature = "abomonation-serialize")]
|
#[cfg(feature = "abomonation-serialize")]
|
||||||
use std::io::{Result as IOResult, Write};
|
use std::io::{Result as IOResult, Write};
|
||||||
|
|
||||||
|
@ -14,7 +13,7 @@ use serde::{Deserialize, Deserializer, Serialize, Serializer};
|
||||||
use abomonation::Abomonation;
|
use abomonation::Abomonation;
|
||||||
|
|
||||||
use simba::scalar::{ClosedNeg, RealField};
|
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::dimension::{U1, U3, U4};
|
||||||
use crate::base::storage::{CStride, RStride};
|
use crate::base::storage::{CStride, RStride};
|
||||||
|
@ -23,7 +22,6 @@ use crate::base::{
|
||||||
};
|
};
|
||||||
|
|
||||||
use crate::geometry::{Point3, Rotation};
|
use crate::geometry::{Point3, Rotation};
|
||||||
use std::ops::Neg;
|
|
||||||
|
|
||||||
/// A quaternion. See the type alias `UnitQuaternion = Unit<Quaternion>` for a quaternion
|
/// A quaternion. See the type alias `UnitQuaternion = Unit<Quaternion>` for a quaternion
|
||||||
/// that may be used as a rotation.
|
/// that may be used as a rotation.
|
||||||
|
|
|
@ -10,7 +10,7 @@ use rand::distributions::{Distribution, OpenClosed01, Standard};
|
||||||
use rand::Rng;
|
use rand::Rng;
|
||||||
|
|
||||||
use simba::scalar::RealField;
|
use simba::scalar::RealField;
|
||||||
use simba::simd::{SimdBool, SimdValue};
|
use simba::simd::SimdBool;
|
||||||
|
|
||||||
use crate::base::dimension::U3;
|
use crate::base::dimension::U3;
|
||||||
use crate::base::storage::Storage;
|
use crate::base::storage::Storage;
|
||||||
|
|
Loading…
Reference in New Issue