nalgebra/geometry/
dual_quaternion.rs

1// Needed otherwise the rkyv macros generate code incompatible with rust-2024
2#![cfg_attr(feature = "rkyv-serialize", allow(unsafe_op_in_unsafe_fn))]
3// The macros break if the references are taken out, for some reason.
4#![allow(clippy::op_ref)]
5
6#[cfg(feature = "rkyv-serialize")]
7use rkyv::bytecheck;
8
9use crate::{
10    Isometry3, Matrix4, Normed, OVector, Point3, Quaternion, Scalar, SimdRealField, Translation3,
11    U8, Unit, UnitQuaternion, Vector3, Zero,
12};
13use approx::{AbsDiffEq, RelativeEq, UlpsEq};
14#[cfg(feature = "serde-serialize-no-std")]
15use serde::{Deserialize, Deserializer, Serialize, Serializer};
16use std::fmt;
17
18use simba::scalar::{ClosedNeg, RealField};
19
20/// A dual quaternion.
21///
22/// # Indexing
23///
24/// `DualQuaternions` are stored as \[..real, ..dual\].
25/// Both of the quaternion components are laid out in `i, j, k, w` order.
26///
27/// # Example
28/// ```
29/// # use nalgebra::{DualQuaternion, Quaternion};
30///
31/// let real = Quaternion::new(1.0, 2.0, 3.0, 4.0);
32/// let dual = Quaternion::new(5.0, 6.0, 7.0, 8.0);
33///
34/// let dq = DualQuaternion::from_real_and_dual(real, dual);
35/// assert_eq!(dq[0], 2.0);
36/// assert_eq!(dq[1], 3.0);
37///
38/// assert_eq!(dq[4], 6.0);
39/// assert_eq!(dq[7], 5.0);
40/// ```
41///
42/// NOTE:
43///  As of December 2020, dual quaternion support is a work in progress.
44///  If a feature that you need is missing, feel free to open an issue or a PR.
45///  See <https://github.com/dimforge/nalgebra/issues/487>
46#[repr(C)]
47#[derive(Debug, Copy, Clone)]
48#[cfg_attr(
49    feature = "rkyv-serialize-no-std",
50    derive(rkyv::Archive, rkyv::Serialize, rkyv::Deserialize),
51    archive(
52        as = "DualQuaternion<T::Archived>",
53        bound(archive = "
54        T: rkyv::Archive,
55        Quaternion<T>: rkyv::Archive<Archived = Quaternion<T::Archived>>
56    ")
57    )
58)]
59#[cfg_attr(feature = "rkyv-serialize", derive(bytecheck::CheckBytes))]
60#[cfg_attr(feature = "defmt", derive(defmt::Format))]
61pub struct DualQuaternion<T> {
62    /// The real component of the quaternion
63    pub real: Quaternion<T>,
64    /// The dual component of the quaternion
65    pub dual: Quaternion<T>,
66}
67
68impl<T: Scalar + Eq> Eq for DualQuaternion<T> {}
69
70impl<T: Scalar> PartialEq for DualQuaternion<T> {
71    #[inline]
72    fn eq(&self, right: &Self) -> bool {
73        self.real == right.real && self.dual == right.dual
74    }
75}
76
77impl<T: Scalar + Zero> Default for DualQuaternion<T> {
78    fn default() -> Self {
79        Self {
80            real: Quaternion::default(),
81            dual: Quaternion::default(),
82        }
83    }
84}
85
86impl<T: SimdRealField> DualQuaternion<T>
87where
88    T::Element: SimdRealField,
89{
90    /// Normalizes this quaternion.
91    ///
92    /// # Example
93    /// ```
94    /// # #[macro_use] extern crate approx;
95    /// # use nalgebra::{DualQuaternion, Quaternion};
96    /// let real = Quaternion::new(1.0, 2.0, 3.0, 4.0);
97    /// let dual = Quaternion::new(5.0, 6.0, 7.0, 8.0);
98    /// let dq = DualQuaternion::from_real_and_dual(real, dual);
99    ///
100    /// let dq_normalized = dq.normalize();
101    ///
102    /// assert_relative_eq!(dq_normalized.real.norm(), 1.0);
103    /// ```
104    #[inline]
105    #[must_use = "Did you mean to use normalize_mut()?"]
106    pub fn normalize(&self) -> Self {
107        let real_norm = self.real.norm();
108
109        Self::from_real_and_dual(
110            self.real.clone() / real_norm.clone(),
111            self.dual.clone() / real_norm,
112        )
113    }
114
115    /// Normalizes this quaternion.
116    ///
117    /// # Example
118    /// ```
119    /// # #[macro_use] extern crate approx;
120    /// # use nalgebra::{DualQuaternion, Quaternion};
121    /// let real = Quaternion::new(1.0, 2.0, 3.0, 4.0);
122    /// let dual = Quaternion::new(5.0, 6.0, 7.0, 8.0);
123    /// let mut dq = DualQuaternion::from_real_and_dual(real, dual);
124    ///
125    /// dq.normalize_mut();
126    ///
127    /// assert_relative_eq!(dq.real.norm(), 1.0);
128    /// ```
129    #[inline]
130    pub fn normalize_mut(&mut self) -> T {
131        let real_norm = self.real.norm();
132        self.real /= real_norm.clone();
133        self.dual /= real_norm.clone();
134        real_norm
135    }
136
137    /// The conjugate of this dual quaternion, containing the conjugate of
138    /// the real and imaginary parts..
139    ///
140    /// # Example
141    /// ```
142    /// # use nalgebra::{DualQuaternion, Quaternion};
143    /// let real = Quaternion::new(1.0, 2.0, 3.0, 4.0);
144    /// let dual = Quaternion::new(5.0, 6.0, 7.0, 8.0);
145    /// let dq = DualQuaternion::from_real_and_dual(real, dual);
146    ///
147    /// let conj = dq.conjugate();
148    /// assert!(conj.real.i == -2.0 && conj.real.j == -3.0 && conj.real.k == -4.0);
149    /// assert!(conj.real.w == 1.0);
150    /// assert!(conj.dual.i == -6.0 && conj.dual.j == -7.0 && conj.dual.k == -8.0);
151    /// assert!(conj.dual.w == 5.0);
152    /// ```
153    #[inline]
154    #[must_use = "Did you mean to use conjugate_mut()?"]
155    pub fn conjugate(&self) -> Self {
156        Self::from_real_and_dual(self.real.conjugate(), self.dual.conjugate())
157    }
158
159    /// Replaces this quaternion by its conjugate.
160    ///
161    /// # Example
162    /// ```
163    /// # use nalgebra::{DualQuaternion, Quaternion};
164    /// let real = Quaternion::new(1.0, 2.0, 3.0, 4.0);
165    /// let dual = Quaternion::new(5.0, 6.0, 7.0, 8.0);
166    /// let mut dq = DualQuaternion::from_real_and_dual(real, dual);
167    ///
168    /// dq.conjugate_mut();
169    /// assert!(dq.real.i == -2.0 && dq.real.j == -3.0 && dq.real.k == -4.0);
170    /// assert!(dq.real.w == 1.0);
171    /// assert!(dq.dual.i == -6.0 && dq.dual.j == -7.0 && dq.dual.k == -8.0);
172    /// assert!(dq.dual.w == 5.0);
173    /// ```
174    #[inline]
175    pub fn conjugate_mut(&mut self) {
176        self.real.conjugate_mut();
177        self.dual.conjugate_mut();
178    }
179
180    /// Inverts this dual quaternion if it is not zero.
181    ///
182    /// # Example
183    /// ```
184    /// # #[macro_use] extern crate approx;
185    /// # use nalgebra::{DualQuaternion, Quaternion};
186    /// let real = Quaternion::new(1.0, 2.0, 3.0, 4.0);
187    /// let dual = Quaternion::new(5.0, 6.0, 7.0, 8.0);
188    /// let dq = DualQuaternion::from_real_and_dual(real, dual);
189    /// let inverse = dq.try_inverse();
190    ///
191    /// assert!(inverse.is_some());
192    /// assert_relative_eq!(inverse.unwrap() * dq, DualQuaternion::identity());
193    ///
194    /// //Non-invertible case
195    /// let zero = Quaternion::new(0.0, 0.0, 0.0, 0.0);
196    /// let dq = DualQuaternion::from_real_and_dual(zero, zero);
197    /// let inverse = dq.try_inverse();
198    ///
199    /// assert!(inverse.is_none());
200    /// ```
201    #[inline]
202    #[must_use = "Did you mean to use try_inverse_mut()?"]
203    pub fn try_inverse(&self) -> Option<Self>
204    where
205        T: RealField,
206    {
207        let mut res = self.clone();
208        if res.try_inverse_mut() {
209            Some(res)
210        } else {
211            None
212        }
213    }
214
215    /// Inverts this dual quaternion in-place if it is not zero.
216    ///
217    /// # Example
218    /// ```
219    /// # #[macro_use] extern crate approx;
220    /// # use nalgebra::{DualQuaternion, Quaternion};
221    /// let real = Quaternion::new(1.0, 2.0, 3.0, 4.0);
222    /// let dual = Quaternion::new(5.0, 6.0, 7.0, 8.0);
223    /// let dq = DualQuaternion::from_real_and_dual(real, dual);
224    /// let mut dq_inverse = dq;
225    /// dq_inverse.try_inverse_mut();
226    ///
227    /// assert_relative_eq!(dq_inverse * dq, DualQuaternion::identity());
228    ///
229    /// //Non-invertible case
230    /// let zero = Quaternion::new(0.0, 0.0, 0.0, 0.0);
231    /// let mut dq = DualQuaternion::from_real_and_dual(zero, zero);
232    /// assert!(!dq.try_inverse_mut());
233    /// ```
234    #[inline]
235    pub fn try_inverse_mut(&mut self) -> bool
236    where
237        T: RealField,
238    {
239        let inverted = self.real.try_inverse_mut();
240        if inverted {
241            self.dual = -self.real.clone() * self.dual.clone() * self.real.clone();
242            true
243        } else {
244            false
245        }
246    }
247
248    /// Linear interpolation between two dual quaternions.
249    ///
250    /// Computes `self * (1 - t) + other * t`.
251    ///
252    /// # Example
253    /// ```
254    /// # use nalgebra::{DualQuaternion, Quaternion};
255    /// let dq1 = DualQuaternion::from_real_and_dual(
256    ///     Quaternion::new(1.0, 0.0, 0.0, 4.0),
257    ///     Quaternion::new(0.0, 2.0, 0.0, 0.0)
258    /// );
259    /// let dq2 = DualQuaternion::from_real_and_dual(
260    ///     Quaternion::new(2.0, 0.0, 1.0, 0.0),
261    ///     Quaternion::new(0.0, 2.0, 0.0, 0.0)
262    /// );
263    /// assert_eq!(dq1.lerp(&dq2, 0.25), DualQuaternion::from_real_and_dual(
264    ///     Quaternion::new(1.25, 0.0, 0.25, 3.0),
265    ///     Quaternion::new(0.0, 2.0, 0.0, 0.0)
266    /// ));
267    /// ```
268    #[inline]
269    #[must_use]
270    pub fn lerp(&self, other: &Self, t: T) -> Self {
271        self * (T::one() - t.clone()) + other * t
272    }
273}
274
275#[cfg(feature = "bytemuck")]
276unsafe impl<T> bytemuck::Zeroable for DualQuaternion<T>
277where
278    T: Scalar + bytemuck::Zeroable,
279    Quaternion<T>: bytemuck::Zeroable,
280{
281}
282
283#[cfg(feature = "bytemuck")]
284unsafe impl<T> bytemuck::Pod for DualQuaternion<T>
285where
286    T: Scalar + bytemuck::Pod,
287    Quaternion<T>: bytemuck::Pod,
288{
289}
290
291#[cfg(feature = "serde-serialize-no-std")]
292impl<T: SimdRealField> Serialize for DualQuaternion<T>
293where
294    T: Serialize,
295{
296    fn serialize<S>(&self, serializer: S) -> Result<<S as Serializer>::Ok, <S as Serializer>::Error>
297    where
298        S: Serializer,
299    {
300        self.as_ref().serialize(serializer)
301    }
302}
303
304#[cfg(feature = "serde-serialize-no-std")]
305impl<'a, T: SimdRealField> Deserialize<'a> for DualQuaternion<T>
306where
307    T: Deserialize<'a>,
308{
309    fn deserialize<Des>(deserializer: Des) -> Result<Self, Des::Error>
310    where
311        Des: Deserializer<'a>,
312    {
313        type Dq<T> = [T; 8];
314
315        let dq: Dq<T> = Dq::<T>::deserialize(deserializer)?;
316
317        Ok(Self {
318            real: Quaternion::new(dq[3].clone(), dq[0].clone(), dq[1].clone(), dq[2].clone()),
319            dual: Quaternion::new(dq[7].clone(), dq[4].clone(), dq[5].clone(), dq[6].clone()),
320        })
321    }
322}
323
324impl<T: RealField> DualQuaternion<T> {
325    #[allow(clippy::wrong_self_convention)]
326    fn to_vector(&self) -> OVector<T, U8> {
327        self.as_ref().clone().into()
328    }
329}
330
331impl<T: RealField + AbsDiffEq<Epsilon = T>> AbsDiffEq for DualQuaternion<T> {
332    type Epsilon = T;
333
334    #[inline]
335    fn default_epsilon() -> Self::Epsilon {
336        T::default_epsilon()
337    }
338
339    #[inline]
340    fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
341        self.to_vector().abs_diff_eq(&other.to_vector(), epsilon.clone()) ||
342        // Account for the double-covering of S², i.e. q = -q
343        self.to_vector().iter().zip(other.to_vector().iter()).all(|(a, b)| a.abs_diff_eq(&-b.clone(), epsilon.clone()))
344    }
345}
346
347impl<T: RealField + RelativeEq<Epsilon = T>> RelativeEq for DualQuaternion<T> {
348    #[inline]
349    fn default_max_relative() -> Self::Epsilon {
350        T::default_max_relative()
351    }
352
353    #[inline]
354    fn relative_eq(
355        &self,
356        other: &Self,
357        epsilon: Self::Epsilon,
358        max_relative: Self::Epsilon,
359    ) -> bool {
360        self.to_vector().relative_eq(&other.to_vector(), epsilon.clone(), max_relative.clone()) ||
361        // Account for the double-covering of S², i.e. q = -q
362        self.to_vector().iter().zip(other.to_vector().iter()).all(|(a, b)| a.relative_eq(&-b.clone(), epsilon.clone(), max_relative.clone()))
363    }
364}
365
366impl<T: RealField + UlpsEq<Epsilon = T>> UlpsEq for DualQuaternion<T> {
367    #[inline]
368    fn default_max_ulps() -> u32 {
369        T::default_max_ulps()
370    }
371
372    #[inline]
373    fn ulps_eq(&self, other: &Self, epsilon: Self::Epsilon, max_ulps: u32) -> bool {
374        self.to_vector().ulps_eq(&other.to_vector(), epsilon.clone(), max_ulps) ||
375        // Account for the double-covering of S², i.e. q = -q.
376        self.to_vector().iter().zip(other.to_vector().iter()).all(|(a, b)| a.ulps_eq(&-b.clone(), epsilon.clone(), max_ulps))
377    }
378}
379
380/// A unit dual quaternion. May be used to represent a rotation followed by a
381/// translation.
382pub type UnitDualQuaternion<T> = Unit<DualQuaternion<T>>;
383
384impl<T: Scalar + ClosedNeg + PartialEq + SimdRealField> PartialEq for UnitDualQuaternion<T> {
385    #[inline]
386    fn eq(&self, rhs: &Self) -> bool {
387        self.as_ref().eq(rhs.as_ref())
388    }
389}
390
391impl<T: Scalar + ClosedNeg + Eq + SimdRealField> Eq for UnitDualQuaternion<T> {}
392
393impl<T: SimdRealField> Normed for DualQuaternion<T> {
394    type Norm = T::SimdRealField;
395
396    #[inline]
397    fn norm(&self) -> T::SimdRealField {
398        self.real.norm()
399    }
400
401    #[inline]
402    fn norm_squared(&self) -> T::SimdRealField {
403        self.real.norm_squared()
404    }
405
406    #[inline]
407    fn scale_mut(&mut self, n: Self::Norm) {
408        self.real.scale_mut(n.clone());
409        self.dual.scale_mut(n);
410    }
411
412    #[inline]
413    fn unscale_mut(&mut self, n: Self::Norm) {
414        self.real.unscale_mut(n.clone());
415        self.dual.unscale_mut(n);
416    }
417}
418
419impl<T: SimdRealField> UnitDualQuaternion<T>
420where
421    T::Element: SimdRealField,
422{
423    /// The underlying dual quaternion.
424    ///
425    /// Same as `self.as_ref()`.
426    ///
427    /// # Example
428    /// ```
429    /// # use nalgebra::{DualQuaternion, UnitDualQuaternion, Quaternion};
430    /// let id = UnitDualQuaternion::identity();
431    /// assert_eq!(*id.dual_quaternion(), DualQuaternion::from_real_and_dual(
432    ///     Quaternion::new(1.0, 0.0, 0.0, 0.0),
433    ///     Quaternion::new(0.0, 0.0, 0.0, 0.0)
434    /// ));
435    /// ```
436    #[inline]
437    #[must_use]
438    pub fn dual_quaternion(&self) -> &DualQuaternion<T> {
439        self.as_ref()
440    }
441
442    /// Compute the conjugate of this unit quaternion.
443    ///
444    /// # Example
445    /// ```
446    /// # use nalgebra::{UnitDualQuaternion, DualQuaternion, Quaternion};
447    /// let qr = Quaternion::new(1.0, 2.0, 3.0, 4.0);
448    /// let qd = Quaternion::new(5.0, 6.0, 7.0, 8.0);
449    /// let unit = UnitDualQuaternion::new_normalize(
450    ///     DualQuaternion::from_real_and_dual(qr, qd)
451    /// );
452    /// let conj = unit.conjugate();
453    /// assert_eq!(conj.real, unit.real.conjugate());
454    /// assert_eq!(conj.dual, unit.dual.conjugate());
455    /// ```
456    #[inline]
457    #[must_use = "Did you mean to use conjugate_mut()?"]
458    pub fn conjugate(&self) -> Self {
459        Self::new_unchecked(self.as_ref().conjugate())
460    }
461
462    /// Compute the conjugate of this unit quaternion in-place.
463    ///
464    /// # Example
465    /// ```
466    /// # use nalgebra::{UnitDualQuaternion, DualQuaternion, Quaternion};
467    /// let qr = Quaternion::new(1.0, 2.0, 3.0, 4.0);
468    /// let qd = Quaternion::new(5.0, 6.0, 7.0, 8.0);
469    /// let unit = UnitDualQuaternion::new_normalize(
470    ///     DualQuaternion::from_real_and_dual(qr, qd)
471    /// );
472    /// let mut conj = unit.clone();
473    /// conj.conjugate_mut();
474    /// assert_eq!(conj.as_ref().real, unit.as_ref().real.conjugate());
475    /// assert_eq!(conj.as_ref().dual, unit.as_ref().dual.conjugate());
476    /// ```
477    #[inline]
478    pub fn conjugate_mut(&mut self) {
479        self.as_mut_unchecked().conjugate_mut()
480    }
481
482    /// Inverts this dual quaternion if it is not zero.
483    ///
484    /// # Example
485    /// ```
486    /// # #[macro_use] extern crate approx;
487    /// # use nalgebra::{UnitDualQuaternion, Quaternion, DualQuaternion};
488    /// let qr = Quaternion::new(1.0, 2.0, 3.0, 4.0);
489    /// let qd = Quaternion::new(5.0, 6.0, 7.0, 8.0);
490    /// let unit = UnitDualQuaternion::new_normalize(DualQuaternion::from_real_and_dual(qr, qd));
491    /// let inv = unit.inverse();
492    /// assert_relative_eq!(unit * inv, UnitDualQuaternion::identity(), epsilon = 1.0e-6);
493    /// assert_relative_eq!(inv * unit, UnitDualQuaternion::identity(), epsilon = 1.0e-6);
494    /// ```
495    #[inline]
496    #[must_use = "Did you mean to use inverse_mut()?"]
497    pub fn inverse(&self) -> Self {
498        let real = Unit::new_unchecked(self.as_ref().real.clone())
499            .inverse()
500            .into_inner();
501        let dual = -real.clone() * self.as_ref().dual.clone() * real.clone();
502        UnitDualQuaternion::new_unchecked(DualQuaternion { real, dual })
503    }
504
505    /// Inverts this dual quaternion in place if it is not zero.
506    ///
507    /// # Example
508    /// ```
509    /// # #[macro_use] extern crate approx;
510    /// # use nalgebra::{UnitDualQuaternion, Quaternion, DualQuaternion};
511    /// let qr = Quaternion::new(1.0, 2.0, 3.0, 4.0);
512    /// let qd = Quaternion::new(5.0, 6.0, 7.0, 8.0);
513    /// let unit = UnitDualQuaternion::new_normalize(DualQuaternion::from_real_and_dual(qr, qd));
514    /// let mut inv = unit.clone();
515    /// inv.inverse_mut();
516    /// assert_relative_eq!(unit * inv, UnitDualQuaternion::identity(), epsilon = 1.0e-6);
517    /// assert_relative_eq!(inv * unit, UnitDualQuaternion::identity(), epsilon = 1.0e-6);
518    /// ```
519    #[inline]
520    pub fn inverse_mut(&mut self) {
521        let quat = self.as_mut_unchecked();
522        quat.real = Unit::new_unchecked(quat.real.clone())
523            .inverse()
524            .into_inner();
525        quat.dual = -quat.real.clone() * quat.dual.clone() * quat.real.clone();
526    }
527
528    /// The unit dual quaternion needed to make `self` and `other` coincide.
529    ///
530    /// The result is such that: `self.isometry_to(other) * self == other`.
531    ///
532    /// # Example
533    /// ```
534    /// # #[macro_use] extern crate approx;
535    /// # use nalgebra::{UnitDualQuaternion, DualQuaternion, Quaternion};
536    /// let qr = Quaternion::new(1.0, 2.0, 3.0, 4.0);
537    /// let qd = Quaternion::new(5.0, 6.0, 7.0, 8.0);
538    /// let dq1 = UnitDualQuaternion::new_normalize(DualQuaternion::from_real_and_dual(qr, qd));
539    /// let dq2 = UnitDualQuaternion::new_normalize(DualQuaternion::from_real_and_dual(qd, qr));
540    /// let dq_to = dq1.isometry_to(&dq2);
541    /// assert_relative_eq!(dq_to * dq1, dq2, epsilon = 1.0e-6);
542    /// ```
543    #[inline]
544    #[must_use]
545    pub fn isometry_to(&self, other: &Self) -> Self {
546        other / self
547    }
548
549    /// Linear interpolation between two unit dual quaternions.
550    ///
551    /// The result is not normalized.
552    ///
553    /// # Example
554    /// ```
555    /// # #[macro_use] extern crate approx;
556    /// # use nalgebra::{UnitDualQuaternion, DualQuaternion, Quaternion};
557    /// let dq1 = UnitDualQuaternion::new_normalize(DualQuaternion::from_real_and_dual(
558    ///     Quaternion::new(0.5, 0.0, 0.5, 0.0),
559    ///     Quaternion::new(0.0, 0.5, 0.0, 0.5)
560    /// ));
561    /// let dq2 = UnitDualQuaternion::new_normalize(DualQuaternion::from_real_and_dual(
562    ///     Quaternion::new(0.5, 0.0, 0.0, 0.5),
563    ///     Quaternion::new(0.5, 0.0, 0.5, 0.0)
564    /// ));
565    /// assert_relative_eq!(
566    ///     UnitDualQuaternion::new_normalize(dq1.lerp(&dq2, 0.5)),
567    ///     UnitDualQuaternion::new_normalize(
568    ///         DualQuaternion::from_real_and_dual(
569    ///             Quaternion::new(0.5, 0.0, 0.25, 0.25),
570    ///             Quaternion::new(0.25, 0.25, 0.25, 0.25)
571    ///         )
572    ///     ),
573    ///     epsilon = 1.0e-6
574    /// );
575    /// ```
576    #[inline]
577    #[must_use]
578    pub fn lerp(&self, other: &Self, t: T) -> DualQuaternion<T> {
579        self.as_ref().lerp(other.as_ref(), t)
580    }
581
582    /// Normalized linear interpolation between two unit quaternions.
583    ///
584    /// This is the same as `self.lerp` except that the result is normalized.
585    ///
586    /// # Example
587    /// ```
588    /// # #[macro_use] extern crate approx;
589    /// # use nalgebra::{UnitDualQuaternion, DualQuaternion, Quaternion};
590    /// let dq1 = UnitDualQuaternion::new_normalize(DualQuaternion::from_real_and_dual(
591    ///     Quaternion::new(0.5, 0.0, 0.5, 0.0),
592    ///     Quaternion::new(0.0, 0.5, 0.0, 0.5)
593    /// ));
594    /// let dq2 = UnitDualQuaternion::new_normalize(DualQuaternion::from_real_and_dual(
595    ///     Quaternion::new(0.5, 0.0, 0.0, 0.5),
596    ///     Quaternion::new(0.5, 0.0, 0.5, 0.0)
597    /// ));
598    /// assert_relative_eq!(dq1.nlerp(&dq2, 0.2), UnitDualQuaternion::new_normalize(
599    ///     DualQuaternion::from_real_and_dual(
600    ///         Quaternion::new(0.5, 0.0, 0.4, 0.1),
601    ///         Quaternion::new(0.1, 0.4, 0.1, 0.4)
602    ///     )
603    /// ), epsilon = 1.0e-6);
604    /// ```
605    #[inline]
606    #[must_use]
607    pub fn nlerp(&self, other: &Self, t: T) -> Self {
608        let mut res = self.lerp(other, t);
609        let _ = res.normalize_mut();
610
611        Self::new_unchecked(res)
612    }
613
614    /// Screw linear interpolation between two unit quaternions. This creates a
615    /// smooth arc from one dual-quaternion to another.
616    ///
617    /// Panics if the angle between both quaternion is 180 degrees (in which
618    /// case the interpolation is not well-defined). Use `.try_sclerp`
619    /// instead to avoid the panic.
620    ///
621    /// # Example
622    /// ```
623    /// # #[macro_use] extern crate approx;
624    /// # use nalgebra::{UnitDualQuaternion, DualQuaternion, UnitQuaternion, Vector3};
625    ///
626    /// let dq1 = UnitDualQuaternion::from_parts(
627    ///     Vector3::new(0.0, 3.0, 0.0).into(),
628    ///     UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_4, 0.0, 0.0),
629    /// );
630    ///
631    /// let dq2 = UnitDualQuaternion::from_parts(
632    ///     Vector3::new(0.0, 0.0, 3.0).into(),
633    ///     UnitQuaternion::from_euler_angles(-std::f32::consts::PI, 0.0, 0.0),
634    /// );
635    ///
636    /// let dq = dq1.sclerp(&dq2, 1.0 / 3.0);
637    ///
638    /// assert_relative_eq!(
639    ///     dq.rotation().euler_angles().0, std::f32::consts::FRAC_PI_2, epsilon = 1.0e-6
640    /// );
641    /// assert_relative_eq!(dq.translation().vector.y, 3.0, epsilon = 1.0e-6);
642    /// ```
643    #[inline]
644    #[must_use]
645    pub fn sclerp(&self, other: &Self, t: T) -> Self
646    where
647        T: RealField,
648    {
649        self.try_sclerp(other, t, T::default_epsilon())
650            .expect("DualQuaternion sclerp: ambiguous configuration.")
651    }
652
653    /// Computes the screw-linear interpolation between two unit quaternions or
654    /// returns `None` if both quaternions are approximately 180 degrees
655    /// apart (in which case the interpolation is not well-defined).
656    ///
657    /// # Arguments
658    /// * `self`: the first quaternion to interpolate from.
659    /// * `other`: the second quaternion to interpolate toward.
660    /// * `t`: the interpolation parameter. Should be between 0 and 1.
661    /// * `epsilon`: the value below which the sinus of the angle separating
662    ///   both quaternion must be to return `None`.
663    #[inline]
664    #[must_use]
665    pub fn try_sclerp(&self, other: &Self, t: T, epsilon: T) -> Option<Self>
666    where
667        T: RealField,
668    {
669        let two = T::one() + T::one();
670        let half = T::one() / two.clone();
671
672        // Invert one of the quaternions if we've got a longest-path
673        // interpolation.
674        let other = {
675            let dot_product = self.as_ref().real.coords.dot(&other.as_ref().real.coords);
676            if relative_eq!(dot_product, T::zero(), epsilon = epsilon.clone()) {
677                return None;
678            }
679
680            if dot_product < T::zero() {
681                -other.clone()
682            } else {
683                other.clone()
684            }
685        };
686
687        let difference = self.as_ref().conjugate() * other.as_ref();
688        let norm_squared = difference.real.vector().norm_squared();
689        if relative_eq!(norm_squared, T::zero(), epsilon = epsilon) {
690            return Some(Self::from_parts(
691                self.translation()
692                    .vector
693                    .lerp(&other.translation().vector, t)
694                    .into(),
695                self.rotation(),
696            ));
697        }
698
699        let scalar: T = difference.real.scalar();
700        let mut angle = two.clone() * scalar.acos();
701
702        let inverse_norm_squared: T = T::one() / norm_squared;
703        let inverse_norm = inverse_norm_squared.sqrt();
704
705        let mut pitch = -two * difference.dual.scalar() * inverse_norm.clone();
706        let direction = difference.real.vector() * inverse_norm.clone();
707        let moment = (difference.dual.vector()
708            - direction.clone() * (pitch.clone() * difference.real.scalar() * half.clone()))
709            * inverse_norm;
710
711        angle *= t.clone();
712        pitch *= t;
713
714        let sin = (half.clone() * angle.clone()).sin();
715        let cos = (half.clone() * angle).cos();
716
717        let real = Quaternion::from_parts(cos.clone(), direction.clone() * sin.clone());
718        let dual = Quaternion::from_parts(
719            -pitch.clone() * half.clone() * sin.clone(),
720            moment * sin + direction * (pitch * half * cos),
721        );
722
723        Some(
724            self * UnitDualQuaternion::new_unchecked(DualQuaternion::from_real_and_dual(
725                real, dual,
726            )),
727        )
728    }
729
730    /// Return the rotation part of this unit dual quaternion.
731    ///
732    /// # Example
733    /// ```
734    /// # #[macro_use] extern crate approx;
735    /// # use nalgebra::{UnitDualQuaternion, UnitQuaternion, Vector3};
736    /// let dq = UnitDualQuaternion::from_parts(
737    ///     Vector3::new(0.0, 3.0, 0.0).into(),
738    ///     UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_4, 0.0, 0.0)
739    /// );
740    ///
741    /// assert_relative_eq!(
742    ///     dq.rotation().angle(), std::f32::consts::FRAC_PI_4, epsilon = 1.0e-6
743    /// );
744    /// ```
745    #[inline]
746    #[must_use]
747    pub fn rotation(&self) -> UnitQuaternion<T> {
748        Unit::new_unchecked(self.as_ref().real.clone())
749    }
750
751    /// Return the translation part of this unit dual quaternion.
752    ///
753    /// # Example
754    /// ```
755    /// # #[macro_use] extern crate approx;
756    /// # use nalgebra::{UnitDualQuaternion, UnitQuaternion, Vector3};
757    /// let dq = UnitDualQuaternion::from_parts(
758    ///     Vector3::new(0.0, 3.0, 0.0).into(),
759    ///     UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_4, 0.0, 0.0)
760    /// );
761    ///
762    /// assert_relative_eq!(
763    ///     dq.translation().vector, Vector3::new(0.0, 3.0, 0.0), epsilon = 1.0e-6
764    /// );
765    /// ```
766    #[inline]
767    #[must_use]
768    pub fn translation(&self) -> Translation3<T> {
769        let two = T::one() + T::one();
770        Translation3::from(
771            ((self.as_ref().dual.clone() * self.as_ref().real.clone().conjugate()) * two)
772                .vector()
773                .into_owned(),
774        )
775    }
776
777    /// Builds an isometry from this unit dual quaternion.
778    ///
779    /// # Example
780    /// ```
781    /// # #[macro_use] extern crate approx;
782    /// # use nalgebra::{UnitDualQuaternion, UnitQuaternion, Vector3};
783    /// let rotation = UnitQuaternion::from_euler_angles(std::f32::consts::PI, 0.0, 0.0);
784    /// let translation = Vector3::new(1.0, 3.0, 2.5);
785    /// let dq = UnitDualQuaternion::from_parts(
786    ///     translation.into(),
787    ///     rotation
788    /// );
789    /// let iso = dq.to_isometry();
790    ///
791    /// assert_relative_eq!(iso.rotation.angle(), std::f32::consts::PI, epsilon = 1.0e-6);
792    /// assert_relative_eq!(iso.translation.vector, translation, epsilon = 1.0e-6);
793    /// ```
794    #[inline]
795    #[must_use]
796    pub fn to_isometry(self) -> Isometry3<T> {
797        Isometry3::from_parts(self.translation(), self.rotation())
798    }
799
800    /// Rotate and translate a point by this unit dual quaternion interpreted
801    /// as an isometry.
802    ///
803    /// This is the same as the multiplication `self * pt`.
804    ///
805    /// # Example
806    /// ```
807    /// # #[macro_use] extern crate approx;
808    /// # use nalgebra::{UnitDualQuaternion, UnitQuaternion, Vector3, Point3};
809    /// let dq = UnitDualQuaternion::from_parts(
810    ///     Vector3::new(0.0, 3.0, 0.0).into(),
811    ///     UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_2, 0.0, 0.0)
812    /// );
813    /// let point = Point3::new(1.0, 2.0, 3.0);
814    ///
815    /// assert_relative_eq!(
816    ///     dq.transform_point(&point), Point3::new(1.0, 0.0, 2.0), epsilon = 1.0e-6
817    /// );
818    /// ```
819    #[inline]
820    #[must_use]
821    pub fn transform_point(&self, pt: &Point3<T>) -> Point3<T> {
822        self * pt
823    }
824
825    /// Rotate a vector by this unit dual quaternion, ignoring the translational
826    /// component.
827    ///
828    /// This is the same as the multiplication `self * v`.
829    ///
830    /// # Example
831    /// ```
832    /// # #[macro_use] extern crate approx;
833    /// # use nalgebra::{UnitDualQuaternion, UnitQuaternion, Vector3};
834    /// let dq = UnitDualQuaternion::from_parts(
835    ///     Vector3::new(0.0, 3.0, 0.0).into(),
836    ///     UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_2, 0.0, 0.0)
837    /// );
838    /// let vector = Vector3::new(1.0, 2.0, 3.0);
839    ///
840    /// assert_relative_eq!(
841    ///     dq.transform_vector(&vector), Vector3::new(1.0, -3.0, 2.0), epsilon = 1.0e-6
842    /// );
843    /// ```
844    #[inline]
845    #[must_use]
846    pub fn transform_vector(&self, v: &Vector3<T>) -> Vector3<T> {
847        self * v
848    }
849
850    /// Rotate and translate a point by the inverse of this unit quaternion.
851    ///
852    /// This may be cheaper than inverting the unit dual quaternion and
853    /// transforming the point.
854    ///
855    /// # Example
856    /// ```
857    /// # #[macro_use] extern crate approx;
858    /// # use nalgebra::{UnitDualQuaternion, UnitQuaternion, Vector3, Point3};
859    /// let dq = UnitDualQuaternion::from_parts(
860    ///     Vector3::new(0.0, 3.0, 0.0).into(),
861    ///     UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_2, 0.0, 0.0)
862    /// );
863    /// let point = Point3::new(1.0, 2.0, 3.0);
864    ///
865    /// assert_relative_eq!(
866    ///     dq.inverse_transform_point(&point), Point3::new(1.0, 3.0, 1.0), epsilon = 1.0e-6
867    /// );
868    /// ```
869    #[inline]
870    #[must_use]
871    pub fn inverse_transform_point(&self, pt: &Point3<T>) -> Point3<T> {
872        self.inverse() * pt
873    }
874
875    /// Rotate a vector by the inverse of this unit quaternion, ignoring the
876    /// translational component.
877    ///
878    /// This may be cheaper than inverting the unit dual quaternion and
879    /// transforming the vector.
880    ///
881    /// # Example
882    /// ```
883    /// # #[macro_use] extern crate approx;
884    /// # use nalgebra::{UnitDualQuaternion, UnitQuaternion, Vector3};
885    /// let dq = UnitDualQuaternion::from_parts(
886    ///     Vector3::new(0.0, 3.0, 0.0).into(),
887    ///     UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_2, 0.0, 0.0)
888    /// );
889    /// let vector = Vector3::new(1.0, 2.0, 3.0);
890    ///
891    /// assert_relative_eq!(
892    ///     dq.inverse_transform_vector(&vector), Vector3::new(1.0, 3.0, -2.0), epsilon = 1.0e-6
893    /// );
894    /// ```
895    #[inline]
896    #[must_use]
897    pub fn inverse_transform_vector(&self, v: &Vector3<T>) -> Vector3<T> {
898        self.inverse() * v
899    }
900
901    /// Rotate a unit vector by the inverse of this unit quaternion, ignoring
902    /// the translational component. This may be
903    /// cheaper than inverting the unit dual quaternion and transforming the
904    /// vector.
905    ///
906    /// # Example
907    /// ```
908    /// # #[macro_use] extern crate approx;
909    /// # use nalgebra::{UnitDualQuaternion, UnitQuaternion, Unit, Vector3};
910    /// let dq = UnitDualQuaternion::from_parts(
911    ///     Vector3::new(0.0, 3.0, 0.0).into(),
912    ///     UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_2, 0.0, 0.0)
913    /// );
914    /// let vector = Unit::new_unchecked(Vector3::new(0.0, 1.0, 0.0));
915    ///
916    /// assert_relative_eq!(
917    ///     dq.inverse_transform_unit_vector(&vector),
918    ///     Unit::new_unchecked(Vector3::new(0.0, 0.0, -1.0)),
919    ///     epsilon = 1.0e-6
920    /// );
921    /// ```
922    #[inline]
923    #[must_use]
924    pub fn inverse_transform_unit_vector(&self, v: &Unit<Vector3<T>>) -> Unit<Vector3<T>> {
925        self.inverse() * v
926    }
927}
928
929impl<T: SimdRealField + RealField> UnitDualQuaternion<T>
930where
931    T::Element: SimdRealField,
932{
933    /// Converts this unit dual quaternion interpreted as an isometry
934    /// into its equivalent homogeneous transformation matrix.
935    ///
936    /// # Example
937    /// ```
938    /// # #[macro_use] extern crate approx;
939    /// # use nalgebra::{Matrix4, UnitDualQuaternion, UnitQuaternion, Vector3};
940    /// let dq = UnitDualQuaternion::from_parts(
941    ///     Vector3::new(1.0, 3.0, 2.0).into(),
942    ///     UnitQuaternion::from_axis_angle(&Vector3::z_axis(), std::f32::consts::FRAC_PI_6)
943    /// );
944    /// let expected = Matrix4::new(0.8660254, -0.5,      0.0, 1.0,
945    ///                             0.5,       0.8660254, 0.0, 3.0,
946    ///                             0.0,       0.0,       1.0, 2.0,
947    ///                             0.0,       0.0,       0.0, 1.0);
948    ///
949    /// assert_relative_eq!(dq.to_homogeneous(), expected, epsilon = 1.0e-6);
950    /// ```
951    #[inline]
952    #[must_use]
953    pub fn to_homogeneous(self) -> Matrix4<T> {
954        self.to_isometry().to_homogeneous()
955    }
956}
957
958impl<T: RealField> Default for UnitDualQuaternion<T> {
959    fn default() -> Self {
960        Self::identity()
961    }
962}
963
964impl<T: RealField + fmt::Display> fmt::Display for UnitDualQuaternion<T> {
965    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
966        match self.rotation().axis() {
967            Some(axis) => {
968                let axis = axis.into_inner();
969                write!(
970                    f,
971                    "UnitDualQuaternion translation: {} − angle: {} − axis: ({}, {}, {})",
972                    self.translation().vector,
973                    self.rotation().angle(),
974                    axis[0],
975                    axis[1],
976                    axis[2]
977                )
978            }
979            None => {
980                write!(
981                    f,
982                    "UnitDualQuaternion translation: {} − angle: {} − axis: (undefined)",
983                    self.translation().vector,
984                    self.rotation().angle()
985                )
986            }
987        }
988    }
989}
990
991impl<T: RealField + AbsDiffEq<Epsilon = T>> AbsDiffEq for UnitDualQuaternion<T> {
992    type Epsilon = T;
993
994    #[inline]
995    fn default_epsilon() -> Self::Epsilon {
996        T::default_epsilon()
997    }
998
999    #[inline]
1000    fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
1001        self.as_ref().abs_diff_eq(other.as_ref(), epsilon)
1002    }
1003}
1004
1005impl<T: RealField + RelativeEq<Epsilon = T>> RelativeEq for UnitDualQuaternion<T> {
1006    #[inline]
1007    fn default_max_relative() -> Self::Epsilon {
1008        T::default_max_relative()
1009    }
1010
1011    #[inline]
1012    fn relative_eq(
1013        &self,
1014        other: &Self,
1015        epsilon: Self::Epsilon,
1016        max_relative: Self::Epsilon,
1017    ) -> bool {
1018        self.as_ref()
1019            .relative_eq(other.as_ref(), epsilon, max_relative)
1020    }
1021}
1022
1023impl<T: RealField + UlpsEq<Epsilon = T>> UlpsEq for UnitDualQuaternion<T> {
1024    #[inline]
1025    fn default_max_ulps() -> u32 {
1026        T::default_max_ulps()
1027    }
1028
1029    #[inline]
1030    fn ulps_eq(&self, other: &Self, epsilon: Self::Epsilon, max_ulps: u32) -> bool {
1031        self.as_ref().ulps_eq(other.as_ref(), epsilon, max_ulps)
1032    }
1033}
nalgebra/geometry/dual_quaternion.rs

nalgebra/geometry/
dual_quaternion.rs
