nalgebra/geometry/
dual_quaternion.rs

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

nalgebra/geometry/
dual_quaternion.rs
