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

Implement additional `DualQuaternion` ops and `UnitDualQuaternion` 

This implements `UnitDualQuaternion` as an alternative to `Isometry3`
for representing 3D isometries, which also provides the `sclerp`
operation which can be used to perform screw-linear interpolation
between two unit dual quaternions.
This commit is contained in:
Terence 2021-01-28 17:25:32 -05:00
parent 3e2ab0119e
commit 12c259f0b4
10 changed files with 2329 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

815
src/geometry/dual_quaternion.rs
View File

@ -1,7 +1,14 @@
use crate::{Quaternion, SimdRealField}; use std::fmt;
use approx::{AbsDiffEq, RelativeEq, UlpsEq};
use crate::{
Quaternion, SimdRealField, VectorN, U8, Vector3, Point3, Isometry3, Unit, Matrix4,
Translation3, UnitQuaternion, Scalar, Normed
};
#[cfg(feature = "serde-serialize")] #[cfg(feature = "serde-serialize")]
use serde::{Deserialize, Deserializer, Serialize, Serializer}; use serde::{Deserialize, Deserializer, Serialize, Serializer};
use simba::scalar::{ClosedNeg, RealField};
/// A dual quaternion. /// A dual quaternion.
/// ///
/// # Indexing /// # Indexing
@ -77,8 +84,147 @@ where
/// relative_eq!(dq.real.norm(), 1.0); /// relative_eq!(dq.real.norm(), 1.0);
/// ``` /// ```
#[inline] #[inline]
pub fn normalize_mut(&mut self) { pub fn normalize_mut(&mut self) -> N {
*self = self.normalize(); 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 +260,666 @@ 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.
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 iso1 = UnitDualQuaternion::new_normalize(DualQuaternion::from_real_and_dual(qr, qd));
/// let iso2 = UnitDualQuaternion::new_normalize(DualQuaternion::from_real_and_dual(qd, qr));
/// let iso_to = iso1.isometry_to(&iso2);
/// assert_relative_eq!(iso_to * iso1, iso2, 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 isometry 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)
}
}

316
src/geometry/dual_quaternion_alga.rs Normal file
View File

@ -0,0 +1,316 @@
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::{
Point3, Quaternion, UnitQuaternion, DualQuaternion, UnitDualQuaternion, Translation3
};
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);

159
src/geometry/dual_quaternion_construction.rs
View File

@ -1,4 +1,8 @@
use crate::{DualQuaternion, Quaternion, SimdRealField}; use crate::{
DualQuaternion, Quaternion, UnitDualQuaternion, SimdRealField, Isometry3,
Translation3, UnitQuaternion
};
use num::{One, Zero};
impl<N: SimdRealField> DualQuaternion<N> { impl<N: SimdRealField> DualQuaternion<N> {
/// Creates a dual quaternion from its rotation and translation components. /// Creates a dual quaternion from its rotation and translation components.
@ -16,7 +20,8 @@ impl<N: SimdRealField> DualQuaternion<N> {
pub fn from_real_and_dual(real: Quaternion<N>, dual: Quaternion<N>) -> Self { pub fn from_real_and_dual(real: Quaternion<N>, dual: Quaternion<N>) -> Self {
Self { real, dual } Self { real, dual }
} }
/// The dual quaternion multiplicative identity
/// The dual quaternion multiplicative identity.
/// ///
/// # Example /// # Example
/// ///
@ -40,3 +45,153 @@ impl<N: SimdRealField> DualQuaternion<N> {
) )
} }
} }
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()
}
}
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()
}
}

186
src/geometry/dual_quaternion_conversion.rs Normal file
View File

@ -0,0 +1,186 @@
use simba::scalar::{RealField, SubsetOf, SupersetOf};
use simba::simd::SimdRealField;
use crate::base::dimension::U3;
use crate::base::{Matrix4, Vector4};
use crate::geometry::{
Isometry3, DualQuaternion, Similarity3, SuperTCategoryOf,
TAffine, Transform, Translation3, UnitQuaternion, UnitDualQuaternion
};
/*
* 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)
}
}

766
src/geometry/dual_quaternion_ops.rs
View File

@ -22,9 +22,16 @@
* - https://cs.gmu.edu/~jmlien/teaching/cs451/uploads/Main/dual-quaternion.pdf * - https://cs.gmu.edu/~jmlien/teaching/cs451/uploads/Main/dual-quaternion.pdf
*/ */
use crate::{DualQuaternion, SimdRealField}; use crate::{
DualQuaternion, SimdRealField, Point3, Point, Vector3, Isometry3, Quaternion,
UnitDualQuaternion, UnitQuaternion, U1, U3, U4, Unit, Allocator,
DefaultAllocator, Vector
};
use crate::base::storage::Storage;
use std::mem; use std::mem;
use std::ops::{Add, Index, IndexMut, Mul, Sub}; use std::ops::{
Add, AddAssign, Div, DivAssign, Index, IndexMut, Mul, MulAssign, Neg, Sub, SubAssign
};
impl<N: SimdRealField> AsRef<[N; 8]> for DualQuaternion<N> { impl<N: SimdRealField> AsRef<[N; 8]> for DualQuaternion<N> {
#[inline] #[inline]
@ -56,49 +63,758 @@ impl<N: SimdRealField> IndexMut<usize> for DualQuaternion<N> {
} }
} }
impl<N: SimdRealField> Mul<DualQuaternion<N>> for DualQuaternion<N> impl<N: SimdRealField> Neg for DualQuaternion<N>
where where
N::Element: SimdRealField, N::Element: SimdRealField,
{ {
type Output = DualQuaternion<N>; type Output = DualQuaternion<N>;
fn mul(self, rhs: Self) -> Self::Output { #[inline]
Self::from_real_and_dual( fn neg(self) -> Self::Output {
self.real * rhs.real, DualQuaternion::from_real_and_dual(-self.real, -self.dual)
self.real * rhs.dual + self.dual * rhs.real, }
}
impl<'a, N: SimdRealField> Neg for &'a DualQuaternion<N>
where
N::Element: SimdRealField,
{
type Output = DualQuaternion<N>;
#[inline]
fn neg(self) -> Self::Output {
DualQuaternion::from_real_and_dual(-&self.real, -&self.dual)
}
}
impl<N: SimdRealField> Neg for UnitDualQuaternion<N>
where
N::Element: SimdRealField,
{
type Output = UnitDualQuaternion<N>;
#[inline]
fn neg(self) -> Self::Output {
UnitDualQuaternion::new_unchecked(-self.into_inner())
}
}
impl<'a, N: SimdRealField> Neg for &'a UnitDualQuaternion<N>
where
N::Element: SimdRealField,
{
type Output = UnitDualQuaternion<N>;
#[inline]
fn neg(self) -> Self::Output {
UnitDualQuaternion::new_unchecked(-self.as_ref())
}
}
macro_rules! dual_quaternion_op_impl(
($Op: ident, $op: ident;
($LhsRDim: ident, $LhsCDim: ident), ($RhsRDim: ident, $RhsCDim: ident)
$(for $Storage: ident: $StoragesBound: ident $(<$($BoundParam: ty),*>)*),*;
$lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Result: ty $(=> $VDimA: ty, $VDimB: ty)*;
$action: expr; $($lives: tt),*) => {
impl<$($lives ,)* N: SimdRealField $(, $Storage: $StoragesBound $(<$($BoundParam),*>)*)*> $Op<$Rhs> for $Lhs
where N::Element: SimdRealField,
DefaultAllocator: Allocator<N, $LhsRDim, $LhsCDim> +
Allocator<N, $RhsRDim, $RhsCDim> {
type Output = $Result;
#[inline]
fn $op($lhs, $rhs: $Rhs) -> Self::Output {
$action
}
}
}
);
// DualQuaternion + DualQuaternion
dual_quaternion_op_impl!(
Add, add;
(U4, U1), (U4, U1);
self: &'a DualQuaternion<N>, rhs: &'b DualQuaternion<N>, Output = DualQuaternion<N>;
DualQuaternion::from_real_and_dual(
&self.real + &rhs.real,
&self.dual + &rhs.dual,
);
'a, 'b);
dual_quaternion_op_impl!(
Add, add;
(U4, U1), (U4, U1);
self: &'a DualQuaternion<N>, rhs: DualQuaternion<N>, Output = DualQuaternion<N>;
DualQuaternion::from_real_and_dual(
&self.real + rhs.real,
&self.dual + rhs.dual,
);
'a);
dual_quaternion_op_impl!(
Add, add;
(U4, U1), (U4, U1);
self: DualQuaternion<N>, rhs: &'b DualQuaternion<N>, Output = DualQuaternion<N>;
DualQuaternion::from_real_and_dual(
self.real + &rhs.real,
self.dual + &rhs.dual,
);
'b);
dual_quaternion_op_impl!(
Add, add;
(U4, U1), (U4, U1);
self: DualQuaternion<N>, rhs: DualQuaternion<N>, Output = DualQuaternion<N>;
DualQuaternion::from_real_and_dual(
self.real + rhs.real,
self.dual + rhs.dual,
); );
// DualQuaternion - DualQuaternion
dual_quaternion_op_impl!(
Sub, sub;
(U4, U1), (U4, U1);
self: &'a DualQuaternion<N>, rhs: &'b DualQuaternion<N>, Output = DualQuaternion<N>;
DualQuaternion::from_real_and_dual(
&self.real - &rhs.real,
&self.dual - &rhs.dual,
);
'a, 'b);
dual_quaternion_op_impl!(
Sub, sub;
(U4, U1), (U4, U1);
self: &'a DualQuaternion<N>, rhs: DualQuaternion<N>, Output = DualQuaternion<N>;
DualQuaternion::from_real_and_dual(
&self.real - rhs.real,
&self.dual - rhs.dual,
);
'a);
dual_quaternion_op_impl!(
Sub, sub;
(U4, U1), (U4, U1);
self: DualQuaternion<N>, rhs: &'b DualQuaternion<N>, Output = DualQuaternion<N>;
DualQuaternion::from_real_and_dual(
self.real - &rhs.real,
self.dual - &rhs.dual,
);
'b);
dual_quaternion_op_impl!(
Sub, sub;
(U4, U1), (U4, U1);
self: DualQuaternion<N>, rhs: DualQuaternion<N>, Output = DualQuaternion<N>;
DualQuaternion::from_real_and_dual(
self.real - rhs.real,
self.dual - rhs.dual,
); );
// DualQuaternion × DualQuaternion
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U4, U1);
self: &'a DualQuaternion<N>, rhs: &'b DualQuaternion<N>, Output = DualQuaternion<N>;
DualQuaternion::from_real_and_dual(
&self.real * &rhs.real,
&self.real * &rhs.dual + &self.dual * &rhs.real,
);
'a, 'b);
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U4, U1);
self: &'a DualQuaternion<N>, rhs: DualQuaternion<N>, Output = DualQuaternion<N>;
self * &rhs;
'a);
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U4, U1);
self: DualQuaternion<N>, rhs: &'b DualQuaternion<N>, Output = DualQuaternion<N>;
&self * rhs;
'b);
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U4, U1);
self: DualQuaternion<N>, rhs: DualQuaternion<N>, Output = DualQuaternion<N>;
&self * &rhs; );
// UnitDualQuaternion × UnitDualQuaternion
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U4, U1);
self: &'a UnitDualQuaternion<N>, rhs: &'b UnitDualQuaternion<N>, Output = UnitDualQuaternion<N>;
UnitDualQuaternion::new_unchecked(self.as_ref() * rhs.as_ref());
'a, 'b);
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U4, U1);
self: &'a UnitDualQuaternion<N>, rhs: UnitDualQuaternion<N>, Output = UnitDualQuaternion<N>;
self * &rhs;
'a);
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U4, U1);
self: UnitDualQuaternion<N>, rhs: &'b UnitDualQuaternion<N>, Output = UnitDualQuaternion<N>;
&self * rhs;
'b);
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U4, U1);
self: UnitDualQuaternion<N>, rhs: UnitDualQuaternion<N>, Output = UnitDualQuaternion<N>;
&self * &rhs; );
// UnitDualQuaternion ÷ UnitDualQuaternion
dual_quaternion_op_impl!(
Div, div;
(U4, U1), (U4, U1);
self: &'a UnitDualQuaternion<N>, rhs: &'b UnitDualQuaternion<N>, Output = UnitDualQuaternion<N>;
#[allow(clippy::suspicious_arithmetic_impl)] { self * rhs.inverse() };
'a, 'b);
dual_quaternion_op_impl!(
Div, div;
(U4, U1), (U4, U1);
self: &'a UnitDualQuaternion<N>, rhs: UnitDualQuaternion<N>, Output = UnitDualQuaternion<N>;
self / &rhs;
'a);
dual_quaternion_op_impl!(
Div, div;
(U4, U1), (U4, U1);
self: UnitDualQuaternion<N>, rhs: &'b UnitDualQuaternion<N>, Output = UnitDualQuaternion<N>;
&self / rhs;
'b);
dual_quaternion_op_impl!(
Div, div;
(U4, U1), (U4, U1);
self: UnitDualQuaternion<N>, rhs: UnitDualQuaternion<N>, Output = UnitDualQuaternion<N>;
&self / &rhs; );
// UnitDualQuaternion × UnitQuaternion
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U4, U1);
self: &'a UnitDualQuaternion<N>, rhs: &'b UnitQuaternion<N>,
Output = UnitDualQuaternion<N> => U1, U4;
self * UnitDualQuaternion::<N>::new_unchecked(DualQuaternion::from_real(rhs.into_inner()));
'a, 'b);
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U4, U1);
self: &'a UnitDualQuaternion<N>, rhs: UnitQuaternion<N>,
Output = UnitDualQuaternion<N> => U3, U3;
self * UnitDualQuaternion::<N>::new_unchecked(DualQuaternion::from_real(rhs.into_inner()));
'a);
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U4, U1);
self: UnitDualQuaternion<N>, rhs: &'b UnitQuaternion<N>,
Output = UnitDualQuaternion<N> => U3, U3;
self * UnitDualQuaternion::<N>::new_unchecked(DualQuaternion::from_real(rhs.into_inner()));
'b);
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U4, U1);
self: UnitDualQuaternion<N>, rhs: UnitQuaternion<N>,
Output = UnitDualQuaternion<N> => U3, U3;
self * UnitDualQuaternion::<N>::new_unchecked(DualQuaternion::from_real(rhs.into_inner())););
// UnitQuaternion × UnitDualQuaternion
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U4, U1);
self: &'a UnitQuaternion<N>, rhs: &'b UnitDualQuaternion<N>,
Output = UnitDualQuaternion<N> => U1, U4;
UnitDualQuaternion::<N>::new_unchecked(DualQuaternion::from_real(self.into_inner())) * rhs;
'a, 'b);
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U4, U1);
self: &'a UnitQuaternion<N>, rhs: UnitDualQuaternion<N>,
Output = UnitDualQuaternion<N> => U3, U3;
UnitDualQuaternion::<N>::new_unchecked(DualQuaternion::from_real(self.into_inner())) * rhs;
'a);
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U4, U1);
self: UnitQuaternion<N>, rhs: &'b UnitDualQuaternion<N>,
Output = UnitDualQuaternion<N> => U3, U3;
UnitDualQuaternion::<N>::new_unchecked(DualQuaternion::from_real(self.into_inner())) * rhs;
'b);
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U4, U1);
self: UnitQuaternion<N>, rhs: UnitDualQuaternion<N>,
Output = UnitDualQuaternion<N> => U3, U3;
UnitDualQuaternion::<N>::new_unchecked(DualQuaternion::from_real(self.into_inner())) * rhs;);
// UnitDualQuaternion × Isometry3
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U3, U1);
self: &'a UnitDualQuaternion<N>, rhs: &'b Isometry3<N>,
Output = UnitDualQuaternion<N> => U3, U1;
self * UnitDualQuaternion::<N>::from_isometry(rhs);
'a, 'b);
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U3, U3);
self: &'a UnitDualQuaternion<N>, rhs: Isometry3<N>,
Output = UnitDualQuaternion<N> => U3, U1;
self * UnitDualQuaternion::<N>::from_isometry(&rhs);
'a);
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U3, U3);
self: UnitDualQuaternion<N>, rhs: &'b Isometry3<N>,
Output = UnitDualQuaternion<N> => U3, U1;
self * UnitDualQuaternion::<N>::from_isometry(rhs);
'b);
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U3, U3);
self: UnitDualQuaternion<N>, rhs: Isometry3<N>,
Output = UnitDualQuaternion<N> => U3, U1;
self * UnitDualQuaternion::<N>::from_isometry(&rhs); );
// UnitDualQuaternion ÷ Isometry
dual_quaternion_op_impl!(
Div, div;
(U4, U1), (U3, U1);
self: &'a UnitDualQuaternion<N>, rhs: &'b Isometry3<N>,
Output = UnitDualQuaternion<N> => U3, U1;
// TODO: can we avoid the conversion to a rotation matrix?
self / UnitDualQuaternion::<N>::from_isometry(rhs);
'a, 'b);
dual_quaternion_op_impl!(
Div, div;
(U4, U1), (U3, U3);
self: &'a UnitDualQuaternion<N>, rhs: Isometry3<N>,
Output = UnitDualQuaternion<N> => U3, U1;
self / UnitDualQuaternion::<N>::from_isometry(&rhs);
'a);
dual_quaternion_op_impl!(
Div, div;
(U4, U1), (U3, U3);
self: UnitDualQuaternion<N>, rhs: &'b Isometry3<N>,
Output = UnitDualQuaternion<N> => U3, U1;
self / UnitDualQuaternion::<N>::from_isometry(rhs);
'b);
dual_quaternion_op_impl!(
Div, div;
(U4, U1), (U3, U3);
self: UnitDualQuaternion<N>, rhs: Isometry3<N>,
Output = UnitDualQuaternion<N> => U3, U1;
self / UnitDualQuaternion::<N>::from_isometry(&rhs); );
// Isometry × UnitDualQuaternion
dual_quaternion_op_impl!(
Mul, mul;
(U3, U1), (U4, U1);
self: &'a Isometry3<N>, rhs: &'b UnitDualQuaternion<N>,
Output = UnitDualQuaternion<N> => U3, U1;
UnitDualQuaternion::<N>::from_isometry(self) * rhs;
'a, 'b);
dual_quaternion_op_impl!(
Mul, mul;
(U3, U1), (U4, U1);
self: &'a Isometry3<N>, rhs: UnitDualQuaternion<N>,
Output = UnitDualQuaternion<N> => U3, U1;
UnitDualQuaternion::<N>::from_isometry(self) * rhs;
'a);
dual_quaternion_op_impl!(
Mul, mul;
(U3, U1), (U4, U1);
self: Isometry3<N>, rhs: &'b UnitDualQuaternion<N>,
Output = UnitDualQuaternion<N> => U3, U1;
UnitDualQuaternion::<N>::from_isometry(&self) * rhs;
'b);
dual_quaternion_op_impl!(
Mul, mul;
(U3, U1), (U4, U1);
self: Isometry3<N>, rhs: UnitDualQuaternion<N>,
Output = UnitDualQuaternion<N> => U3, U1;
UnitDualQuaternion::<N>::from_isometry(&self) * rhs; );
// Isometry ÷ UnitDualQuaternion
dual_quaternion_op_impl!(
Div, div;
(U3, U1), (U4, U1);
self: &'a Isometry3<N>, rhs: &'b UnitDualQuaternion<N>,
Output = UnitDualQuaternion<N> => U3, U1;
// TODO: can we avoid the conversion from a rotation matrix?
UnitDualQuaternion::<N>::from_isometry(self) / rhs;
'a, 'b);
dual_quaternion_op_impl!(
Div, div;
(U3, U1), (U4, U1);
self: &'a Isometry3<N>, rhs: UnitDualQuaternion<N>,
Output = UnitDualQuaternion<N> => U3, U1;
UnitDualQuaternion::<N>::from_isometry(self) / rhs;
'a);
dual_quaternion_op_impl!(
Div, div;
(U3, U1), (U4, U1);
self: Isometry3<N>, rhs: &'b UnitDualQuaternion<N>,
Output = UnitDualQuaternion<N> => U3, U1;
UnitDualQuaternion::<N>::from_isometry(&self) / rhs;
'b);
dual_quaternion_op_impl!(
Div, div;
(U3, U1), (U4, U1);
self: Isometry3<N>, rhs: UnitDualQuaternion<N>,
Output = UnitDualQuaternion<N> => U3, U1;
UnitDualQuaternion::<N>::from_isometry(&self) / rhs; );
// UnitDualQuaternion × Vector
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U3, U1) for SB: Storage<N, U3> ;
self: &'a UnitDualQuaternion<N>, rhs: &'b Vector<N, U3, SB>,
Output = Vector3<N> => U3, U1;
Unit::new_unchecked(self.as_ref().real) * rhs;
'a, 'b);
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U3, U1) for SB: Storage<N, U3> ;
self: &'a UnitDualQuaternion<N>, rhs: Vector<N, U3, SB>,
Output = Vector3<N> => U3, U1;
self * &rhs;
'a);
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U3, U1) for SB: Storage<N, U3> ;
self: UnitDualQuaternion<N>, rhs: &'b Vector<N, U3, SB>,
Output = Vector3<N> => U3, U1;
&self * rhs;
'b);
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U3, U1) for SB: Storage<N, U3> ;
self: UnitDualQuaternion<N>, rhs: Vector<N, U3, SB>,
Output = Vector3<N> => U3, U1;
&self * &rhs; );
// UnitDualQuaternion × Point
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U3, U1);
self: &'a UnitDualQuaternion<N>, rhs: &'b Point3<N>,
Output = Point3<N> => U3, U1;
{
let two: N = crate::convert(2.0f64);
let q_point = Quaternion::from_parts(N::zero(), rhs.coords.clone());
Point::from(
((self.as_ref().real * q_point + self.as_ref().dual * two) * self.as_ref().real.conjugate())
.vector()
.into_owned(),
)
};
'a, 'b);
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U3, U1);
self: &'a UnitDualQuaternion<N>, rhs: Point3<N>,
Output = Point3<N> => U3, U1;
self * &rhs;
'a);
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U3, U1);
self: UnitDualQuaternion<N>, rhs: &'b Point3<N>,
Output = Point3<N> => U3, U1;
&self * rhs;
'b);
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U3, U1);
self: UnitDualQuaternion<N>, rhs: Point3<N>,
Output = Point3<N> => U3, U1;
&self * &rhs; );
// UnitDualQuaternion × Unit<Vector>
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U3, U1) for SB: Storage<N, U3> ;
self: &'a UnitDualQuaternion<N>, rhs: &'b Unit<Vector<N, U3, SB>>,
Output = Unit<Vector3<N>> => U3, U4;
Unit::new_unchecked(self * rhs.as_ref());
'a, 'b);
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U3, U1) for SB: Storage<N, U3> ;
self: &'a UnitDualQuaternion<N>, rhs: Unit<Vector<N, U3, SB>>,
Output = Unit<Vector3<N>> => U3, U4;
Unit::new_unchecked(self * rhs.into_inner());
'a);
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U3, U1) for SB: Storage<N, U3> ;
self: UnitDualQuaternion<N>, rhs: &'b Unit<Vector<N, U3, SB>>,
Output = Unit<Vector3<N>> => U3, U4;
Unit::new_unchecked(self * rhs.as_ref());
'b);
dual_quaternion_op_impl!(
Mul, mul;
(U4, U1), (U3, U1) for SB: Storage<N, U3> ;
self: UnitDualQuaternion<N>, rhs: Unit<Vector<N, U3, SB>>,
Output = Unit<Vector3<N>> => U3, U4;
Unit::new_unchecked(self * rhs.into_inner()); );
macro_rules! left_scalar_mul_impl(
($($T: ty),* $(,)*) => {$(
impl Mul<DualQuaternion<$T>> for $T {
type Output = DualQuaternion<$T>;
#[inline]
fn mul(self, right: DualQuaternion<$T>) -> Self::Output {
DualQuaternion::from_real_and_dual(
self * right.real,
self * right.dual
) )
} }
} }
impl<N: SimdRealField> Mul<N> for DualQuaternion<N> impl<'b> Mul<&'b DualQuaternion<$T>> for $T {
where type Output = DualQuaternion<$T>;
N::Element: SimdRealField,
#[inline]
fn mul(self, right: &'b DualQuaternion<$T>) -> Self::Output {
DualQuaternion::from_real_and_dual(
self * &right.real,
self * &right.dual
)
}
}
)*}
);
left_scalar_mul_impl!(f32, f64);
macro_rules! dual_quaternion_op_impl(
($OpAssign: ident, $op_assign: ident;
($LhsRDim: ident, $LhsCDim: ident), ($RhsRDim: ident, $RhsCDim: ident);
$lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty $(=> $VDimA: ty, $VDimB: ty)*;
$action: expr; $($lives: tt),*) => {
impl<$($lives ,)* N: SimdRealField> $OpAssign<$Rhs> for $Lhs
where N::Element: SimdRealField,
DefaultAllocator: Allocator<N, $LhsRDim, $LhsCDim> +
Allocator<N, $RhsRDim, $RhsCDim> {
#[inline]
fn $op_assign(&mut $lhs, $rhs: $Rhs) {
$action
}
}
}
);
// DualQuaternion += DualQuaternion
dual_quaternion_op_impl!(
AddAssign, add_assign;
(U4, U1), (U4, U1);
self: DualQuaternion<N>, rhs: &'b DualQuaternion<N>;
{ {
self.real += &rhs.real;
self.dual += &rhs.dual;
};
'b);
dual_quaternion_op_impl!(
AddAssign, add_assign;
(U4, U1), (U4, U1);
self: DualQuaternion<N>, rhs: DualQuaternion<N>;
{
self.real += rhs.real;
self.dual += rhs.dual;
};);
// DualQuaternion -= DualQuaternion
dual_quaternion_op_impl!(
SubAssign, sub_assign;
(U4, U1), (U4, U1);
self: DualQuaternion<N>, rhs: &'b DualQuaternion<N>;
{
self.real -= &rhs.real;
self.dual -= &rhs.dual;
};
'b);
dual_quaternion_op_impl!(
SubAssign, sub_assign;
(U4, U1), (U4, U1);
self: DualQuaternion<N>, rhs: DualQuaternion<N>;
{
self.real -= rhs.real;
self.dual -= rhs.dual;
};);
// DualQuaternion ×= DualQuaternion
dual_quaternion_op_impl!(
MulAssign, mul_assign;
(U4, U1), (U4, U1);
self: DualQuaternion<N>, rhs: &'b DualQuaternion<N>;
{
let res = &*self * rhs;
self.real.coords.copy_from(&res.real.coords);
self.dual.coords.copy_from(&res.dual.coords);
};
'b);
dual_quaternion_op_impl!(
MulAssign, mul_assign;
(U4, U1), (U4, U1);
self: DualQuaternion<N>, rhs: DualQuaternion<N>;
*self *= &rhs; );
// UnitDualQuaternion ×= UnitDualQuaternion
dual_quaternion_op_impl!(
MulAssign, mul_assign;
(U4, U1), (U4, U1);
self: UnitDualQuaternion<N>, rhs: &'b UnitDualQuaternion<N>;
{
let res = &*self * rhs;
self.as_mut_unchecked().real.coords.copy_from(&res.as_ref().real.coords);
self.as_mut_unchecked().dual.coords.copy_from(&res.as_ref().dual.coords);
};
'b);
dual_quaternion_op_impl!(
MulAssign, mul_assign;
(U4, U1), (U4, U1);
self: UnitDualQuaternion<N>, rhs: UnitDualQuaternion<N>;
*self *= &rhs; );
// UnitDualQuaternion ÷= UnitDualQuaternion
dual_quaternion_op_impl!(
DivAssign, div_assign;
(U4, U1), (U4, U1);
self: UnitDualQuaternion<N>, rhs: &'b UnitDualQuaternion<N>;
{
let res = &*self / rhs;
self.as_mut_unchecked().real.coords.copy_from(&res.as_ref().real.coords);
self.as_mut_unchecked().dual.coords.copy_from(&res.as_ref().dual.coords);
};
'b);
dual_quaternion_op_impl!(
DivAssign, div_assign;
(U4, U1), (U4, U1);
self: UnitDualQuaternion<N>, rhs: UnitDualQuaternion<N>;
*self /= &rhs; );
// UnitDualQuaternion ×= Isometry3
dual_quaternion_op_impl!(
MulAssign, mul_assign;
(U4, U1), (U3, U1);
self: UnitDualQuaternion<N>, rhs: &'b Isometry3<N> => U3, U1;
{
let res = &*self * rhs;
self.as_mut_unchecked().real.coords.copy_from(&res.as_ref().real.coords);
self.as_mut_unchecked().dual.coords.copy_from(&res.as_ref().dual.coords);
};
'b);
dual_quaternion_op_impl!(
MulAssign, mul_assign;
(U4, U1), (U3, U1);
self: UnitDualQuaternion<N>, rhs: Isometry3<N> => U3, U1;
*self *= &rhs; );
// UnitDualQuaternion ÷= Isometry3
dual_quaternion_op_impl!(
DivAssign, div_assign;
(U4, U1), (U3, U1);
self: UnitDualQuaternion<N>, rhs: &'b Isometry3<N> => U3, U1;
{
let res = &*self / rhs;
self.as_mut_unchecked().real.coords.copy_from(&res.as_ref().real.coords);
self.as_mut_unchecked().dual.coords.copy_from(&res.as_ref().dual.coords);
};
'b);
dual_quaternion_op_impl!(
DivAssign, div_assign;
(U4, U1), (U3, U1);
self: UnitDualQuaternion<N>, rhs: Isometry3<N> => U3, U1;
*self /= &rhs; );
macro_rules! scalar_op_impl(
($($Op: ident, $op: ident, $OpAssign: ident, $op_assign: ident);* $(;)*) => {$(
impl<N: SimdRealField> $Op<N> for DualQuaternion<N>
where N::Element: SimdRealField {
type Output = DualQuaternion<N>; type Output = DualQuaternion<N>;
fn mul(self, rhs: N) -> Self::Output { #[inline]
Self::from_real_and_dual(self.real * rhs, self.dual * rhs) fn $op(self, n: N) -> Self::Output {
DualQuaternion::from_real_and_dual(
self.real.$op(n),
self.dual.$op(n)
)
} }
} }
impl<N: SimdRealField> Add<DualQuaternion<N>> for DualQuaternion<N> impl<'a, N: SimdRealField> $Op<N> for &'a DualQuaternion<N>
where where N::Element: SimdRealField {
N::Element: SimdRealField,
{
type Output = DualQuaternion<N>; type Output = DualQuaternion<N>;
fn add(self, rhs: DualQuaternion<N>) -> Self::Output { #[inline]
Self::from_real_and_dual(self.real + rhs.real, self.dual + rhs.dual) fn $op(self, n: N) -> Self::Output {
DualQuaternion::from_real_and_dual(
self.real.$op(n),
self.dual.$op(n)
)
} }
} }
impl<N: SimdRealField> Sub<DualQuaternion<N>> for DualQuaternion<N> impl<N: SimdRealField> $OpAssign<N> for DualQuaternion<N>
where where N::Element: SimdRealField {
N::Element: SimdRealField,
{
type Output = DualQuaternion<N>;
fn sub(self, rhs: DualQuaternion<N>) -> Self::Output { #[inline]
Self::from_real_and_dual(self.real - rhs.real, self.dual - rhs.dual) fn $op_assign(&mut self, n: N) {
self.real.$op_assign(n);
self.dual.$op_assign(n);
} }
} }
)*}
);
scalar_op_impl!(
Mul, mul, MulAssign, mul_assign;
Div, div, DivAssign, div_assign;
);

28
src/geometry/isometry_conversion.rs
View File

@ -6,7 +6,8 @@ use crate::base::dimension::{DimMin, DimName, DimNameAdd, DimNameSum, U1};
use crate::base::{DefaultAllocator, MatrixN, Scalar}; use crate::base::{DefaultAllocator, MatrixN, Scalar};
use crate::geometry::{ 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 * Isometry -> Isometry
* Isometry3 -> UnitDualQuaternion
* Isometry -> Similarity * Isometry -> Similarity
* Isometry -> Transform * Isometry -> Transform
* Isometry -> Matrix (homogeneous) * 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> impl<N1, N2, D: DimName, R1, R2> SubsetOf<Similarity<N2, D, R2>> for Isometry<N1, D, R1>
where where
N1: RealField, N1: RealField,

3
src/geometry/mod.rs
View File

@ -36,7 +36,10 @@ mod quaternion_ops;
mod quaternion_simba; mod quaternion_simba;
mod dual_quaternion; mod dual_quaternion;
#[cfg(feature = "alga")]
mod dual_quaternion_alga;
mod dual_quaternion_construction; mod dual_quaternion_construction;
mod dual_quaternion_conversion;
mod dual_quaternion_ops; mod dual_quaternion_ops;
mod unit_complex; mod unit_complex;

25
src/geometry/quaternion_conversion.rs
View File

@ -10,7 +10,7 @@ use crate::base::dimension::U3;
use crate::base::{Matrix3, Matrix4, Scalar, Vector4}; use crate::base::{Matrix3, Matrix4, Scalar, Vector4};
use crate::geometry::{ use crate::geometry::{
AbstractRotation, Isometry, Quaternion, Rotation, Rotation3, Similarity, SuperTCategoryOf, AbstractRotation, Isometry, Quaternion, Rotation, Rotation3, Similarity, SuperTCategoryOf,
TAffine, Transform, Translation, UnitQuaternion, TAffine, Transform, Translation, UnitQuaternion, UnitDualQuaternion
}; };
/* /*
@ -21,6 +21,7 @@ use crate::geometry::{
* UnitQuaternion -> UnitQuaternion * UnitQuaternion -> UnitQuaternion
* UnitQuaternion -> Rotation<U3> * UnitQuaternion -> Rotation<U3>
* UnitQuaternion -> Isometry<U3> * UnitQuaternion -> Isometry<U3>
* UnitQuaternion -> UnitDualQuaternion
* UnitQuaternion -> Similarity<U3> * UnitQuaternion -> Similarity<U3>
* UnitQuaternion -> Transform<U3> * UnitQuaternion -> Transform<U3>
* UnitQuaternion -> Matrix<U4> (homogeneous) * 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> impl<N1, N2, R> SubsetOf<Similarity<N2, U3, R>> for UnitQuaternion<N1>
where where
N1: RealField, N1: RealField,

28
src/geometry/rotation_conversion.rs
View File

@ -12,7 +12,7 @@ use crate::base::{DefaultAllocator, Matrix2, Matrix3, Matrix4, MatrixN, Scalar};
use crate::geometry::{ use crate::geometry::{
AbstractRotation, Isometry, Rotation, Rotation2, Rotation3, Similarity, SuperTCategoryOf, AbstractRotation, Isometry, Rotation, Rotation2, Rotation3, Similarity, SuperTCategoryOf,
TAffine, Transform, Translation, UnitComplex, UnitQuaternion, TAffine, Transform, Translation, UnitComplex, UnitQuaternion, UnitDualQuaternion,
}; };
/* /*
@ -21,6 +21,7 @@ use crate::geometry::{
* *
* Rotation -> Rotation * Rotation -> Rotation
* Rotation3 -> UnitQuaternion * Rotation3 -> UnitQuaternion
* Rotation3 -> UnitDualQuaternion
* Rotation2 -> UnitComplex * Rotation2 -> UnitComplex
* Rotation -> Isometry * Rotation -> Isometry
* Rotation -> Similarity * 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> impl<N1, N2> SubsetOf<UnitComplex<N2>> for Rotation2<N1>
where where
N1: RealField, N1: RealField,

26
src/geometry/translation_conversion.rs
View File

@ -9,6 +9,7 @@ use crate::base::{DefaultAllocator, MatrixN, Scalar, VectorN};
use crate::geometry::{ use crate::geometry::{
AbstractRotation, Isometry, Similarity, SuperTCategoryOf, TAffine, Transform, Translation, AbstractRotation, Isometry, Similarity, SuperTCategoryOf, TAffine, Transform, Translation,
Translation3, UnitDualQuaternion, UnitQuaternion
}; };
/* /*
@ -17,6 +18,7 @@ use crate::geometry::{
* *
* Translation -> Translation * Translation -> Translation
* Translation -> Isometry * Translation -> Isometry
* Translation3 -> UnitDualQuaternion
* Translation -> Similarity * Translation -> Similarity
* Translation -> Transform * Translation -> Transform
* Translation -> Matrix (homogeneous) * 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> impl<N1, N2, D: DimName, R> SubsetOf<Similarity<N2, D, R>> for Translation<N1, D>
where where
N1: RealField, N1: RealField,