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
parent
3e2ab0119e
commit
12c259f0b4
|
|
@ -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")]
|
||||
use serde::{Deserialize, Deserializer, Serialize, Serializer};
|
||||
|
||||
use simba::scalar::{ClosedNeg, RealField};
|
||||
|
||||
/// A dual quaternion.
|
||||
///
|
||||
/// # Indexing
|
||||
|
|
@ -77,8 +84,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 +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)
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
|
|
@ -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> {
|
||||
/// 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 {
|
||||
Self { real, dual }
|
||||
}
|
||||
/// The dual quaternion multiplicative identity
|
||||
|
||||
/// The dual quaternion multiplicative identity.
|
||||
///
|
||||
/// # 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()
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -22,9 +22,16 @@
|
|||
* - 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::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> {
|
||||
#[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
|
||||
N::Element: SimdRealField,
|
||||
{
|
||||
type Output = DualQuaternion<N>;
|
||||
|
||||
fn mul(self, rhs: Self) -> Self::Output {
|
||||
Self::from_real_and_dual(
|
||||
self.real * rhs.real,
|
||||
self.real * rhs.dual + self.dual * rhs.real,
|
||||
#[inline]
|
||||
fn neg(self) -> Self::Output {
|
||||
DualQuaternion::from_real_and_dual(-self.real, -self.dual)
|
||||
}
|
||||
}
|
||||
|
||||
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<'b> Mul<&'b DualQuaternion<$T>> for $T {
|
||||
type Output = DualQuaternion<$T>;
|
||||
|
||||
#[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
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
);
|
||||
|
||||
impl<N: SimdRealField> Mul<N> for DualQuaternion<N>
|
||||
where
|
||||
N::Element: SimdRealField,
|
||||
{
|
||||
type Output = DualQuaternion<N>;
|
||||
// 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);
|
||||
|
||||
fn mul(self, rhs: N) -> Self::Output {
|
||||
Self::from_real_and_dual(self.real * rhs, self.dual * rhs)
|
||||
}
|
||||
}
|
||||
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;
|
||||
};);
|
||||
|
||||
impl<N: SimdRealField> Add<DualQuaternion<N>> for DualQuaternion<N>
|
||||
where
|
||||
N::Element: SimdRealField,
|
||||
{
|
||||
type Output = DualQuaternion<N>;
|
||||
// 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);
|
||||
|
||||
fn add(self, rhs: DualQuaternion<N>) -> Self::Output {
|
||||
Self::from_real_and_dual(self.real + rhs.real, self.dual + rhs.dual)
|
||||
}
|
||||
}
|
||||
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;
|
||||
};);
|
||||
|
||||
impl<N: SimdRealField> Sub<DualQuaternion<N>> for DualQuaternion<N>
|
||||
where
|
||||
N::Element: SimdRealField,
|
||||
{
|
||||
type Output = DualQuaternion<N>;
|
||||
// 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);
|
||||
|
||||
fn sub(self, rhs: DualQuaternion<N>) -> Self::Output {
|
||||
Self::from_real_and_dual(self.real - rhs.real, self.dual - rhs.dual)
|
||||
}
|
||||
}
|
||||
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>;
|
||||
|
||||
#[inline]
|
||||
fn $op(self, n: N) -> Self::Output {
|
||||
DualQuaternion::from_real_and_dual(
|
||||
self.real.$op(n),
|
||||
self.dual.$op(n)
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
impl<'a, N: SimdRealField> $Op<N> for &'a DualQuaternion<N>
|
||||
where N::Element: SimdRealField {
|
||||
type Output = DualQuaternion<N>;
|
||||
|
||||
#[inline]
|
||||
fn $op(self, n: N) -> Self::Output {
|
||||
DualQuaternion::from_real_and_dual(
|
||||
self.real.$op(n),
|
||||
self.dual.$op(n)
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: SimdRealField> $OpAssign<N> for DualQuaternion<N>
|
||||
where N::Element: SimdRealField {
|
||||
|
||||
#[inline]
|
||||
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;
|
||||
);
|
||||
|
|
|
|||
|
|
@ -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, UnitQuaternion, UnitDualQuaternion
|
||||
};
|
||||
|
||||
/*
|
||||
|
|
@ -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, UnitQuaternion, UnitDualQuaternion,
|
||||
};
|
||||
|
||||
/*
|
||||
|
|
@ -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,
|
||||
|
|
|
|||
Loading…
Reference in New Issue