forked from M-Labs/nalgebra
Merge pull request #824 from tpdickso/dual_quaternion
Implement additional `DualQuaternion` ops and `UnitDualQuaternion`
This commit is contained in:
Sébastien Crozet
GitHub
commit
fa2e025a1e
@ -1,6 +1,13 @@
|
||||
use crate::{Quaternion, SimdRealField};
|
||||
use crate::{
|
||||
Isometry3, Matrix4, Normed, Point3, Quaternion, Scalar, SimdRealField, Translation3, Unit,
|
||||
UnitQuaternion, Vector3, VectorN, Zero, U8,
|
||||
};
|
||||
use approx::{AbsDiffEq, RelativeEq, UlpsEq};
|
||||
#[cfg(feature = "serde-serialize")]
|
||||
use serde::{Deserialize, Deserializer, Serialize, Serializer};
|
||||
use std::fmt;
|
||||
|
||||
use simba::scalar::{ClosedNeg, RealField};
|
||||
|
||||
/// A dual quaternion.
|
||||
///
|
||||
@ -28,14 +35,23 @@ use serde::{Deserialize, Deserializer, Serialize, Serializer};
|
||||
/// 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)]
|
||||
pub struct DualQuaternion<N: SimdRealField> {
|
||||
#[derive(Debug, Eq, PartialEq, Copy, Clone)]
|
||||
pub struct DualQuaternion<N: Scalar> {
|
||||
/// The real component of the quaternion
|
||||
pub real: Quaternion<N>,
|
||||
/// The dual component of the quaternion
|
||||
pub dual: Quaternion<N>,
|
||||
}
|
||||
|
||||
impl<N: Scalar + Zero> Default for DualQuaternion<N> {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
real: Quaternion::default(),
|
||||
dual: Quaternion::default(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: SimdRealField> DualQuaternion<N>
|
||||
where
|
||||
N::Element: SimdRealField,
|
||||
@ -77,8 +93,147 @@ where
|
||||
/// relative_eq!(dq.real.norm(), 1.0);
|
||||
/// ```
|
||||
#[inline]
|
||||
pub fn normalize_mut(&mut self) {
|
||||
*self = self.normalize();
|
||||
pub fn normalize_mut(&mut self) -> N {
|
||||
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
|
||||
N: RealField,
|
||||
{
|
||||
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
|
||||
N: RealField,
|
||||
{
|
||||
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]
|
||||
pub fn lerp(&self, other: &Self, t: N) -> Self {
|
||||
self * (N::one() - t) + other * t
|
||||
}
|
||||
}
|
||||
|
||||
@ -114,3 +269,669 @@ where
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: RealField> DualQuaternion<N> {
|
||||
fn to_vector(&self) -> VectorN<N, U8> {
|
||||
self.as_ref().clone().into()
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: RealField + AbsDiffEq<Epsilon = N>> AbsDiffEq for DualQuaternion<N> {
|
||||
type Epsilon = N;
|
||||
|
||||
#[inline]
|
||||
fn default_epsilon() -> Self::Epsilon {
|
||||
N::default_epsilon()
|
||||
}
|
||||
|
||||
#[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))
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: RealField + RelativeEq<Epsilon = N>> RelativeEq for DualQuaternion<N> {
|
||||
#[inline]
|
||||
fn default_max_relative() -> Self::Epsilon {
|
||||
N::default_max_relative()
|
||||
}
|
||||
|
||||
#[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))
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: RealField + UlpsEq<Epsilon = N>> UlpsEq for DualQuaternion<N> {
|
||||
#[inline]
|
||||
fn default_max_ulps() -> u32 {
|
||||
N::default_max_ulps()
|
||||
}
|
||||
|
||||
#[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))
|
||||
}
|
||||
}
|
||||
|
||||
/// A unit quaternions. May be used to represent a rotation followed by a translation.
|
||||
pub type UnitDualQuaternion<N> = Unit<DualQuaternion<N>>;
|
||||
|
||||
impl<N: Scalar + ClosedNeg + PartialEq + SimdRealField> PartialEq for UnitDualQuaternion<N> {
|
||||
#[inline]
|
||||
fn eq(&self, rhs: &Self) -> bool {
|
||||
self.as_ref().eq(rhs.as_ref())
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: Scalar + ClosedNeg + Eq + SimdRealField> Eq for UnitDualQuaternion<N> {}
|
||||
|
||||
impl<N: SimdRealField> Normed for DualQuaternion<N> {
|
||||
type Norm = N::SimdRealField;
|
||||
|
||||
#[inline]
|
||||
fn norm(&self) -> N::SimdRealField {
|
||||
self.real.norm()
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn norm_squared(&self) -> N::SimdRealField {
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: SimdRealField> UnitDualQuaternion<N>
|
||||
where
|
||||
N::Element: SimdRealField,
|
||||
{
|
||||
/// 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]
|
||||
pub fn dual_quaternion(&self) -> &DualQuaternion<N> {
|
||||
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 {
|
||||
let real = Unit::new_unchecked(self.as_ref().real)
|
||||
.inverse()
|
||||
.into_inner();
|
||||
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]
|
||||
#[must_use = "Did you mean to use inverse_mut()?"]
|
||||
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);
|
||||
/// 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);
|
||||
/// ```
|
||||
#[inline]
|
||||
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]
|
||||
pub fn lerp(&self, other: &Self, t: N) -> DualQuaternion<N> {
|
||||
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]
|
||||
pub fn nlerp(&self, other: &Self, t: N) -> Self {
|
||||
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
|
||||
/// smooth arc from one dual-quaternion to another.
|
||||
///
|
||||
/// 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]
|
||||
pub fn sclerp(&self, other: &Self, t: N) -> Self
|
||||
where
|
||||
N: RealField,
|
||||
{
|
||||
self.try_sclerp(other, t, N::default_epsilon())
|
||||
.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]
|
||||
pub fn try_sclerp(&self, other: &Self, t: N, epsilon: N) -> Option<Self>
|
||||
where
|
||||
N: RealField,
|
||||
{
|
||||
let two = N::one() + N::one();
|
||||
let half = N::one() / two;
|
||||
|
||||
// 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);
|
||||
if dot_product < N::zero() {
|
||||
-other.clone()
|
||||
} else {
|
||||
other.clone()
|
||||
}
|
||||
};
|
||||
|
||||
let difference = self.as_ref().conjugate() * other.as_ref();
|
||||
let norm_squared = difference.real.vector().norm_squared();
|
||||
if relative_eq!(norm_squared, N::zero(), epsilon = epsilon) {
|
||||
return None;
|
||||
}
|
||||
|
||||
let inverse_norm_squared = N::one() / norm_squared;
|
||||
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),
|
||||
);
|
||||
|
||||
Some(
|
||||
self * UnitDualQuaternion::new_unchecked(DualQuaternion::from_real_and_dual(
|
||||
real, dual,
|
||||
)),
|
||||
)
|
||||
}
|
||||
|
||||
/// 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]
|
||||
pub fn rotation(&self) -> UnitQuaternion<N> {
|
||||
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]
|
||||
pub fn translation(&self) -> Translation3<N> {
|
||||
let two = N::one() + N::one();
|
||||
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]
|
||||
pub fn to_isometry(&self) -> Isometry3<N> {
|
||||
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]
|
||||
pub fn transform_point(&self, pt: &Point3<N>) -> Point3<N> {
|
||||
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]
|
||||
pub fn transform_vector(&self, v: &Vector3<N>) -> Vector3<N> {
|
||||
self * v
|
||||
}
|
||||
|
||||
/// Rotate and translate a point by the inverse of this unit quaternion.
|
||||
///
|
||||
/// This may be cheaper than inverting the unit dual quaternion and
|
||||
/// transforming the point.
|
||||
///
|
||||
/// ```
|
||||
/// # #[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]
|
||||
pub fn inverse_transform_point(&self, pt: &Point3<N>) -> Point3<N> {
|
||||
self.inverse() * pt
|
||||
}
|
||||
|
||||
/// Rotate a 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, 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]
|
||||
pub fn inverse_transform_vector(&self, v: &Vector3<N>) -> Vector3<N> {
|
||||
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]
|
||||
pub fn inverse_transform_unit_vector(&self, v: &Unit<Vector3<N>>) -> Unit<Vector3<N>> {
|
||||
self.inverse() * v
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: SimdRealField + RealField> UnitDualQuaternion<N>
|
||||
where
|
||||
N::Element: SimdRealField,
|
||||
{
|
||||
/// 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]
|
||||
pub fn to_homogeneous(&self) -> Matrix4<N> {
|
||||
self.to_isometry().to_homogeneous()
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: RealField> Default for UnitDualQuaternion<N> {
|
||||
fn default() -> Self {
|
||||
Self::identity()
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: RealField + fmt::Display> fmt::Display for UnitDualQuaternion<N> {
|
||||
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()
|
||||
)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: RealField + AbsDiffEq<Epsilon = N>> AbsDiffEq for UnitDualQuaternion<N> {
|
||||
type Epsilon = N;
|
||||
|
||||
#[inline]
|
||||
fn default_epsilon() -> Self::Epsilon {
|
||||
N::default_epsilon()
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
|
||||
self.as_ref().abs_diff_eq(other.as_ref(), epsilon)
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: RealField + RelativeEq<Epsilon = N>> RelativeEq for UnitDualQuaternion<N> {
|
||||
#[inline]
|
||||
fn default_max_relative() -> Self::Epsilon {
|
||||
N::default_max_relative()
|
||||
}
|
||||
|
||||
#[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)
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: RealField + UlpsEq<Epsilon = N>> UlpsEq for UnitDualQuaternion<N> {
|
||||
#[inline]
|
||||
fn default_max_ulps() -> u32 {
|
||||
N::default_max_ulps()
|
||||
}
|
||||
|
||||
#[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)
|
||||
}
|
||||
}
|
||||
|
||||
324
src/geometry/dual_quaternion_alga.rs
Normal file
324
src/geometry/dual_quaternion_alga.rs
Normal file
@ -0,0 +1,324 @@
|
||||
use num::Zero;
|
||||
|
||||
use alga::general::{
|
||||
AbstractGroup, AbstractGroupAbelian, AbstractLoop, AbstractMagma, AbstractModule,
|
||||
AbstractMonoid, AbstractQuasigroup, AbstractSemigroup, Additive, Id, Identity, Module,
|
||||
Multiplicative, RealField, TwoSidedInverse,
|
||||
};
|
||||
use alga::linear::{
|
||||
AffineTransformation, DirectIsometry, FiniteDimVectorSpace, Isometry, NormedSpace,
|
||||
ProjectiveTransformation, Similarity, Transformation, VectorSpace,
|
||||
};
|
||||
|
||||
use crate::base::Vector3;
|
||||
use crate::geometry::{
|
||||
DualQuaternion, Point3, Quaternion, Translation3, UnitDualQuaternion, UnitQuaternion,
|
||||
};
|
||||
|
||||
impl<N: RealField + simba::scalar::RealField> Identity<Multiplicative> for DualQuaternion<N> {
|
||||
#[inline]
|
||||
fn identity() -> Self {
|
||||
Self::identity()
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: RealField + simba::scalar::RealField> Identity<Additive> for DualQuaternion<N> {
|
||||
#[inline]
|
||||
fn identity() -> Self {
|
||||
Self::zero()
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: RealField + simba::scalar::RealField> AbstractMagma<Multiplicative> for DualQuaternion<N> {
|
||||
#[inline]
|
||||
fn operate(&self, rhs: &Self) -> Self {
|
||||
self * rhs
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: RealField + simba::scalar::RealField> AbstractMagma<Additive> for DualQuaternion<N> {
|
||||
#[inline]
|
||||
fn operate(&self, rhs: &Self) -> Self {
|
||||
self + rhs
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: RealField + simba::scalar::RealField> TwoSidedInverse<Additive> for DualQuaternion<N> {
|
||||
#[inline]
|
||||
fn two_sided_inverse(&self) -> Self {
|
||||
-self
|
||||
}
|
||||
}
|
||||
|
||||
macro_rules! impl_structures(
|
||||
($DualQuaternion: ident; $($marker: ident<$operator: ident>),* $(,)*) => {$(
|
||||
impl<N: RealField + simba::scalar::RealField> $marker<$operator> for $DualQuaternion<N> { }
|
||||
)*}
|
||||
);
|
||||
|
||||
impl_structures!(
|
||||
DualQuaternion;
|
||||
AbstractSemigroup<Multiplicative>,
|
||||
AbstractMonoid<Multiplicative>,
|
||||
|
||||
AbstractSemigroup<Additive>,
|
||||
AbstractQuasigroup<Additive>,
|
||||
AbstractMonoid<Additive>,
|
||||
AbstractLoop<Additive>,
|
||||
AbstractGroup<Additive>,
|
||||
AbstractGroupAbelian<Additive>
|
||||
);
|
||||
|
||||
/*
|
||||
*
|
||||
* Vector space.
|
||||
*
|
||||
*/
|
||||
impl<N: RealField + simba::scalar::RealField> AbstractModule for DualQuaternion<N> {
|
||||
type AbstractRing = N;
|
||||
|
||||
#[inline]
|
||||
fn multiply_by(&self, n: N) -> Self {
|
||||
self * n
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: RealField + simba::scalar::RealField> Module for DualQuaternion<N> {
|
||||
type Ring = N;
|
||||
}
|
||||
|
||||
impl<N: RealField + simba::scalar::RealField> VectorSpace for DualQuaternion<N> {
|
||||
type Field = N;
|
||||
}
|
||||
|
||||
impl<N: RealField + simba::scalar::RealField> FiniteDimVectorSpace for DualQuaternion<N> {
|
||||
#[inline]
|
||||
fn dimension() -> usize {
|
||||
8
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn canonical_basis_element(i: usize) -> Self {
|
||||
if i < 4 {
|
||||
DualQuaternion::from_real_and_dual(
|
||||
Quaternion::canonical_basis_element(i),
|
||||
Quaternion::zero(),
|
||||
)
|
||||
} else {
|
||||
DualQuaternion::from_real_and_dual(
|
||||
Quaternion::zero(),
|
||||
Quaternion::canonical_basis_element(i - 4),
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn dot(&self, other: &Self) -> N {
|
||||
self.real.dot(&other.real) + self.dual.dot(&other.dual)
|
||||
}
|
||||
|
||||
#[inline]
|
||||
unsafe fn component_unchecked(&self, i: usize) -> &N {
|
||||
self.as_ref().get_unchecked(i)
|
||||
}
|
||||
|
||||
#[inline]
|
||||
unsafe fn component_unchecked_mut(&mut self, i: usize) -> &mut N {
|
||||
self.as_mut().get_unchecked_mut(i)
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: RealField + simba::scalar::RealField> NormedSpace for DualQuaternion<N> {
|
||||
type RealField = N;
|
||||
type ComplexField = N;
|
||||
|
||||
#[inline]
|
||||
fn norm_squared(&self) -> N {
|
||||
self.real.norm_squared()
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn norm(&self) -> N {
|
||||
self.real.norm()
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn normalize(&self) -> Self {
|
||||
self.normalize()
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn normalize_mut(&mut self) -> N {
|
||||
self.normalize_mut()
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn try_normalize(&self, min_norm: N) -> Option<Self> {
|
||||
let real_norm = self.real.norm();
|
||||
if real_norm > min_norm {
|
||||
Some(Self::from_real_and_dual(
|
||||
self.real / real_norm,
|
||||
self.dual / real_norm,
|
||||
))
|
||||
} else {
|
||||
None
|
||||
}
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn try_normalize_mut(&mut self, min_norm: N) -> Option<N> {
|
||||
let real_norm = self.real.norm();
|
||||
if real_norm > min_norm {
|
||||
self.real /= real_norm;
|
||||
self.dual /= real_norm;
|
||||
Some(real_norm)
|
||||
} else {
|
||||
None
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
* Implementations for UnitDualQuaternion.
|
||||
*
|
||||
*/
|
||||
impl<N: RealField + simba::scalar::RealField> Identity<Multiplicative> for UnitDualQuaternion<N> {
|
||||
#[inline]
|
||||
fn identity() -> Self {
|
||||
Self::identity()
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: RealField + simba::scalar::RealField> AbstractMagma<Multiplicative>
|
||||
for UnitDualQuaternion<N>
|
||||
{
|
||||
#[inline]
|
||||
fn operate(&self, rhs: &Self) -> Self {
|
||||
self * rhs
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: RealField + simba::scalar::RealField> TwoSidedInverse<Multiplicative>
|
||||
for UnitDualQuaternion<N>
|
||||
{
|
||||
#[inline]
|
||||
fn two_sided_inverse(&self) -> Self {
|
||||
self.inverse()
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn two_sided_inverse_mut(&mut self) {
|
||||
self.inverse_mut()
|
||||
}
|
||||
}
|
||||
|
||||
impl_structures!(
|
||||
UnitDualQuaternion;
|
||||
AbstractSemigroup<Multiplicative>,
|
||||
AbstractQuasigroup<Multiplicative>,
|
||||
AbstractMonoid<Multiplicative>,
|
||||
AbstractLoop<Multiplicative>,
|
||||
AbstractGroup<Multiplicative>
|
||||
);
|
||||
|
||||
impl<N: RealField + simba::scalar::RealField> Transformation<Point3<N>> for UnitDualQuaternion<N> {
|
||||
#[inline]
|
||||
fn transform_point(&self, pt: &Point3<N>) -> Point3<N> {
|
||||
self.transform_point(pt)
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn transform_vector(&self, v: &Vector3<N>) -> Vector3<N> {
|
||||
self.transform_vector(v)
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: RealField + simba::scalar::RealField> ProjectiveTransformation<Point3<N>>
|
||||
for UnitDualQuaternion<N>
|
||||
{
|
||||
#[inline]
|
||||
fn inverse_transform_point(&self, pt: &Point3<N>) -> Point3<N> {
|
||||
self.inverse_transform_point(pt)
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn inverse_transform_vector(&self, v: &Vector3<N>) -> Vector3<N> {
|
||||
self.inverse_transform_vector(v)
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: RealField + simba::scalar::RealField> AffineTransformation<Point3<N>>
|
||||
for UnitDualQuaternion<N>
|
||||
{
|
||||
type Rotation = UnitQuaternion<N>;
|
||||
type NonUniformScaling = Id;
|
||||
type Translation = Translation3<N>;
|
||||
|
||||
#[inline]
|
||||
fn decompose(&self) -> (Self::Translation, Self::Rotation, Id, Self::Rotation) {
|
||||
(
|
||||
self.translation(),
|
||||
self.rotation(),
|
||||
Id::new(),
|
||||
UnitQuaternion::identity(),
|
||||
)
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn append_translation(&self, translation: &Self::Translation) -> Self {
|
||||
self * Self::from_parts(translation.clone(), UnitQuaternion::identity())
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn prepend_translation(&self, translation: &Self::Translation) -> Self {
|
||||
Self::from_parts(translation.clone(), UnitQuaternion::identity()) * self
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn append_rotation(&self, r: &Self::Rotation) -> Self {
|
||||
r * self
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn prepend_rotation(&self, r: &Self::Rotation) -> Self {
|
||||
self * r
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn append_scaling(&self, _: &Self::NonUniformScaling) -> Self {
|
||||
self.clone()
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn prepend_scaling(&self, _: &Self::NonUniformScaling) -> Self {
|
||||
self.clone()
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: RealField + simba::scalar::RealField> Similarity<Point3<N>> for UnitDualQuaternion<N> {
|
||||
type Scaling = Id;
|
||||
|
||||
#[inline]
|
||||
fn translation(&self) -> Translation3<N> {
|
||||
self.translation()
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn rotation(&self) -> UnitQuaternion<N> {
|
||||
self.rotation()
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn scaling(&self) -> Id {
|
||||
Id::new()
|
||||
}
|
||||
}
|
||||
|
||||
macro_rules! marker_impl(
|
||||
($($Trait: ident),*) => {$(
|
||||
impl<N: RealField + simba::scalar::RealField> $Trait<Point3<N>> for UnitDualQuaternion<N> { }
|
||||
)*}
|
||||
);
|
||||
|
||||
marker_impl!(Isometry, DirectIsometry);
|
||||
@ -1,6 +1,12 @@
|
||||
use crate::{DualQuaternion, Quaternion, SimdRealField};
|
||||
use crate::{
|
||||
DualQuaternion, Isometry3, Quaternion, Scalar, SimdRealField, Translation3, UnitDualQuaternion,
|
||||
UnitQuaternion,
|
||||
};
|
||||
use num::{One, Zero};
|
||||
#[cfg(feature = "arbitrary")]
|
||||
use quickcheck::{Arbitrary, Gen};
|
||||
|
||||
impl<N: SimdRealField> DualQuaternion<N> {
|
||||
impl<N: Scalar> DualQuaternion<N> {
|
||||
/// Creates a dual quaternion from its rotation and translation components.
|
||||
///
|
||||
/// # Example
|
||||
@ -16,7 +22,8 @@ impl<N: SimdRealField> DualQuaternion<N> {
|
||||
pub fn from_real_and_dual(real: Quaternion<N>, dual: Quaternion<N>) -> Self {
|
||||
Self { real, dual }
|
||||
}
|
||||
/// The dual quaternion multiplicative identity
|
||||
|
||||
/// The dual quaternion multiplicative identity.
|
||||
///
|
||||
/// # Example
|
||||
///
|
||||
@ -33,10 +40,183 @@ impl<N: SimdRealField> DualQuaternion<N> {
|
||||
/// assert_eq!(dq2 * dq1, dq2);
|
||||
/// ```
|
||||
#[inline]
|
||||
pub fn identity() -> Self {
|
||||
pub fn identity() -> Self
|
||||
where
|
||||
N: SimdRealField,
|
||||
{
|
||||
Self::from_real_and_dual(
|
||||
Quaternion::from_real(N::one()),
|
||||
Quaternion::from_real(N::zero()),
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: SimdRealField> DualQuaternion<N>
|
||||
where
|
||||
N::Element: SimdRealField,
|
||||
{
|
||||
/// Creates a dual quaternion from only its real part, with no translation
|
||||
/// component.
|
||||
///
|
||||
/// # Example
|
||||
/// ```
|
||||
/// # use nalgebra::{DualQuaternion, Quaternion};
|
||||
/// let rot = Quaternion::new(1.0, 2.0, 3.0, 4.0);
|
||||
///
|
||||
/// let dq = DualQuaternion::from_real(rot);
|
||||
/// assert_eq!(dq.real.w, 1.0);
|
||||
/// assert_eq!(dq.dual.w, 0.0);
|
||||
/// ```
|
||||
#[inline]
|
||||
pub fn from_real(real: Quaternion<N>) -> Self {
|
||||
Self {
|
||||
real,
|
||||
dual: Quaternion::zero(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: SimdRealField> One for DualQuaternion<N>
|
||||
where
|
||||
N::Element: SimdRealField,
|
||||
{
|
||||
#[inline]
|
||||
fn one() -> Self {
|
||||
Self::identity()
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: SimdRealField> Zero for DualQuaternion<N>
|
||||
where
|
||||
N::Element: SimdRealField,
|
||||
{
|
||||
#[inline]
|
||||
fn zero() -> Self {
|
||||
DualQuaternion::from_real_and_dual(Quaternion::zero(), Quaternion::zero())
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn is_zero(&self) -> bool {
|
||||
self.real.is_zero() && self.dual.is_zero()
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "arbitrary")]
|
||||
impl<N> Arbitrary for DualQuaternion<N>
|
||||
where
|
||||
N: SimdRealField + Arbitrary + Send,
|
||||
N::Element: SimdRealField,
|
||||
{
|
||||
#[inline]
|
||||
fn arbitrary<G: Gen>(rng: &mut G) -> Self {
|
||||
Self::from_real_and_dual(Arbitrary::arbitrary(rng), Arbitrary::arbitrary(rng))
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: SimdRealField> UnitDualQuaternion<N> {
|
||||
/// The unit dual quaternion multiplicative identity, which also represents
|
||||
/// the identity transformation as an isometry.
|
||||
///
|
||||
/// ```
|
||||
/// # use nalgebra::{UnitDualQuaternion, UnitQuaternion, Vector3, Point3};
|
||||
/// let ident = UnitDualQuaternion::identity();
|
||||
/// let point = Point3::new(1.0, -4.3, 3.33);
|
||||
///
|
||||
/// assert_eq!(ident * point, point);
|
||||
/// assert_eq!(ident, ident.inverse());
|
||||
/// ```
|
||||
#[inline]
|
||||
pub fn identity() -> Self {
|
||||
Self::new_unchecked(DualQuaternion::identity())
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: SimdRealField> UnitDualQuaternion<N>
|
||||
where
|
||||
N::Element: SimdRealField,
|
||||
{
|
||||
/// Return a dual quaternion representing the translation and orientation
|
||||
/// given by the provided rotation quaternion and translation vector.
|
||||
///
|
||||
/// ```
|
||||
/// # #[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 * point, Point3::new(1.0, 0.0, 2.0), epsilon = 1.0e-6);
|
||||
/// ```
|
||||
#[inline]
|
||||
pub fn from_parts(translation: Translation3<N>, rotation: UnitQuaternion<N>) -> Self {
|
||||
let half: N = crate::convert(0.5f64);
|
||||
UnitDualQuaternion::new_unchecked(DualQuaternion {
|
||||
real: rotation.clone().into_inner(),
|
||||
dual: Quaternion::from_parts(N::zero(), translation.vector)
|
||||
* rotation.clone().into_inner()
|
||||
* half,
|
||||
})
|
||||
}
|
||||
|
||||
/// Return a unit dual quaternion representing the translation and orientation
|
||||
/// given by the provided isometry.
|
||||
///
|
||||
/// ```
|
||||
/// # #[macro_use] extern crate approx;
|
||||
/// # use nalgebra::{Isometry3, UnitDualQuaternion, UnitQuaternion, Vector3, Point3};
|
||||
/// let iso = Isometry3::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 dq = UnitDualQuaternion::from_isometry(&iso);
|
||||
/// let point = Point3::new(1.0, 2.0, 3.0);
|
||||
///
|
||||
/// assert_relative_eq!(dq * point, iso * point, epsilon = 1.0e-6);
|
||||
/// ```
|
||||
#[inline]
|
||||
pub fn from_isometry(isometry: &Isometry3<N>) -> Self {
|
||||
UnitDualQuaternion::from_parts(isometry.translation, isometry.rotation)
|
||||
}
|
||||
|
||||
/// Creates a dual quaternion from a unit quaternion rotation.
|
||||
///
|
||||
/// # Example
|
||||
/// ```
|
||||
/// # #[macro_use] extern crate approx;
|
||||
/// # use nalgebra::{UnitQuaternion, UnitDualQuaternion, Quaternion};
|
||||
/// let q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
|
||||
/// let rot = UnitQuaternion::new_normalize(q);
|
||||
///
|
||||
/// let dq = UnitDualQuaternion::from_rotation(rot);
|
||||
/// assert_relative_eq!(dq.as_ref().real.norm(), 1.0, epsilon = 1.0e-6);
|
||||
/// assert_eq!(dq.as_ref().dual.norm(), 0.0);
|
||||
/// ```
|
||||
#[inline]
|
||||
pub fn from_rotation(rotation: UnitQuaternion<N>) -> Self {
|
||||
Self::new_unchecked(DualQuaternion::from_real(rotation.into_inner()))
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: SimdRealField> One for UnitDualQuaternion<N>
|
||||
where
|
||||
N::Element: SimdRealField,
|
||||
{
|
||||
#[inline]
|
||||
fn one() -> Self {
|
||||
Self::identity()
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "arbitrary")]
|
||||
impl<N> Arbitrary for UnitDualQuaternion<N>
|
||||
where
|
||||
N: SimdRealField + Arbitrary + Send,
|
||||
N::Element: SimdRealField,
|
||||
{
|
||||
#[inline]
|
||||
fn arbitrary<G: Gen>(rng: &mut G) -> Self {
|
||||
Self::new_normalize(Arbitrary::arbitrary(rng))
|
||||
}
|
||||
}
|
||||
|
||||
188
src/geometry/dual_quaternion_conversion.rs
Normal file
188
src/geometry/dual_quaternion_conversion.rs
Normal file
@ -0,0 +1,188 @@
|
||||
use simba::scalar::{RealField, SubsetOf, SupersetOf};
|
||||
use simba::simd::SimdRealField;
|
||||
|
||||
use crate::base::dimension::U3;
|
||||
use crate::base::{Matrix4, Vector4};
|
||||
use crate::geometry::{
|
||||
DualQuaternion, Isometry3, Similarity3, SuperTCategoryOf, TAffine, Transform, Translation3,
|
||||
UnitDualQuaternion, UnitQuaternion,
|
||||
};
|
||||
|
||||
/*
|
||||
* This file provides the following conversions:
|
||||
* =============================================
|
||||
*
|
||||
* DualQuaternion -> DualQuaternion
|
||||
* UnitDualQuaternion -> UnitDualQuaternion
|
||||
* UnitDualQuaternion -> Isometry<U3>
|
||||
* UnitDualQuaternion -> Similarity<U3>
|
||||
* UnitDualQuaternion -> Transform<U3>
|
||||
* UnitDualQuaternion -> Matrix<U4> (homogeneous)
|
||||
*
|
||||
* NOTE:
|
||||
* UnitDualQuaternion -> DualQuaternion is already provided by: Unit<T> -> T
|
||||
*/
|
||||
|
||||
impl<N1, N2> SubsetOf<DualQuaternion<N2>> for DualQuaternion<N1>
|
||||
where
|
||||
N1: SimdRealField,
|
||||
N2: SimdRealField + SupersetOf<N1>,
|
||||
{
|
||||
#[inline]
|
||||
fn to_superset(&self) -> DualQuaternion<N2> {
|
||||
DualQuaternion::from_real_and_dual(self.real.to_superset(), self.dual.to_superset())
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn is_in_subset(dq: &DualQuaternion<N2>) -> bool {
|
||||
crate::is_convertible::<_, Vector4<N1>>(&dq.real.coords)
|
||||
&& crate::is_convertible::<_, Vector4<N1>>(&dq.dual.coords)
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn from_superset_unchecked(dq: &DualQuaternion<N2>) -> Self {
|
||||
DualQuaternion::from_real_and_dual(
|
||||
dq.real.to_subset_unchecked(),
|
||||
dq.dual.to_subset_unchecked(),
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
impl<N1, N2> SubsetOf<UnitDualQuaternion<N2>> for UnitDualQuaternion<N1>
|
||||
where
|
||||
N1: SimdRealField,
|
||||
N2: SimdRealField + SupersetOf<N1>,
|
||||
{
|
||||
#[inline]
|
||||
fn to_superset(&self) -> UnitDualQuaternion<N2> {
|
||||
UnitDualQuaternion::new_unchecked(self.as_ref().to_superset())
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn is_in_subset(dq: &UnitDualQuaternion<N2>) -> bool {
|
||||
crate::is_convertible::<_, DualQuaternion<N1>>(dq.as_ref())
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn from_superset_unchecked(dq: &UnitDualQuaternion<N2>) -> Self {
|
||||
Self::new_unchecked(crate::convert_ref_unchecked(dq.as_ref()))
|
||||
}
|
||||
}
|
||||
|
||||
impl<N1, N2> SubsetOf<Isometry3<N2>> for UnitDualQuaternion<N1>
|
||||
where
|
||||
N1: RealField,
|
||||
N2: RealField + SupersetOf<N1>,
|
||||
{
|
||||
#[inline]
|
||||
fn to_superset(&self) -> Isometry3<N2> {
|
||||
let dq: UnitDualQuaternion<N2> = self.to_superset();
|
||||
let iso = dq.to_isometry();
|
||||
crate::convert_unchecked(iso)
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn is_in_subset(iso: &Isometry3<N2>) -> bool {
|
||||
crate::is_convertible::<_, UnitQuaternion<N1>>(&iso.rotation)
|
||||
&& crate::is_convertible::<_, Translation3<N1>>(&iso.translation)
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn from_superset_unchecked(iso: &Isometry3<N2>) -> Self {
|
||||
let dq = UnitDualQuaternion::<N2>::from_isometry(iso);
|
||||
crate::convert_unchecked(dq)
|
||||
}
|
||||
}
|
||||
|
||||
impl<N1, N2> SubsetOf<Similarity3<N2>> for UnitDualQuaternion<N1>
|
||||
where
|
||||
N1: RealField,
|
||||
N2: RealField + SupersetOf<N1>,
|
||||
{
|
||||
#[inline]
|
||||
fn to_superset(&self) -> Similarity3<N2> {
|
||||
Similarity3::from_isometry(crate::convert_ref(self), N2::one())
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn is_in_subset(sim: &Similarity3<N2>) -> bool {
|
||||
sim.scaling() == N2::one()
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn from_superset_unchecked(sim: &Similarity3<N2>) -> Self {
|
||||
crate::convert_ref_unchecked(&sim.isometry)
|
||||
}
|
||||
}
|
||||
|
||||
impl<N1, N2, C> SubsetOf<Transform<N2, U3, C>> for UnitDualQuaternion<N1>
|
||||
where
|
||||
N1: RealField,
|
||||
N2: RealField + SupersetOf<N1>,
|
||||
C: SuperTCategoryOf<TAffine>,
|
||||
{
|
||||
#[inline]
|
||||
fn to_superset(&self) -> Transform<N2, U3, C> {
|
||||
Transform::from_matrix_unchecked(self.to_homogeneous().to_superset())
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn is_in_subset(t: &Transform<N2, U3, C>) -> bool {
|
||||
<Self as SubsetOf<_>>::is_in_subset(t.matrix())
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn from_superset_unchecked(t: &Transform<N2, U3, C>) -> Self {
|
||||
Self::from_superset_unchecked(t.matrix())
|
||||
}
|
||||
}
|
||||
|
||||
impl<N1: RealField, N2: RealField + SupersetOf<N1>> SubsetOf<Matrix4<N2>>
|
||||
for UnitDualQuaternion<N1>
|
||||
{
|
||||
#[inline]
|
||||
fn to_superset(&self) -> Matrix4<N2> {
|
||||
self.to_homogeneous().to_superset()
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn is_in_subset(m: &Matrix4<N2>) -> bool {
|
||||
crate::is_convertible::<_, Isometry3<N1>>(m)
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn from_superset_unchecked(m: &Matrix4<N2>) -> Self {
|
||||
let iso: Isometry3<N1> = crate::convert_ref_unchecked(m);
|
||||
Self::from_isometry(&iso)
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: SimdRealField + RealField> From<UnitDualQuaternion<N>> for Matrix4<N>
|
||||
where
|
||||
N::Element: SimdRealField,
|
||||
{
|
||||
#[inline]
|
||||
fn from(dq: UnitDualQuaternion<N>) -> Self {
|
||||
dq.to_homogeneous()
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: SimdRealField> From<UnitDualQuaternion<N>> for Isometry3<N>
|
||||
where
|
||||
N::Element: SimdRealField,
|
||||
{
|
||||
#[inline]
|
||||
fn from(dq: UnitDualQuaternion<N>) -> Self {
|
||||
dq.to_isometry()
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: SimdRealField> From<Isometry3<N>> for UnitDualQuaternion<N>
|
||||
where
|
||||
N::Element: SimdRealField,
|
||||
{
|
||||
#[inline]
|
||||
fn from(iso: Isometry3<N>) -> Self {
|
||||
Self::from_isometry(&iso)
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@ -6,7 +6,8 @@ use crate::base::dimension::{DimMin, DimName, DimNameAdd, DimNameSum, U1};
|
||||
use crate::base::{DefaultAllocator, MatrixN, Scalar};
|
||||
|
||||
use crate::geometry::{
|
||||
AbstractRotation, Isometry, Similarity, SuperTCategoryOf, TAffine, Transform, Translation,
|
||||
AbstractRotation, Isometry, Isometry3, Similarity, SuperTCategoryOf, TAffine, Transform,
|
||||
Translation, UnitDualQuaternion, UnitQuaternion,
|
||||
};
|
||||
|
||||
/*
|
||||
@ -14,6 +15,7 @@ use crate::geometry::{
|
||||
* =============================================
|
||||
*
|
||||
* Isometry -> Isometry
|
||||
* Isometry3 -> UnitDualQuaternion
|
||||
* Isometry -> Similarity
|
||||
* Isometry -> Transform
|
||||
* Isometry -> Matrix (homogeneous)
|
||||
@ -47,6 +49,30 @@ where
|
||||
}
|
||||
}
|
||||
|
||||
impl<N1, N2> SubsetOf<UnitDualQuaternion<N2>> for Isometry3<N1>
|
||||
where
|
||||
N1: RealField,
|
||||
N2: RealField + SupersetOf<N1>,
|
||||
{
|
||||
#[inline]
|
||||
fn to_superset(&self) -> UnitDualQuaternion<N2> {
|
||||
let dq = UnitDualQuaternion::<N1>::from_isometry(self);
|
||||
dq.to_superset()
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn is_in_subset(dq: &UnitDualQuaternion<N2>) -> bool {
|
||||
crate::is_convertible::<_, UnitQuaternion<N1>>(&dq.rotation())
|
||||
&& crate::is_convertible::<_, Translation<N1, _>>(&dq.translation())
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn from_superset_unchecked(dq: &UnitDualQuaternion<N2>) -> Self {
|
||||
let dq: UnitDualQuaternion<N1> = crate::convert_ref_unchecked(dq);
|
||||
dq.to_isometry()
|
||||
}
|
||||
}
|
||||
|
||||
impl<N1, N2, D: DimName, R1, R2> SubsetOf<Similarity<N2, D, R2>> for Isometry<N1, D, R1>
|
||||
where
|
||||
N1: RealField,
|
||||
|
||||
@ -36,7 +36,10 @@ mod quaternion_ops;
|
||||
mod quaternion_simba;
|
||||
|
||||
mod dual_quaternion;
|
||||
#[cfg(feature = "alga")]
|
||||
mod dual_quaternion_alga;
|
||||
mod dual_quaternion_construction;
|
||||
mod dual_quaternion_conversion;
|
||||
mod dual_quaternion_ops;
|
||||
|
||||
mod unit_complex;
|
||||
|
||||
@ -10,7 +10,7 @@ use crate::base::dimension::U3;
|
||||
use crate::base::{Matrix3, Matrix4, Scalar, Vector4};
|
||||
use crate::geometry::{
|
||||
AbstractRotation, Isometry, Quaternion, Rotation, Rotation3, Similarity, SuperTCategoryOf,
|
||||
TAffine, Transform, Translation, UnitQuaternion,
|
||||
TAffine, Transform, Translation, UnitDualQuaternion, UnitQuaternion,
|
||||
};
|
||||
|
||||
/*
|
||||
@ -21,6 +21,7 @@ use crate::geometry::{
|
||||
* UnitQuaternion -> UnitQuaternion
|
||||
* UnitQuaternion -> Rotation<U3>
|
||||
* UnitQuaternion -> Isometry<U3>
|
||||
* UnitQuaternion -> UnitDualQuaternion
|
||||
* UnitQuaternion -> Similarity<U3>
|
||||
* UnitQuaternion -> Transform<U3>
|
||||
* UnitQuaternion -> Matrix<U4> (homogeneous)
|
||||
@ -121,6 +122,28 @@ where
|
||||
}
|
||||
}
|
||||
|
||||
impl<N1, N2> SubsetOf<UnitDualQuaternion<N2>> for UnitQuaternion<N1>
|
||||
where
|
||||
N1: RealField,
|
||||
N2: RealField + SupersetOf<N1>,
|
||||
{
|
||||
#[inline]
|
||||
fn to_superset(&self) -> UnitDualQuaternion<N2> {
|
||||
let q: UnitQuaternion<N2> = crate::convert_ref(self);
|
||||
UnitDualQuaternion::from_rotation(q)
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn is_in_subset(dq: &UnitDualQuaternion<N2>) -> bool {
|
||||
dq.translation().vector.is_zero()
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn from_superset_unchecked(dq: &UnitDualQuaternion<N2>) -> Self {
|
||||
crate::convert_unchecked(dq.rotation())
|
||||
}
|
||||
}
|
||||
|
||||
impl<N1, N2, R> SubsetOf<Similarity<N2, U3, R>> for UnitQuaternion<N1>
|
||||
where
|
||||
N1: RealField,
|
||||
|
||||
@ -12,7 +12,7 @@ use crate::base::{DefaultAllocator, Matrix2, Matrix3, Matrix4, MatrixN, Scalar};
|
||||
|
||||
use crate::geometry::{
|
||||
AbstractRotation, Isometry, Rotation, Rotation2, Rotation3, Similarity, SuperTCategoryOf,
|
||||
TAffine, Transform, Translation, UnitComplex, UnitQuaternion,
|
||||
TAffine, Transform, Translation, UnitComplex, UnitDualQuaternion, UnitQuaternion,
|
||||
};
|
||||
|
||||
/*
|
||||
@ -21,6 +21,7 @@ use crate::geometry::{
|
||||
*
|
||||
* Rotation -> Rotation
|
||||
* Rotation3 -> UnitQuaternion
|
||||
* Rotation3 -> UnitDualQuaternion
|
||||
* Rotation2 -> UnitComplex
|
||||
* Rotation -> Isometry
|
||||
* Rotation -> Similarity
|
||||
@ -75,6 +76,31 @@ where
|
||||
}
|
||||
}
|
||||
|
||||
impl<N1, N2> SubsetOf<UnitDualQuaternion<N2>> for Rotation3<N1>
|
||||
where
|
||||
N1: RealField,
|
||||
N2: RealField + SupersetOf<N1>,
|
||||
{
|
||||
#[inline]
|
||||
fn to_superset(&self) -> UnitDualQuaternion<N2> {
|
||||
let q = UnitQuaternion::<N1>::from_rotation_matrix(self);
|
||||
let dq = UnitDualQuaternion::from_rotation(q);
|
||||
dq.to_superset()
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn is_in_subset(dq: &UnitDualQuaternion<N2>) -> bool {
|
||||
crate::is_convertible::<_, UnitQuaternion<N1>>(&dq.rotation())
|
||||
&& dq.translation().vector.is_zero()
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn from_superset_unchecked(dq: &UnitDualQuaternion<N2>) -> Self {
|
||||
let dq: UnitDualQuaternion<N1> = crate::convert_ref_unchecked(dq);
|
||||
dq.rotation().to_rotation_matrix()
|
||||
}
|
||||
}
|
||||
|
||||
impl<N1, N2> SubsetOf<UnitComplex<N2>> for Rotation2<N1>
|
||||
where
|
||||
N1: RealField,
|
||||
|
||||
@ -9,6 +9,7 @@ use crate::base::{DefaultAllocator, MatrixN, Scalar, VectorN};
|
||||
|
||||
use crate::geometry::{
|
||||
AbstractRotation, Isometry, Similarity, SuperTCategoryOf, TAffine, Transform, Translation,
|
||||
Translation3, UnitDualQuaternion, UnitQuaternion,
|
||||
};
|
||||
|
||||
/*
|
||||
@ -17,6 +18,7 @@ use crate::geometry::{
|
||||
*
|
||||
* Translation -> Translation
|
||||
* Translation -> Isometry
|
||||
* Translation3 -> UnitDualQuaternion
|
||||
* Translation -> Similarity
|
||||
* Translation -> Transform
|
||||
* Translation -> Matrix (homogeneous)
|
||||
@ -69,6 +71,30 @@ where
|
||||
}
|
||||
}
|
||||
|
||||
impl<N1, N2> SubsetOf<UnitDualQuaternion<N2>> for Translation3<N1>
|
||||
where
|
||||
N1: RealField,
|
||||
N2: RealField + SupersetOf<N1>,
|
||||
{
|
||||
#[inline]
|
||||
fn to_superset(&self) -> UnitDualQuaternion<N2> {
|
||||
let dq = UnitDualQuaternion::<N1>::from_parts(self.clone(), UnitQuaternion::identity());
|
||||
dq.to_superset()
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn is_in_subset(dq: &UnitDualQuaternion<N2>) -> bool {
|
||||
crate::is_convertible::<_, Translation<N1, _>>(&dq.translation())
|
||||
&& dq.rotation() == UnitQuaternion::identity()
|
||||
}
|
||||
|
||||
#[inline]
|
||||
fn from_superset_unchecked(dq: &UnitDualQuaternion<N2>) -> Self {
|
||||
let dq: UnitDualQuaternion<N1> = crate::convert_ref_unchecked(dq);
|
||||
dq.translation()
|
||||
}
|
||||
}
|
||||
|
||||
impl<N1, N2, D: DimName, R> SubsetOf<Similarity<N2, D, R>> for Translation<N1, D>
|
||||
where
|
||||
N1: RealField,
|
||||
|
||||
204
tests/geometry/dual_quaternion.rs
Normal file
204
tests/geometry/dual_quaternion.rs
Normal file
@ -0,0 +1,204 @@
|
||||
#![cfg(feature = "arbitrary")]
|
||||
#![allow(non_snake_case)]
|
||||
|
||||
use na::{
|
||||
DualQuaternion, Isometry3, Point3, Translation3, UnitDualQuaternion, UnitQuaternion, Vector3,
|
||||
};
|
||||
|
||||
quickcheck!(
|
||||
fn isometry_equivalence(iso: Isometry3<f64>, p: Point3<f64>, v: Vector3<f64>) -> bool {
|
||||
let dq = UnitDualQuaternion::from_isometry(&iso);
|
||||
|
||||
relative_eq!(iso * p, dq * p, epsilon = 1.0e-7)
|
||||
&& relative_eq!(iso * v, dq * v, epsilon = 1.0e-7)
|
||||
}
|
||||
|
||||
fn inverse_is_identity(i: UnitDualQuaternion<f64>, p: Point3<f64>, v: Vector3<f64>) -> bool {
|
||||
let ii = i.inverse();
|
||||
|
||||
relative_eq!(i * ii, UnitDualQuaternion::identity(), epsilon = 1.0e-7)
|
||||
&& relative_eq!(ii * i, UnitDualQuaternion::identity(), epsilon = 1.0e-7)
|
||||
&& relative_eq!((i * ii) * p, p, epsilon = 1.0e-7)
|
||||
&& relative_eq!((ii * i) * p, p, epsilon = 1.0e-7)
|
||||
&& relative_eq!((i * ii) * v, v, epsilon = 1.0e-7)
|
||||
&& relative_eq!((ii * i) * v, v, epsilon = 1.0e-7)
|
||||
}
|
||||
|
||||
#[cfg_attr(rustfmt, rustfmt_skip)]
|
||||
fn multiply_equals_alga_transform(
|
||||
dq: UnitDualQuaternion<f64>,
|
||||
v: Vector3<f64>,
|
||||
p: Point3<f64>
|
||||
) -> bool {
|
||||
dq * v == dq.transform_vector(&v)
|
||||
&& dq * p == dq.transform_point(&p)
|
||||
&& relative_eq!(
|
||||
dq.inverse() * v,
|
||||
dq.inverse_transform_vector(&v),
|
||||
epsilon = 1.0e-7
|
||||
)
|
||||
&& relative_eq!(
|
||||
dq.inverse() * p,
|
||||
dq.inverse_transform_point(&p),
|
||||
epsilon = 1.0e-7
|
||||
)
|
||||
}
|
||||
|
||||
#[cfg_attr(rustfmt, rustfmt_skip)]
|
||||
fn composition(
|
||||
dq: UnitDualQuaternion<f64>,
|
||||
uq: UnitQuaternion<f64>,
|
||||
t: Translation3<f64>,
|
||||
v: Vector3<f64>,
|
||||
p: Point3<f64>
|
||||
) -> bool {
|
||||
// (rotation × dual quaternion) * point = rotation × (dual quaternion * point)
|
||||
relative_eq!((uq * dq) * v, uq * (dq * v), epsilon = 1.0e-7) &&
|
||||
relative_eq!((uq * dq) * p, uq * (dq * p), epsilon = 1.0e-7) &&
|
||||
|
||||
// (dual quaternion × rotation) * point = dual quaternion × (rotation * point)
|
||||
relative_eq!((dq * uq) * v, dq * (uq * v), epsilon = 1.0e-7) &&
|
||||
relative_eq!((dq * uq) * p, dq * (uq * p), epsilon = 1.0e-7) &&
|
||||
|
||||
// (translation × dual quaternion) * point = translation × (dual quaternion * point)
|
||||
relative_eq!((t * dq) * v, (dq * v), epsilon = 1.0e-7) &&
|
||||
relative_eq!((t * dq) * p, t * (dq * p), epsilon = 1.0e-7) &&
|
||||
|
||||
// (dual quaternion × translation) * point = dual quaternion × (translation * point)
|
||||
relative_eq!((dq * t) * v, dq * v, epsilon = 1.0e-7) &&
|
||||
relative_eq!((dq * t) * p, dq * (t * p), epsilon = 1.0e-7)
|
||||
}
|
||||
|
||||
#[cfg_attr(rustfmt, rustfmt_skip)]
|
||||
fn all_op_exist(
|
||||
dq: DualQuaternion<f64>,
|
||||
udq: UnitDualQuaternion<f64>,
|
||||
uq: UnitQuaternion<f64>,
|
||||
s: f64,
|
||||
t: Translation3<f64>,
|
||||
v: Vector3<f64>,
|
||||
p: Point3<f64>
|
||||
) -> bool {
|
||||
let dqMs: DualQuaternion<_> = dq * s;
|
||||
|
||||
let dqMdq: DualQuaternion<_> = dq * dq;
|
||||
let dqMudq: DualQuaternion<_> = dq * udq;
|
||||
let udqMdq: DualQuaternion<_> = udq * dq;
|
||||
|
||||
let iMi: UnitDualQuaternion<_> = udq * udq;
|
||||
let iMuq: UnitDualQuaternion<_> = udq * uq;
|
||||
let iDi: UnitDualQuaternion<_> = udq / udq;
|
||||
let iDuq: UnitDualQuaternion<_> = udq / uq;
|
||||
|
||||
let iMp: Point3<_> = udq * p;
|
||||
let iMv: Vector3<_> = udq * v;
|
||||
|
||||
let iMt: UnitDualQuaternion<_> = udq * t;
|
||||
let tMi: UnitDualQuaternion<_> = t * udq;
|
||||
|
||||
let uqMi: UnitDualQuaternion<_> = uq * udq;
|
||||
let uqDi: UnitDualQuaternion<_> = uq / udq;
|
||||
|
||||
let mut dqMs1 = dq;
|
||||
|
||||
let mut dqMdq1 = dq;
|
||||
let mut dqMdq2 = dq;
|
||||
|
||||
let mut dqMudq1 = dq;
|
||||
let mut dqMudq2 = dq;
|
||||
|
||||
let mut iMt1 = udq;
|
||||
let mut iMt2 = udq;
|
||||
|
||||
let mut iMi1 = udq;
|
||||
let mut iMi2 = udq;
|
||||
|
||||
let mut iMuq1 = udq;
|
||||
let mut iMuq2 = udq;
|
||||
|
||||
let mut iDi1 = udq;
|
||||
let mut iDi2 = udq;
|
||||
|
||||
let mut iDuq1 = udq;
|
||||
let mut iDuq2 = udq;
|
||||
|
||||
dqMs1 *= s;
|
||||
|
||||
dqMdq1 *= dq;
|
||||
dqMdq2 *= &dq;
|
||||
|
||||
dqMudq1 *= udq;
|
||||
dqMudq2 *= &udq;
|
||||
|
||||
iMt1 *= t;
|
||||
iMt2 *= &t;
|
||||
|
||||
iMi1 *= udq;
|
||||
iMi2 *= &udq;
|
||||
|
||||
iMuq1 *= uq;
|
||||
iMuq2 *= &uq;
|
||||
|
||||
iDi1 /= udq;
|
||||
iDi2 /= &udq;
|
||||
|
||||
iDuq1 /= uq;
|
||||
iDuq2 /= &uq;
|
||||
|
||||
dqMs == dqMs1
|
||||
&& dqMdq == dqMdq1
|
||||
&& dqMdq == dqMdq2
|
||||
&& dqMudq == dqMudq1
|
||||
&& dqMudq == dqMudq2
|
||||
&& iMt == iMt1
|
||||
&& iMt == iMt2
|
||||
&& iMi == iMi1
|
||||
&& iMi == iMi2
|
||||
&& iMuq == iMuq1
|
||||
&& iMuq == iMuq2
|
||||
&& iDi == iDi1
|
||||
&& iDi == iDi2
|
||||
&& iDuq == iDuq1
|
||||
&& iDuq == iDuq2
|
||||
&& dqMs == &dq * s
|
||||
&& dqMdq == &dq * &dq
|
||||
&& dqMdq == dq * &dq
|
||||
&& dqMdq == &dq * dq
|
||||
&& dqMudq == &dq * &udq
|
||||
&& dqMudq == dq * &udq
|
||||
&& dqMudq == &dq * udq
|
||||
&& udqMdq == &udq * &dq
|
||||
&& udqMdq == udq * &dq
|
||||
&& udqMdq == &udq * dq
|
||||
&& iMi == &udq * &udq
|
||||
&& iMi == udq * &udq
|
||||
&& iMi == &udq * udq
|
||||
&& iMuq == &udq * &uq
|
||||
&& iMuq == udq * &uq
|
||||
&& iMuq == &udq * uq
|
||||
&& iDi == &udq / &udq
|
||||
&& iDi == udq / &udq
|
||||
&& iDi == &udq / udq
|
||||
&& iDuq == &udq / &uq
|
||||
&& iDuq == udq / &uq
|
||||
&& iDuq == &udq / uq
|
||||
&& iMp == &udq * &p
|
||||
&& iMp == udq * &p
|
||||
&& iMp == &udq * p
|
||||
&& iMv == &udq * &v
|
||||
&& iMv == udq * &v
|
||||
&& iMv == &udq * v
|
||||
&& iMt == &udq * &t
|
||||
&& iMt == udq * &t
|
||||
&& iMt == &udq * t
|
||||
&& tMi == &t * &udq
|
||||
&& tMi == t * &udq
|
||||
&& tMi == &t * udq
|
||||
&& uqMi == &uq * &udq
|
||||
&& uqMi == uq * &udq
|
||||
&& uqMi == &uq * udq
|
||||
&& uqDi == &uq / &udq
|
||||
&& uqDi == uq / &udq
|
||||
&& uqDi == &uq / udq
|
||||
}
|
||||
);
|
||||
@ -1,3 +1,4 @@
|
||||
mod dual_quaternion;
|
||||
mod isometry;
|
||||
mod point;
|
||||
mod projection;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user