2021-06-18 15:45:37 +08:00
|
|
|
|
// The macros break if the references are taken out, for some reason.
|
|
|
|
|
#![allow(clippy::op_ref)]
|
|
|
|
|
|
2021-01-29 06:25:32 +08:00
|
|
|
|
use crate::{
|
2021-04-11 17:00:38 +08:00
|
|
|
|
Isometry3, Matrix4, Normed, OVector, Point3, Quaternion, Scalar, SimdRealField, Translation3,
|
|
|
|
|
Unit, UnitQuaternion, Vector3, Zero, U8,
|
2021-01-29 06:25:32 +08:00
|
|
|
|
};
|
2021-01-29 07:46:14 +08:00
|
|
|
|
use approx::{AbsDiffEq, RelativeEq, UlpsEq};
|
2021-04-12 18:14:16 +08:00
|
|
|
|
#[cfg(feature = "serde-serialize-no-std")]
|
2020-12-19 01:19:38 +08:00
|
|
|
|
use serde::{Deserialize, Deserializer, Serialize, Serializer};
|
2021-01-29 07:46:14 +08:00
|
|
|
|
use std::fmt;
|
2020-12-16 22:02:02 +08:00
|
|
|
|
|
2021-01-29 06:25:32 +08:00
|
|
|
|
use simba::scalar::{ClosedNeg, RealField};
|
|
|
|
|
|
2020-12-16 22:02:02 +08:00
|
|
|
|
/// A dual quaternion.
|
|
|
|
|
///
|
|
|
|
|
/// # Indexing
|
|
|
|
|
///
|
|
|
|
|
/// DualQuaternions are stored as \[..real, ..dual\].
|
2020-12-18 23:09:56 +08:00
|
|
|
|
/// Both of the quaternion components are laid out in `i, j, k, w` order.
|
2020-12-16 22:02:02 +08:00
|
|
|
|
///
|
|
|
|
|
/// ```
|
|
|
|
|
/// # 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);
|
2020-12-18 23:09:56 +08:00
|
|
|
|
/// assert_eq!(dq[0], 2.0);
|
|
|
|
|
/// assert_eq!(dq[1], 3.0);
|
|
|
|
|
///
|
|
|
|
|
/// assert_eq!(dq[4], 6.0);
|
|
|
|
|
/// assert_eq!(dq[7], 5.0);
|
2020-12-16 22:02:02 +08:00
|
|
|
|
/// ```
|
|
|
|
|
///
|
|
|
|
|
/// 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)]
|
2021-07-06 08:04:10 +08:00
|
|
|
|
#[derive(Debug, Copy, Clone)]
|
|
|
|
|
pub struct DualQuaternion<T> {
|
2020-12-18 23:09:56 +08:00
|
|
|
|
/// The real component of the quaternion
|
2021-04-11 17:00:38 +08:00
|
|
|
|
pub real: Quaternion<T>,
|
2020-12-18 23:09:56 +08:00
|
|
|
|
/// The dual component of the quaternion
|
2021-04-11 17:00:38 +08:00
|
|
|
|
pub dual: Quaternion<T>,
|
2020-12-16 22:02:02 +08:00
|
|
|
|
}
|
|
|
|
|
|
2021-07-16 14:53:28 +08:00
|
|
|
|
impl<T: Eq> Eq for DualQuaternion<T> {}
|
2021-07-06 08:04:10 +08:00
|
|
|
|
|
2021-07-16 14:53:28 +08:00
|
|
|
|
impl<T: PartialEq> PartialEq for DualQuaternion<T> {
|
2021-07-06 08:04:10 +08:00
|
|
|
|
#[inline]
|
|
|
|
|
fn eq(&self, right: &Self) -> bool {
|
|
|
|
|
self.real == right.real && self.dual == right.dual
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2021-07-16 14:53:28 +08:00
|
|
|
|
impl<T: Zero + Clone> Default for DualQuaternion<T> {
|
2021-02-25 22:01:53 +08:00
|
|
|
|
fn default() -> Self {
|
|
|
|
|
Self {
|
|
|
|
|
real: Quaternion::default(),
|
|
|
|
|
dual: Quaternion::default(),
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2021-04-11 17:00:38 +08:00
|
|
|
|
impl<T: SimdRealField> DualQuaternion<T>
|
2020-12-16 22:02:02 +08:00
|
|
|
|
where
|
2021-04-11 17:00:38 +08:00
|
|
|
|
T::Element: SimdRealField,
|
2020-12-16 22:02:02 +08:00
|
|
|
|
{
|
|
|
|
|
/// 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();
|
|
|
|
|
///
|
2020-12-18 23:09:56 +08:00
|
|
|
|
/// relative_eq!(dq_normalized.real.norm(), 1.0);
|
2020-12-16 22:02:02 +08:00
|
|
|
|
/// ```
|
|
|
|
|
#[inline]
|
|
|
|
|
#[must_use = "Did you mean to use normalize_mut()?"]
|
|
|
|
|
pub fn normalize(&self) -> Self {
|
2020-12-18 23:09:56 +08:00
|
|
|
|
let real_norm = self.real.norm();
|
2020-12-16 22:02:02 +08:00
|
|
|
|
|
2020-12-18 23:09:56 +08:00
|
|
|
|
Self::from_real_and_dual(self.real / real_norm, self.dual / real_norm)
|
2020-12-16 22:02:02 +08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// 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();
|
|
|
|
|
///
|
2020-12-18 23:09:56 +08:00
|
|
|
|
/// relative_eq!(dq.real.norm(), 1.0);
|
2020-12-16 22:02:02 +08:00
|
|
|
|
/// ```
|
|
|
|
|
#[inline]
|
2021-04-11 17:00:38 +08:00
|
|
|
|
pub fn normalize_mut(&mut self) -> T {
|
2021-01-29 06:25:32 +08:00
|
|
|
|
let real_norm = self.real.norm();
|
|
|
|
|
self.real /= real_norm;
|
|
|
|
|
self.dual /= real_norm;
|
|
|
|
|
real_norm
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// The conjugate of this dual quaternion, containing the conjugate of
|
|
|
|
|
/// the real and imaginary parts..
|
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # 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 conj = dq.conjugate();
|
|
|
|
|
/// assert!(conj.real.i == -2.0 && conj.real.j == -3.0 && conj.real.k == -4.0);
|
|
|
|
|
/// assert!(conj.real.w == 1.0);
|
|
|
|
|
/// assert!(conj.dual.i == -6.0 && conj.dual.j == -7.0 && conj.dual.k == -8.0);
|
|
|
|
|
/// assert!(conj.dual.w == 5.0);
|
|
|
|
|
/// ```
|
|
|
|
|
#[inline]
|
|
|
|
|
#[must_use = "Did you mean to use conjugate_mut()?"]
|
|
|
|
|
pub fn conjugate(&self) -> Self {
|
|
|
|
|
Self::from_real_and_dual(self.real.conjugate(), self.dual.conjugate())
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Replaces this quaternion by its conjugate.
|
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # 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.conjugate_mut();
|
|
|
|
|
/// assert!(dq.real.i == -2.0 && dq.real.j == -3.0 && dq.real.k == -4.0);
|
|
|
|
|
/// assert!(dq.real.w == 1.0);
|
|
|
|
|
/// assert!(dq.dual.i == -6.0 && dq.dual.j == -7.0 && dq.dual.k == -8.0);
|
|
|
|
|
/// assert!(dq.dual.w == 5.0);
|
|
|
|
|
/// ```
|
|
|
|
|
#[inline]
|
|
|
|
|
pub fn conjugate_mut(&mut self) {
|
|
|
|
|
self.real.conjugate_mut();
|
|
|
|
|
self.dual.conjugate_mut();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Inverts this dual quaternion if it is not zero.
|
|
|
|
|
///
|
|
|
|
|
/// # 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 inverse = dq.try_inverse();
|
|
|
|
|
///
|
|
|
|
|
/// assert!(inverse.is_some());
|
|
|
|
|
/// assert_relative_eq!(inverse.unwrap() * dq, DualQuaternion::identity());
|
|
|
|
|
///
|
|
|
|
|
/// //Non-invertible case
|
|
|
|
|
/// let zero = Quaternion::new(0.0, 0.0, 0.0, 0.0);
|
|
|
|
|
/// let dq = DualQuaternion::from_real_and_dual(zero, zero);
|
|
|
|
|
/// let inverse = dq.try_inverse();
|
|
|
|
|
///
|
|
|
|
|
/// assert!(inverse.is_none());
|
|
|
|
|
/// ```
|
|
|
|
|
#[inline]
|
|
|
|
|
#[must_use = "Did you mean to use try_inverse_mut()?"]
|
|
|
|
|
pub fn try_inverse(&self) -> Option<Self>
|
|
|
|
|
where
|
2021-04-11 17:00:38 +08:00
|
|
|
|
T: RealField,
|
2021-01-29 06:25:32 +08:00
|
|
|
|
{
|
|
|
|
|
let mut res = *self;
|
|
|
|
|
if res.try_inverse_mut() {
|
|
|
|
|
Some(res)
|
|
|
|
|
} else {
|
|
|
|
|
None
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Inverts this dual quaternion in-place if it is not zero.
|
|
|
|
|
///
|
|
|
|
|
/// # 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 mut dq_inverse = dq;
|
|
|
|
|
/// dq_inverse.try_inverse_mut();
|
|
|
|
|
///
|
|
|
|
|
/// assert_relative_eq!(dq_inverse * dq, DualQuaternion::identity());
|
|
|
|
|
///
|
|
|
|
|
/// //Non-invertible case
|
|
|
|
|
/// let zero = Quaternion::new(0.0, 0.0, 0.0, 0.0);
|
|
|
|
|
/// let mut dq = DualQuaternion::from_real_and_dual(zero, zero);
|
|
|
|
|
/// assert!(!dq.try_inverse_mut());
|
|
|
|
|
/// ```
|
|
|
|
|
#[inline]
|
|
|
|
|
pub fn try_inverse_mut(&mut self) -> bool
|
|
|
|
|
where
|
2021-04-11 17:00:38 +08:00
|
|
|
|
T: RealField,
|
2021-01-29 06:25:32 +08:00
|
|
|
|
{
|
|
|
|
|
let inverted = self.real.try_inverse_mut();
|
|
|
|
|
if inverted {
|
|
|
|
|
self.dual = -self.real * self.dual * self.real;
|
|
|
|
|
true
|
|
|
|
|
} else {
|
|
|
|
|
false
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Linear interpolation between two dual quaternions.
|
|
|
|
|
///
|
|
|
|
|
/// Computes `self * (1 - t) + other * t`.
|
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # use nalgebra::{DualQuaternion, Quaternion};
|
|
|
|
|
/// let dq1 = DualQuaternion::from_real_and_dual(
|
|
|
|
|
/// Quaternion::new(1.0, 0.0, 0.0, 4.0),
|
|
|
|
|
/// Quaternion::new(0.0, 2.0, 0.0, 0.0)
|
|
|
|
|
/// );
|
|
|
|
|
/// let dq2 = DualQuaternion::from_real_and_dual(
|
|
|
|
|
/// Quaternion::new(2.0, 0.0, 1.0, 0.0),
|
|
|
|
|
/// Quaternion::new(0.0, 2.0, 0.0, 0.0)
|
|
|
|
|
/// );
|
|
|
|
|
/// assert_eq!(dq1.lerp(&dq2, 0.25), DualQuaternion::from_real_and_dual(
|
|
|
|
|
/// Quaternion::new(1.25, 0.0, 0.25, 3.0),
|
|
|
|
|
/// Quaternion::new(0.0, 2.0, 0.0, 0.0)
|
|
|
|
|
/// ));
|
|
|
|
|
/// ```
|
|
|
|
|
#[inline]
|
2021-06-07 22:34:03 +08:00
|
|
|
|
#[must_use]
|
2021-04-11 17:00:38 +08:00
|
|
|
|
pub fn lerp(&self, other: &Self, t: T) -> Self {
|
|
|
|
|
self * (T::one() - t) + other * t
|
2020-12-16 22:02:02 +08:00
|
|
|
|
}
|
|
|
|
|
}
|
2020-12-19 01:19:38 +08:00
|
|
|
|
|
2021-04-12 18:14:16 +08:00
|
|
|
|
#[cfg(feature = "serde-serialize-no-std")]
|
2021-04-11 17:00:38 +08:00
|
|
|
|
impl<T: SimdRealField> Serialize for DualQuaternion<T>
|
2020-12-19 01:19:38 +08:00
|
|
|
|
where
|
2021-04-11 17:00:38 +08:00
|
|
|
|
T: Serialize,
|
2020-12-19 01:19:38 +08:00
|
|
|
|
{
|
|
|
|
|
fn serialize<S>(&self, serializer: S) -> Result<<S as Serializer>::Ok, <S as Serializer>::Error>
|
|
|
|
|
where
|
|
|
|
|
S: Serializer,
|
|
|
|
|
{
|
|
|
|
|
self.as_ref().serialize(serializer)
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2021-04-12 18:14:16 +08:00
|
|
|
|
#[cfg(feature = "serde-serialize-no-std")]
|
2021-04-11 17:00:38 +08:00
|
|
|
|
impl<'a, T: SimdRealField> Deserialize<'a> for DualQuaternion<T>
|
2020-12-19 01:19:38 +08:00
|
|
|
|
where
|
2021-04-11 17:00:38 +08:00
|
|
|
|
T: Deserialize<'a>,
|
2020-12-19 01:19:38 +08:00
|
|
|
|
{
|
|
|
|
|
fn deserialize<Des>(deserializer: Des) -> Result<Self, Des::Error>
|
|
|
|
|
where
|
|
|
|
|
Des: Deserializer<'a>,
|
|
|
|
|
{
|
2021-04-11 17:00:38 +08:00
|
|
|
|
type Dq<T> = [T; 8];
|
2020-12-19 01:19:38 +08:00
|
|
|
|
|
2021-04-11 17:00:38 +08:00
|
|
|
|
let dq: Dq<T> = Dq::<T>::deserialize(deserializer)?;
|
2020-12-19 01:19:38 +08:00
|
|
|
|
|
|
|
|
|
Ok(Self {
|
|
|
|
|
real: Quaternion::new(dq[3], dq[0], dq[1], dq[2]),
|
|
|
|
|
dual: Quaternion::new(dq[7], dq[4], dq[5], dq[6]),
|
|
|
|
|
})
|
|
|
|
|
}
|
|
|
|
|
}
|
2021-01-29 06:25:32 +08:00
|
|
|
|
|
2021-04-11 17:00:38 +08:00
|
|
|
|
impl<T: RealField> DualQuaternion<T> {
|
2021-07-06 05:51:27 +08:00
|
|
|
|
fn to_vector(self) -> OVector<T, U8> {
|
2021-06-18 15:45:37 +08:00
|
|
|
|
(*self.as_ref()).into()
|
2021-01-29 06:25:32 +08:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2021-04-11 17:00:38 +08:00
|
|
|
|
impl<T: RealField + AbsDiffEq<Epsilon = T>> AbsDiffEq for DualQuaternion<T> {
|
|
|
|
|
type Epsilon = T;
|
2021-01-29 06:25:32 +08:00
|
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
|
fn default_epsilon() -> Self::Epsilon {
|
2021-04-11 17:00:38 +08:00
|
|
|
|
T::default_epsilon()
|
2021-01-29 06:25:32 +08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
|
fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
|
|
|
|
|
self.to_vector().abs_diff_eq(&other.to_vector(), epsilon) ||
|
|
|
|
|
// Account for the double-covering of S², i.e. q = -q
|
|
|
|
|
self.to_vector().iter().zip(other.to_vector().iter()).all(|(a, b)| a.abs_diff_eq(&-*b, epsilon))
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2021-04-11 17:00:38 +08:00
|
|
|
|
impl<T: RealField + RelativeEq<Epsilon = T>> RelativeEq for DualQuaternion<T> {
|
2021-01-29 06:25:32 +08:00
|
|
|
|
#[inline]
|
|
|
|
|
fn default_max_relative() -> Self::Epsilon {
|
2021-04-11 17:00:38 +08:00
|
|
|
|
T::default_max_relative()
|
2021-01-29 06:25:32 +08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
|
fn relative_eq(
|
|
|
|
|
&self,
|
|
|
|
|
other: &Self,
|
|
|
|
|
epsilon: Self::Epsilon,
|
|
|
|
|
max_relative: Self::Epsilon,
|
|
|
|
|
) -> bool {
|
|
|
|
|
self.to_vector().relative_eq(&other.to_vector(), epsilon, max_relative) ||
|
|
|
|
|
// Account for the double-covering of S², i.e. q = -q
|
|
|
|
|
self.to_vector().iter().zip(other.to_vector().iter()).all(|(a, b)| a.relative_eq(&-*b, epsilon, max_relative))
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2021-04-11 17:00:38 +08:00
|
|
|
|
impl<T: RealField + UlpsEq<Epsilon = T>> UlpsEq for DualQuaternion<T> {
|
2021-01-29 06:25:32 +08:00
|
|
|
|
#[inline]
|
|
|
|
|
fn default_max_ulps() -> u32 {
|
2021-04-11 17:00:38 +08:00
|
|
|
|
T::default_max_ulps()
|
2021-01-29 06:25:32 +08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
|
fn ulps_eq(&self, other: &Self, epsilon: Self::Epsilon, max_ulps: u32) -> bool {
|
|
|
|
|
self.to_vector().ulps_eq(&other.to_vector(), epsilon, max_ulps) ||
|
|
|
|
|
// Account for the double-covering of S², i.e. q = -q.
|
|
|
|
|
self.to_vector().iter().zip(other.to_vector().iter()).all(|(a, b)| a.ulps_eq(&-*b, epsilon, max_ulps))
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2021-02-25 21:45:26 +08:00
|
|
|
|
/// A unit quaternions. May be used to represent a rotation followed by a translation.
|
2021-04-11 17:00:38 +08:00
|
|
|
|
pub type UnitDualQuaternion<T> = Unit<DualQuaternion<T>>;
|
2021-01-29 06:25:32 +08:00
|
|
|
|
|
2021-04-11 17:00:38 +08:00
|
|
|
|
impl<T: Scalar + ClosedNeg + PartialEq + SimdRealField> PartialEq for UnitDualQuaternion<T> {
|
2021-01-29 06:25:32 +08:00
|
|
|
|
#[inline]
|
|
|
|
|
fn eq(&self, rhs: &Self) -> bool {
|
|
|
|
|
self.as_ref().eq(rhs.as_ref())
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2021-04-11 17:00:38 +08:00
|
|
|
|
impl<T: Scalar + ClosedNeg + Eq + SimdRealField> Eq for UnitDualQuaternion<T> {}
|
2021-01-29 06:25:32 +08:00
|
|
|
|
|
2021-04-11 17:00:38 +08:00
|
|
|
|
impl<T: SimdRealField> Normed for DualQuaternion<T> {
|
|
|
|
|
type Norm = T::SimdRealField;
|
2021-01-29 06:25:32 +08:00
|
|
|
|
|
|
|
|
|
#[inline]
|
2021-04-11 17:00:38 +08:00
|
|
|
|
fn norm(&self) -> T::SimdRealField {
|
2021-01-29 06:25:32 +08:00
|
|
|
|
self.real.norm()
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#[inline]
|
2021-04-11 17:00:38 +08:00
|
|
|
|
fn norm_squared(&self) -> T::SimdRealField {
|
2021-01-29 06:25:32 +08:00
|
|
|
|
self.real.norm_squared()
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
|
fn scale_mut(&mut self, n: Self::Norm) {
|
|
|
|
|
self.real.scale_mut(n);
|
|
|
|
|
self.dual.scale_mut(n);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
|
fn unscale_mut(&mut self, n: Self::Norm) {
|
|
|
|
|
self.real.unscale_mut(n);
|
|
|
|
|
self.dual.unscale_mut(n);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2021-04-11 17:00:38 +08:00
|
|
|
|
impl<T: SimdRealField> UnitDualQuaternion<T>
|
2021-01-29 06:25:32 +08:00
|
|
|
|
where
|
2021-04-11 17:00:38 +08:00
|
|
|
|
T::Element: SimdRealField,
|
2021-01-29 06:25:32 +08:00
|
|
|
|
{
|
|
|
|
|
/// The underlying dual quaternion.
|
|
|
|
|
///
|
|
|
|
|
/// Same as `self.as_ref()`.
|
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # use nalgebra::{DualQuaternion, UnitDualQuaternion, Quaternion};
|
|
|
|
|
/// let id = UnitDualQuaternion::identity();
|
|
|
|
|
/// assert_eq!(*id.dual_quaternion(), DualQuaternion::from_real_and_dual(
|
|
|
|
|
/// Quaternion::new(1.0, 0.0, 0.0, 0.0),
|
|
|
|
|
/// Quaternion::new(0.0, 0.0, 0.0, 0.0)
|
|
|
|
|
/// ));
|
|
|
|
|
/// ```
|
|
|
|
|
#[inline]
|
2021-06-07 22:34:03 +08:00
|
|
|
|
#[must_use]
|
2021-04-11 17:00:38 +08:00
|
|
|
|
pub fn dual_quaternion(&self) -> &DualQuaternion<T> {
|
2021-01-29 06:25:32 +08:00
|
|
|
|
self.as_ref()
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Compute the conjugate of this unit quaternion.
|
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # use nalgebra::{UnitDualQuaternion, DualQuaternion, Quaternion};
|
|
|
|
|
/// let qr = Quaternion::new(1.0, 2.0, 3.0, 4.0);
|
|
|
|
|
/// let qd = Quaternion::new(5.0, 6.0, 7.0, 8.0);
|
|
|
|
|
/// let unit = UnitDualQuaternion::new_normalize(
|
|
|
|
|
/// DualQuaternion::from_real_and_dual(qr, qd)
|
|
|
|
|
/// );
|
|
|
|
|
/// let conj = unit.conjugate();
|
|
|
|
|
/// assert_eq!(conj.real, unit.real.conjugate());
|
|
|
|
|
/// assert_eq!(conj.dual, unit.dual.conjugate());
|
|
|
|
|
/// ```
|
|
|
|
|
#[inline]
|
|
|
|
|
#[must_use = "Did you mean to use conjugate_mut()?"]
|
|
|
|
|
pub fn conjugate(&self) -> Self {
|
|
|
|
|
Self::new_unchecked(self.as_ref().conjugate())
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Compute the conjugate of this unit quaternion in-place.
|
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # use nalgebra::{UnitDualQuaternion, DualQuaternion, Quaternion};
|
|
|
|
|
/// let qr = Quaternion::new(1.0, 2.0, 3.0, 4.0);
|
|
|
|
|
/// let qd = Quaternion::new(5.0, 6.0, 7.0, 8.0);
|
|
|
|
|
/// let unit = UnitDualQuaternion::new_normalize(
|
|
|
|
|
/// DualQuaternion::from_real_and_dual(qr, qd)
|
|
|
|
|
/// );
|
|
|
|
|
/// let mut conj = unit.clone();
|
|
|
|
|
/// conj.conjugate_mut();
|
|
|
|
|
/// assert_eq!(conj.as_ref().real, unit.as_ref().real.conjugate());
|
|
|
|
|
/// assert_eq!(conj.as_ref().dual, unit.as_ref().dual.conjugate());
|
|
|
|
|
/// ```
|
|
|
|
|
#[inline]
|
|
|
|
|
pub fn conjugate_mut(&mut self) {
|
|
|
|
|
self.as_mut_unchecked().conjugate_mut()
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Inverts this dual quaternion if it is not zero.
|
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::{UnitDualQuaternion, Quaternion, DualQuaternion};
|
|
|
|
|
/// let qr = Quaternion::new(1.0, 2.0, 3.0, 4.0);
|
|
|
|
|
/// let qd = Quaternion::new(5.0, 6.0, 7.0, 8.0);
|
|
|
|
|
/// let unit = UnitDualQuaternion::new_normalize(DualQuaternion::from_real_and_dual(qr, qd));
|
|
|
|
|
/// let inv = unit.inverse();
|
|
|
|
|
/// assert_relative_eq!(unit * inv, UnitDualQuaternion::identity(), epsilon = 1.0e-6);
|
|
|
|
|
/// assert_relative_eq!(inv * unit, UnitDualQuaternion::identity(), epsilon = 1.0e-6);
|
|
|
|
|
/// ```
|
|
|
|
|
#[inline]
|
|
|
|
|
#[must_use = "Did you mean to use inverse_mut()?"]
|
|
|
|
|
pub fn inverse(&self) -> Self {
|
2021-01-29 07:46:14 +08:00
|
|
|
|
let real = Unit::new_unchecked(self.as_ref().real)
|
|
|
|
|
.inverse()
|
|
|
|
|
.into_inner();
|
2021-01-29 06:25:32 +08:00
|
|
|
|
let dual = -real * self.as_ref().dual * real;
|
|
|
|
|
UnitDualQuaternion::new_unchecked(DualQuaternion { real, dual })
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Inverts this dual quaternion in place if it is not zero.
|
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::{UnitDualQuaternion, Quaternion, DualQuaternion};
|
|
|
|
|
/// let qr = Quaternion::new(1.0, 2.0, 3.0, 4.0);
|
|
|
|
|
/// let qd = Quaternion::new(5.0, 6.0, 7.0, 8.0);
|
|
|
|
|
/// let unit = UnitDualQuaternion::new_normalize(DualQuaternion::from_real_and_dual(qr, qd));
|
|
|
|
|
/// let mut inv = unit.clone();
|
|
|
|
|
/// inv.inverse_mut();
|
|
|
|
|
/// assert_relative_eq!(unit * inv, UnitDualQuaternion::identity(), epsilon = 1.0e-6);
|
|
|
|
|
/// assert_relative_eq!(inv * unit, UnitDualQuaternion::identity(), epsilon = 1.0e-6);
|
|
|
|
|
/// ```
|
|
|
|
|
#[inline]
|
|
|
|
|
pub fn inverse_mut(&mut self) {
|
|
|
|
|
let quat = self.as_mut_unchecked();
|
|
|
|
|
quat.real = Unit::new_unchecked(quat.real).inverse().into_inner();
|
|
|
|
|
quat.dual = -quat.real * quat.dual * quat.real;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// The unit dual quaternion needed to make `self` and `other` coincide.
|
|
|
|
|
///
|
|
|
|
|
/// The result is such that: `self.isometry_to(other) * self == other`.
|
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::{UnitDualQuaternion, DualQuaternion, Quaternion};
|
|
|
|
|
/// let qr = Quaternion::new(1.0, 2.0, 3.0, 4.0);
|
|
|
|
|
/// let qd = Quaternion::new(5.0, 6.0, 7.0, 8.0);
|
2021-02-25 21:45:26 +08:00
|
|
|
|
/// let dq1 = UnitDualQuaternion::new_normalize(DualQuaternion::from_real_and_dual(qr, qd));
|
|
|
|
|
/// let dq2 = UnitDualQuaternion::new_normalize(DualQuaternion::from_real_and_dual(qd, qr));
|
|
|
|
|
/// let dq_to = dq1.isometry_to(&dq2);
|
|
|
|
|
/// assert_relative_eq!(dq_to * dq1, dq2, epsilon = 1.0e-6);
|
2021-01-29 06:25:32 +08:00
|
|
|
|
/// ```
|
|
|
|
|
#[inline]
|
2021-06-07 22:34:03 +08:00
|
|
|
|
#[must_use]
|
2021-01-29 06:25:32 +08:00
|
|
|
|
pub fn isometry_to(&self, other: &Self) -> Self {
|
|
|
|
|
other / self
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Linear interpolation between two unit dual quaternions.
|
|
|
|
|
///
|
|
|
|
|
/// The result is not normalized.
|
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::{UnitDualQuaternion, DualQuaternion, Quaternion};
|
|
|
|
|
/// let dq1 = UnitDualQuaternion::new_normalize(DualQuaternion::from_real_and_dual(
|
|
|
|
|
/// Quaternion::new(0.5, 0.0, 0.5, 0.0),
|
|
|
|
|
/// Quaternion::new(0.0, 0.5, 0.0, 0.5)
|
|
|
|
|
/// ));
|
|
|
|
|
/// let dq2 = UnitDualQuaternion::new_normalize(DualQuaternion::from_real_and_dual(
|
|
|
|
|
/// Quaternion::new(0.5, 0.0, 0.0, 0.5),
|
|
|
|
|
/// Quaternion::new(0.5, 0.0, 0.5, 0.0)
|
|
|
|
|
/// ));
|
|
|
|
|
/// assert_relative_eq!(
|
|
|
|
|
/// UnitDualQuaternion::new_normalize(dq1.lerp(&dq2, 0.5)),
|
|
|
|
|
/// UnitDualQuaternion::new_normalize(
|
|
|
|
|
/// DualQuaternion::from_real_and_dual(
|
|
|
|
|
/// Quaternion::new(0.5, 0.0, 0.25, 0.25),
|
|
|
|
|
/// Quaternion::new(0.25, 0.25, 0.25, 0.25)
|
|
|
|
|
/// )
|
|
|
|
|
/// ),
|
|
|
|
|
/// epsilon = 1.0e-6
|
|
|
|
|
/// );
|
|
|
|
|
/// ```
|
|
|
|
|
#[inline]
|
2021-06-07 22:34:03 +08:00
|
|
|
|
#[must_use]
|
2021-04-11 17:00:38 +08:00
|
|
|
|
pub fn lerp(&self, other: &Self, t: T) -> DualQuaternion<T> {
|
2021-01-29 06:25:32 +08:00
|
|
|
|
self.as_ref().lerp(other.as_ref(), t)
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Normalized linear interpolation between two unit quaternions.
|
|
|
|
|
///
|
|
|
|
|
/// This is the same as `self.lerp` except that the result is normalized.
|
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::{UnitDualQuaternion, DualQuaternion, Quaternion};
|
|
|
|
|
/// let dq1 = UnitDualQuaternion::new_normalize(DualQuaternion::from_real_and_dual(
|
|
|
|
|
/// Quaternion::new(0.5, 0.0, 0.5, 0.0),
|
|
|
|
|
/// Quaternion::new(0.0, 0.5, 0.0, 0.5)
|
|
|
|
|
/// ));
|
|
|
|
|
/// let dq2 = UnitDualQuaternion::new_normalize(DualQuaternion::from_real_and_dual(
|
|
|
|
|
/// Quaternion::new(0.5, 0.0, 0.0, 0.5),
|
|
|
|
|
/// Quaternion::new(0.5, 0.0, 0.5, 0.0)
|
|
|
|
|
/// ));
|
|
|
|
|
/// assert_relative_eq!(dq1.nlerp(&dq2, 0.2), UnitDualQuaternion::new_normalize(
|
|
|
|
|
/// DualQuaternion::from_real_and_dual(
|
|
|
|
|
/// Quaternion::new(0.5, 0.0, 0.4, 0.1),
|
|
|
|
|
/// Quaternion::new(0.1, 0.4, 0.1, 0.4)
|
|
|
|
|
/// )
|
|
|
|
|
/// ), epsilon = 1.0e-6);
|
|
|
|
|
/// ```
|
|
|
|
|
#[inline]
|
2021-06-07 22:34:03 +08:00
|
|
|
|
#[must_use]
|
2021-04-11 17:00:38 +08:00
|
|
|
|
pub fn nlerp(&self, other: &Self, t: T) -> Self {
|
2021-01-29 06:25:32 +08:00
|
|
|
|
let mut res = self.lerp(other, t);
|
|
|
|
|
let _ = res.normalize_mut();
|
|
|
|
|
|
|
|
|
|
Self::new_unchecked(res)
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Screw linear interpolation between two unit quaternions. This creates a
|
2021-02-25 21:45:26 +08:00
|
|
|
|
/// smooth arc from one dual-quaternion to another.
|
2021-01-29 06:25:32 +08:00
|
|
|
|
///
|
|
|
|
|
/// Panics if the angle between both quaternion is 180 degrees (in which case the interpolation
|
|
|
|
|
/// is not well-defined). Use `.try_sclerp` instead to avoid the panic.
|
|
|
|
|
///
|
|
|
|
|
/// # Example
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::{UnitDualQuaternion, DualQuaternion, UnitQuaternion, Vector3};
|
|
|
|
|
///
|
|
|
|
|
/// let dq1 = UnitDualQuaternion::from_parts(
|
|
|
|
|
/// Vector3::new(0.0, 3.0, 0.0).into(),
|
|
|
|
|
/// UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_4, 0.0, 0.0),
|
|
|
|
|
/// );
|
|
|
|
|
///
|
|
|
|
|
/// let dq2 = UnitDualQuaternion::from_parts(
|
|
|
|
|
/// Vector3::new(0.0, 0.0, 3.0).into(),
|
|
|
|
|
/// UnitQuaternion::from_euler_angles(-std::f32::consts::PI, 0.0, 0.0),
|
|
|
|
|
/// );
|
|
|
|
|
///
|
|
|
|
|
/// let dq = dq1.sclerp(&dq2, 1.0 / 3.0);
|
|
|
|
|
///
|
|
|
|
|
/// assert_relative_eq!(
|
|
|
|
|
/// dq.rotation().euler_angles().0, std::f32::consts::FRAC_PI_2, epsilon = 1.0e-6
|
|
|
|
|
/// );
|
|
|
|
|
/// assert_relative_eq!(dq.translation().vector.y, 3.0, epsilon = 1.0e-6);
|
|
|
|
|
#[inline]
|
2021-06-07 22:34:03 +08:00
|
|
|
|
#[must_use]
|
2021-04-11 17:00:38 +08:00
|
|
|
|
pub fn sclerp(&self, other: &Self, t: T) -> Self
|
2021-01-29 06:25:32 +08:00
|
|
|
|
where
|
2021-04-11 17:00:38 +08:00
|
|
|
|
T: RealField,
|
2021-01-29 06:25:32 +08:00
|
|
|
|
{
|
2021-04-11 17:00:38 +08:00
|
|
|
|
self.try_sclerp(other, t, T::default_epsilon())
|
2021-01-29 06:25:32 +08:00
|
|
|
|
.expect("DualQuaternion sclerp: ambiguous configuration.")
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Computes the screw-linear interpolation between two unit quaternions or returns `None`
|
|
|
|
|
/// if both quaternions are approximately 180 degrees apart (in which case the interpolation is
|
|
|
|
|
/// not well-defined).
|
|
|
|
|
///
|
|
|
|
|
/// # Arguments
|
|
|
|
|
/// * `self`: the first quaternion to interpolate from.
|
|
|
|
|
/// * `other`: the second quaternion to interpolate toward.
|
|
|
|
|
/// * `t`: the interpolation parameter. Should be between 0 and 1.
|
|
|
|
|
/// * `epsilon`: the value below which the sinus of the angle separating both quaternion
|
|
|
|
|
/// must be to return `None`.
|
|
|
|
|
#[inline]
|
2021-06-07 22:34:03 +08:00
|
|
|
|
#[must_use]
|
2021-04-11 17:00:38 +08:00
|
|
|
|
pub fn try_sclerp(&self, other: &Self, t: T, epsilon: T) -> Option<Self>
|
2021-01-29 06:25:32 +08:00
|
|
|
|
where
|
2021-04-11 17:00:38 +08:00
|
|
|
|
T: RealField,
|
2021-01-29 06:25:32 +08:00
|
|
|
|
{
|
2021-04-11 17:00:38 +08:00
|
|
|
|
let two = T::one() + T::one();
|
|
|
|
|
let half = T::one() / two;
|
2021-01-29 06:25:32 +08:00
|
|
|
|
|
|
|
|
|
// Invert one of the quaternions if we've got a longest-path
|
|
|
|
|
// interpolation.
|
|
|
|
|
let other = {
|
|
|
|
|
let dot_product = self.as_ref().real.coords.dot(&other.as_ref().real.coords);
|
2021-04-11 17:00:38 +08:00
|
|
|
|
if dot_product < T::zero() {
|
2021-06-18 15:45:37 +08:00
|
|
|
|
-*other
|
2021-01-29 06:25:32 +08:00
|
|
|
|
} else {
|
2021-06-18 15:45:37 +08:00
|
|
|
|
*other
|
2021-01-29 06:25:32 +08:00
|
|
|
|
}
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
let difference = self.as_ref().conjugate() * other.as_ref();
|
|
|
|
|
let norm_squared = difference.real.vector().norm_squared();
|
2021-04-11 17:00:38 +08:00
|
|
|
|
if relative_eq!(norm_squared, T::zero(), epsilon = epsilon) {
|
2021-01-29 06:25:32 +08:00
|
|
|
|
return None;
|
|
|
|
|
}
|
|
|
|
|
|
2021-04-11 17:00:38 +08:00
|
|
|
|
let inverse_norm_squared = T::one() / norm_squared;
|
2021-01-29 06:25:32 +08:00
|
|
|
|
let inverse_norm = inverse_norm_squared.sqrt();
|
|
|
|
|
|
|
|
|
|
let mut angle = two * difference.real.scalar().acos();
|
|
|
|
|
let mut pitch = -two * difference.dual.scalar() * inverse_norm;
|
|
|
|
|
let direction = difference.real.vector() * inverse_norm;
|
|
|
|
|
let moment = (difference.dual.vector()
|
|
|
|
|
- direction * (pitch * difference.real.scalar() * half))
|
|
|
|
|
* inverse_norm;
|
|
|
|
|
|
|
|
|
|
angle *= t;
|
|
|
|
|
pitch *= t;
|
|
|
|
|
|
|
|
|
|
let sin = (half * angle).sin();
|
|
|
|
|
let cos = (half * angle).cos();
|
|
|
|
|
let real = Quaternion::from_parts(cos, direction * sin);
|
|
|
|
|
let dual = Quaternion::from_parts(
|
|
|
|
|
-pitch * half * sin,
|
|
|
|
|
moment * sin + direction * (pitch * half * cos),
|
|
|
|
|
);
|
|
|
|
|
|
2021-01-29 07:46:14 +08:00
|
|
|
|
Some(
|
|
|
|
|
self * UnitDualQuaternion::new_unchecked(DualQuaternion::from_real_and_dual(
|
|
|
|
|
real, dual,
|
|
|
|
|
)),
|
|
|
|
|
)
|
2021-01-29 06:25:32 +08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Return the rotation part of this unit dual quaternion.
|
|
|
|
|
///
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::{UnitDualQuaternion, UnitQuaternion, Vector3};
|
|
|
|
|
/// let dq = UnitDualQuaternion::from_parts(
|
|
|
|
|
/// Vector3::new(0.0, 3.0, 0.0).into(),
|
|
|
|
|
/// UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_4, 0.0, 0.0)
|
|
|
|
|
/// );
|
|
|
|
|
///
|
|
|
|
|
/// assert_relative_eq!(
|
|
|
|
|
/// dq.rotation().angle(), std::f32::consts::FRAC_PI_4, epsilon = 1.0e-6
|
|
|
|
|
/// );
|
|
|
|
|
/// ```
|
|
|
|
|
#[inline]
|
2021-06-07 22:34:03 +08:00
|
|
|
|
#[must_use]
|
2021-04-11 17:00:38 +08:00
|
|
|
|
pub fn rotation(&self) -> UnitQuaternion<T> {
|
2021-01-29 06:25:32 +08:00
|
|
|
|
Unit::new_unchecked(self.as_ref().real)
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Return the translation part of this unit dual quaternion.
|
|
|
|
|
///
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::{UnitDualQuaternion, UnitQuaternion, Vector3};
|
|
|
|
|
/// let dq = UnitDualQuaternion::from_parts(
|
|
|
|
|
/// Vector3::new(0.0, 3.0, 0.0).into(),
|
|
|
|
|
/// UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_4, 0.0, 0.0)
|
|
|
|
|
/// );
|
|
|
|
|
///
|
|
|
|
|
/// assert_relative_eq!(
|
|
|
|
|
/// dq.translation().vector, Vector3::new(0.0, 3.0, 0.0), epsilon = 1.0e-6
|
|
|
|
|
/// );
|
|
|
|
|
/// ```
|
|
|
|
|
#[inline]
|
2021-06-07 22:34:03 +08:00
|
|
|
|
#[must_use]
|
2021-04-11 17:00:38 +08:00
|
|
|
|
pub fn translation(&self) -> Translation3<T> {
|
|
|
|
|
let two = T::one() + T::one();
|
2021-01-29 06:25:32 +08:00
|
|
|
|
Translation3::from(
|
|
|
|
|
((self.as_ref().dual * self.as_ref().real.conjugate()) * two)
|
|
|
|
|
.vector()
|
|
|
|
|
.into_owned(),
|
|
|
|
|
)
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Builds an isometry from this unit dual quaternion.
|
|
|
|
|
///
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::{UnitDualQuaternion, UnitQuaternion, Vector3};
|
|
|
|
|
/// let rotation = UnitQuaternion::from_euler_angles(std::f32::consts::PI, 0.0, 0.0);
|
|
|
|
|
/// let translation = Vector3::new(1.0, 3.0, 2.5);
|
|
|
|
|
/// let dq = UnitDualQuaternion::from_parts(
|
|
|
|
|
/// translation.into(),
|
|
|
|
|
/// rotation
|
|
|
|
|
/// );
|
|
|
|
|
/// let iso = dq.to_isometry();
|
|
|
|
|
///
|
|
|
|
|
/// assert_relative_eq!(iso.rotation.angle(), std::f32::consts::PI, epsilon = 1.0e-6);
|
|
|
|
|
/// assert_relative_eq!(iso.translation.vector, translation, epsilon = 1.0e-6);
|
|
|
|
|
/// ```
|
|
|
|
|
#[inline]
|
2021-06-07 22:34:03 +08:00
|
|
|
|
#[must_use]
|
2021-07-06 05:51:27 +08:00
|
|
|
|
pub fn to_isometry(self) -> Isometry3<T> {
|
2021-01-29 06:25:32 +08:00
|
|
|
|
Isometry3::from_parts(self.translation(), self.rotation())
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Rotate and translate a point by this unit dual quaternion interpreted
|
|
|
|
|
/// as an isometry.
|
|
|
|
|
///
|
|
|
|
|
/// This is the same as the multiplication `self * pt`.
|
|
|
|
|
///
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::{UnitDualQuaternion, UnitQuaternion, Vector3, Point3};
|
|
|
|
|
/// let dq = UnitDualQuaternion::from_parts(
|
|
|
|
|
/// Vector3::new(0.0, 3.0, 0.0).into(),
|
|
|
|
|
/// UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_2, 0.0, 0.0)
|
|
|
|
|
/// );
|
|
|
|
|
/// let point = Point3::new(1.0, 2.0, 3.0);
|
|
|
|
|
///
|
|
|
|
|
/// assert_relative_eq!(
|
|
|
|
|
/// dq.transform_point(&point), Point3::new(1.0, 0.0, 2.0), epsilon = 1.0e-6
|
|
|
|
|
/// );
|
|
|
|
|
/// ```
|
|
|
|
|
#[inline]
|
2021-06-07 22:34:03 +08:00
|
|
|
|
#[must_use]
|
2021-04-11 17:00:38 +08:00
|
|
|
|
pub fn transform_point(&self, pt: &Point3<T>) -> Point3<T> {
|
2021-01-29 06:25:32 +08:00
|
|
|
|
self * pt
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Rotate a vector by this unit dual quaternion, ignoring the translational
|
|
|
|
|
/// component.
|
|
|
|
|
///
|
|
|
|
|
/// This is the same as the multiplication `self * v`.
|
|
|
|
|
///
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::{UnitDualQuaternion, UnitQuaternion, Vector3};
|
|
|
|
|
/// let dq = UnitDualQuaternion::from_parts(
|
|
|
|
|
/// Vector3::new(0.0, 3.0, 0.0).into(),
|
|
|
|
|
/// UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_2, 0.0, 0.0)
|
|
|
|
|
/// );
|
|
|
|
|
/// let vector = Vector3::new(1.0, 2.0, 3.0);
|
|
|
|
|
///
|
|
|
|
|
/// assert_relative_eq!(
|
|
|
|
|
/// dq.transform_vector(&vector), Vector3::new(1.0, -3.0, 2.0), epsilon = 1.0e-6
|
|
|
|
|
/// );
|
|
|
|
|
/// ```
|
|
|
|
|
#[inline]
|
2021-06-07 22:34:03 +08:00
|
|
|
|
#[must_use]
|
2021-04-11 17:00:38 +08:00
|
|
|
|
pub fn transform_vector(&self, v: &Vector3<T>) -> Vector3<T> {
|
2021-01-29 06:25:32 +08:00
|
|
|
|
self * v
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Rotate and translate a point by the inverse of this unit quaternion.
|
2021-02-25 21:45:26 +08:00
|
|
|
|
///
|
|
|
|
|
/// This may be cheaper than inverting the unit dual quaternion and
|
|
|
|
|
/// transforming the point.
|
2021-01-29 06:25:32 +08:00
|
|
|
|
///
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::{UnitDualQuaternion, UnitQuaternion, Vector3, Point3};
|
|
|
|
|
/// let dq = UnitDualQuaternion::from_parts(
|
|
|
|
|
/// Vector3::new(0.0, 3.0, 0.0).into(),
|
|
|
|
|
/// UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_2, 0.0, 0.0)
|
|
|
|
|
/// );
|
|
|
|
|
/// let point = Point3::new(1.0, 2.0, 3.0);
|
|
|
|
|
///
|
|
|
|
|
/// assert_relative_eq!(
|
|
|
|
|
/// dq.inverse_transform_point(&point), Point3::new(1.0, 3.0, 1.0), epsilon = 1.0e-6
|
|
|
|
|
/// );
|
|
|
|
|
/// ```
|
|
|
|
|
#[inline]
|
2021-06-07 22:34:03 +08:00
|
|
|
|
#[must_use]
|
2021-04-11 17:00:38 +08:00
|
|
|
|
pub fn inverse_transform_point(&self, pt: &Point3<T>) -> Point3<T> {
|
2021-01-29 06:25:32 +08:00
|
|
|
|
self.inverse() * pt
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Rotate a vector by the inverse of this unit quaternion, ignoring the
|
2021-02-25 21:45:26 +08:00
|
|
|
|
/// translational component.
|
|
|
|
|
///
|
|
|
|
|
/// This may be cheaper than inverting the unit dual quaternion and
|
|
|
|
|
/// transforming the vector.
|
2021-01-29 06:25:32 +08:00
|
|
|
|
///
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::{UnitDualQuaternion, UnitQuaternion, Vector3};
|
|
|
|
|
/// let dq = UnitDualQuaternion::from_parts(
|
|
|
|
|
/// Vector3::new(0.0, 3.0, 0.0).into(),
|
|
|
|
|
/// UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_2, 0.0, 0.0)
|
|
|
|
|
/// );
|
|
|
|
|
/// let vector = Vector3::new(1.0, 2.0, 3.0);
|
|
|
|
|
///
|
|
|
|
|
/// assert_relative_eq!(
|
|
|
|
|
/// dq.inverse_transform_vector(&vector), Vector3::new(1.0, 3.0, -2.0), epsilon = 1.0e-6
|
|
|
|
|
/// );
|
|
|
|
|
/// ```
|
|
|
|
|
#[inline]
|
2021-06-07 22:34:03 +08:00
|
|
|
|
#[must_use]
|
2021-04-11 17:00:38 +08:00
|
|
|
|
pub fn inverse_transform_vector(&self, v: &Vector3<T>) -> Vector3<T> {
|
2021-01-29 06:25:32 +08:00
|
|
|
|
self.inverse() * v
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Rotate a unit vector by the inverse of this unit quaternion, ignoring
|
|
|
|
|
/// the translational component. This may be
|
|
|
|
|
/// cheaper than inverting the unit dual quaternion and transforming the
|
|
|
|
|
/// vector.
|
|
|
|
|
///
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::{UnitDualQuaternion, UnitQuaternion, Unit, Vector3};
|
|
|
|
|
/// let dq = UnitDualQuaternion::from_parts(
|
|
|
|
|
/// Vector3::new(0.0, 3.0, 0.0).into(),
|
|
|
|
|
/// UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_2, 0.0, 0.0)
|
|
|
|
|
/// );
|
|
|
|
|
/// let vector = Unit::new_unchecked(Vector3::new(0.0, 1.0, 0.0));
|
|
|
|
|
///
|
|
|
|
|
/// assert_relative_eq!(
|
|
|
|
|
/// dq.inverse_transform_unit_vector(&vector),
|
|
|
|
|
/// Unit::new_unchecked(Vector3::new(0.0, 0.0, -1.0)),
|
|
|
|
|
/// epsilon = 1.0e-6
|
|
|
|
|
/// );
|
|
|
|
|
/// ```
|
|
|
|
|
#[inline]
|
2021-06-07 22:34:03 +08:00
|
|
|
|
#[must_use]
|
2021-04-11 17:00:38 +08:00
|
|
|
|
pub fn inverse_transform_unit_vector(&self, v: &Unit<Vector3<T>>) -> Unit<Vector3<T>> {
|
2021-01-29 06:25:32 +08:00
|
|
|
|
self.inverse() * v
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2021-04-11 17:00:38 +08:00
|
|
|
|
impl<T: SimdRealField + RealField> UnitDualQuaternion<T>
|
2021-01-29 06:25:32 +08:00
|
|
|
|
where
|
2021-04-11 17:00:38 +08:00
|
|
|
|
T::Element: SimdRealField,
|
2021-01-29 06:25:32 +08:00
|
|
|
|
{
|
|
|
|
|
/// Converts this unit dual quaternion interpreted as an isometry
|
|
|
|
|
/// into its equivalent homogeneous transformation matrix.
|
|
|
|
|
///
|
|
|
|
|
/// ```
|
|
|
|
|
/// # #[macro_use] extern crate approx;
|
|
|
|
|
/// # use nalgebra::{Matrix4, UnitDualQuaternion, UnitQuaternion, Vector3};
|
|
|
|
|
/// let dq = UnitDualQuaternion::from_parts(
|
|
|
|
|
/// Vector3::new(1.0, 3.0, 2.0).into(),
|
|
|
|
|
/// UnitQuaternion::from_axis_angle(&Vector3::z_axis(), std::f32::consts::FRAC_PI_6)
|
|
|
|
|
/// );
|
|
|
|
|
/// let expected = Matrix4::new(0.8660254, -0.5, 0.0, 1.0,
|
|
|
|
|
/// 0.5, 0.8660254, 0.0, 3.0,
|
|
|
|
|
/// 0.0, 0.0, 1.0, 2.0,
|
|
|
|
|
/// 0.0, 0.0, 0.0, 1.0);
|
|
|
|
|
///
|
|
|
|
|
/// assert_relative_eq!(dq.to_homogeneous(), expected, epsilon = 1.0e-6);
|
|
|
|
|
/// ```
|
|
|
|
|
#[inline]
|
2021-06-07 22:34:03 +08:00
|
|
|
|
#[must_use]
|
2021-07-06 05:51:27 +08:00
|
|
|
|
pub fn to_homogeneous(self) -> Matrix4<T> {
|
2021-01-29 06:25:32 +08:00
|
|
|
|
self.to_isometry().to_homogeneous()
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2021-04-11 17:00:38 +08:00
|
|
|
|
impl<T: RealField> Default for UnitDualQuaternion<T> {
|
2021-01-29 06:25:32 +08:00
|
|
|
|
fn default() -> Self {
|
|
|
|
|
Self::identity()
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2021-04-11 17:00:38 +08:00
|
|
|
|
impl<T: RealField + fmt::Display> fmt::Display for UnitDualQuaternion<T> {
|
2021-01-29 06:25:32 +08:00
|
|
|
|
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
|
|
|
|
|
if let Some(axis) = self.rotation().axis() {
|
|
|
|
|
let axis = axis.into_inner();
|
|
|
|
|
write!(
|
|
|
|
|
f,
|
|
|
|
|
"UnitDualQuaternion translation: {} − angle: {} − axis: ({}, {}, {})",
|
|
|
|
|
self.translation().vector,
|
|
|
|
|
self.rotation().angle(),
|
|
|
|
|
axis[0],
|
|
|
|
|
axis[1],
|
|
|
|
|
axis[2]
|
|
|
|
|
)
|
|
|
|
|
} else {
|
|
|
|
|
write!(
|
|
|
|
|
f,
|
|
|
|
|
"UnitDualQuaternion translation: {} − angle: {} − axis: (undefined)",
|
|
|
|
|
self.translation().vector,
|
|
|
|
|
self.rotation().angle()
|
|
|
|
|
)
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2021-04-11 17:00:38 +08:00
|
|
|
|
impl<T: RealField + AbsDiffEq<Epsilon = T>> AbsDiffEq for UnitDualQuaternion<T> {
|
|
|
|
|
type Epsilon = T;
|
2021-01-29 06:25:32 +08:00
|
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
|
fn default_epsilon() -> Self::Epsilon {
|
2021-04-11 17:00:38 +08:00
|
|
|
|
T::default_epsilon()
|
2021-01-29 06:25:32 +08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
|
fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
|
|
|
|
|
self.as_ref().abs_diff_eq(other.as_ref(), epsilon)
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2021-04-11 17:00:38 +08:00
|
|
|
|
impl<T: RealField + RelativeEq<Epsilon = T>> RelativeEq for UnitDualQuaternion<T> {
|
2021-01-29 06:25:32 +08:00
|
|
|
|
#[inline]
|
|
|
|
|
fn default_max_relative() -> Self::Epsilon {
|
2021-04-11 17:00:38 +08:00
|
|
|
|
T::default_max_relative()
|
2021-01-29 06:25:32 +08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
|
fn relative_eq(
|
|
|
|
|
&self,
|
|
|
|
|
other: &Self,
|
|
|
|
|
epsilon: Self::Epsilon,
|
|
|
|
|
max_relative: Self::Epsilon,
|
|
|
|
|
) -> bool {
|
|
|
|
|
self.as_ref()
|
|
|
|
|
.relative_eq(other.as_ref(), epsilon, max_relative)
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2021-04-11 17:00:38 +08:00
|
|
|
|
impl<T: RealField + UlpsEq<Epsilon = T>> UlpsEq for UnitDualQuaternion<T> {
|
2021-01-29 06:25:32 +08:00
|
|
|
|
#[inline]
|
|
|
|
|
fn default_max_ulps() -> u32 {
|
2021-04-11 17:00:38 +08:00
|
|
|
|
T::default_max_ulps()
|
2021-01-29 06:25:32 +08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
|
fn ulps_eq(&self, other: &Self, epsilon: Self::Epsilon, max_ulps: u32) -> bool {
|
|
|
|
|
self.as_ref().ulps_eq(other.as_ref(), epsilon, max_ulps)
|
|
|
|
|
}
|
|
|
|
|
}
|