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