nalgebra/geometry/
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
4use approx::{AbsDiffEq, RelativeEq, UlpsEq};
5use num::Zero;
6use std::fmt;
7use std::hash::{Hash, Hasher};
8
9#[cfg(feature = "serde-serialize-no-std")]
10use crate::base::storage::Owned;
11#[cfg(feature = "serde-serialize-no-std")]
12use serde::{Deserialize, Deserializer, Serialize, Serializer};
13
14use simba::scalar::{ClosedNeg, RealField};
15use simba::simd::{SimdBool, SimdOption, SimdRealField};
16
17use crate::base::dimension::{U1, U3, U4};
18use crate::base::storage::{CStride, RStride};
19use crate::base::{
20    Matrix3, Matrix4, MatrixView, MatrixViewMut, Normed, Scalar, Unit, Vector3, Vector4,
21};
22
23use crate::geometry::{Point3, Rotation};
24
25#[cfg(feature = "rkyv-serialize")]
26use rkyv::bytecheck;
27
28/// A quaternion. See the type alias `UnitQuaternion = Unit<Quaternion>` for a quaternion
29/// that may be used as a rotation.
30#[repr(C)]
31#[derive(Copy, Clone)]
32#[cfg_attr(
33    feature = "rkyv-serialize-no-std",
34    derive(rkyv::Archive, rkyv::Serialize, rkyv::Deserialize),
35    archive(
36        as = "Quaternion<T::Archived>",
37        bound(archive = "
38            T: rkyv::Archive,
39            Vector4<T>: rkyv::Archive<Archived = Vector4<T::Archived>>
40        ")
41    )
42)]
43#[cfg_attr(feature = "rkyv-serialize", derive(bytecheck::CheckBytes))]
44#[cfg_attr(feature = "defmt", derive(defmt::Format))]
45pub struct Quaternion<T> {
46    /// This quaternion as a 4D vector of coordinates in the `[ x, y, z, w ]` storage order.
47    pub coords: Vector4<T>,
48}
49
50impl<T: fmt::Debug> fmt::Debug for Quaternion<T> {
51    fn fmt(&self, formatter: &mut fmt::Formatter<'_>) -> Result<(), fmt::Error> {
52        self.coords.as_slice().fmt(formatter)
53    }
54}
55
56impl<T: Scalar + Hash> Hash for Quaternion<T> {
57    fn hash<H: Hasher>(&self, state: &mut H) {
58        self.coords.hash(state)
59    }
60}
61
62impl<T: Scalar + Eq> Eq for Quaternion<T> {}
63
64impl<T: Scalar> PartialEq for Quaternion<T> {
65    #[inline]
66    fn eq(&self, right: &Self) -> bool {
67        self.coords == right.coords
68    }
69}
70
71impl<T: Scalar + Zero> Default for Quaternion<T> {
72    fn default() -> Self {
73        Quaternion {
74            coords: Vector4::zeros(),
75        }
76    }
77}
78
79#[cfg(feature = "bytemuck")]
80unsafe impl<T: Scalar> bytemuck::Zeroable for Quaternion<T> where Vector4<T>: bytemuck::Zeroable {}
81
82#[cfg(feature = "bytemuck")]
83unsafe impl<T: Scalar> bytemuck::Pod for Quaternion<T>
84where
85    Vector4<T>: bytemuck::Pod,
86    T: Copy,
87{
88}
89
90#[cfg(feature = "serde-serialize-no-std")]
91impl<T: Scalar> Serialize for Quaternion<T>
92where
93    Owned<T, U4>: Serialize,
94{
95    fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error>
96    where
97        S: Serializer,
98    {
99        self.coords.serialize(serializer)
100    }
101}
102
103#[cfg(feature = "serde-serialize-no-std")]
104impl<'a, T: Scalar> Deserialize<'a> for Quaternion<T>
105where
106    Owned<T, U4>: Deserialize<'a>,
107{
108    fn deserialize<Des>(deserializer: Des) -> Result<Self, Des::Error>
109    where
110        Des: Deserializer<'a>,
111    {
112        let coords = Vector4::<T>::deserialize(deserializer)?;
113
114        Ok(Self::from(coords))
115    }
116}
117
118impl<T: SimdRealField> Quaternion<T>
119where
120    T::Element: SimdRealField,
121{
122    /// Moves this unit quaternion into one that owns its data.
123    #[inline]
124    #[deprecated(note = "This method is a no-op and will be removed in a future release.")]
125    pub const fn into_owned(self) -> Self {
126        self
127    }
128
129    /// Clones this unit quaternion into one that owns its data.
130    #[inline]
131    #[deprecated(note = "This method is a no-op and will be removed in a future release.")]
132    pub fn clone_owned(&self) -> Self {
133        Self::from(self.coords.clone_owned())
134    }
135
136    /// Normalizes this quaternion.
137    ///
138    /// # Example
139    /// ```
140    /// # #[macro_use] extern crate approx;
141    /// # use nalgebra::Quaternion;
142    /// let q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
143    /// let q_normalized = q.normalize();
144    /// assert_relative_eq!(q_normalized.norm(), 1.0);
145    /// ```
146    #[inline]
147    #[must_use = "Did you mean to use normalize_mut()?"]
148    pub fn normalize(&self) -> Self {
149        Self::from(self.coords.normalize())
150    }
151
152    /// The imaginary part of this quaternion.
153    #[inline]
154    #[must_use]
155    pub fn imag(&self) -> Vector3<T> {
156        self.coords.xyz()
157    }
158
159    /// The conjugate of this quaternion.
160    ///
161    /// # Example
162    /// ```
163    /// # use nalgebra::Quaternion;
164    /// let q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
165    /// let conj = q.conjugate();
166    /// assert!(conj.i == -2.0 && conj.j == -3.0 && conj.k == -4.0 && conj.w == 1.0);
167    /// ```
168    #[inline]
169    #[must_use = "Did you mean to use conjugate_mut()?"]
170    pub fn conjugate(&self) -> Self {
171        Self::from_parts(self.w.clone(), -self.imag())
172    }
173
174    /// Linear interpolation between two quaternion.
175    ///
176    /// Computes `self * (1 - t) + other * t`.
177    ///
178    /// # Example
179    /// ```
180    /// # use nalgebra::Quaternion;
181    /// let q1 = Quaternion::new(1.0, 2.0, 3.0, 4.0);
182    /// let q2 = Quaternion::new(10.0, 20.0, 30.0, 40.0);
183    ///
184    /// assert_eq!(q1.lerp(&q2, 0.1), Quaternion::new(1.9, 3.8, 5.7, 7.6));
185    /// ```
186    #[inline]
187    #[must_use]
188    pub fn lerp(&self, other: &Self, t: T) -> Self {
189        self * (T::one() - t.clone()) + other * t
190    }
191
192    /// The vector part `(i, j, k)` of this quaternion.
193    ///
194    /// # Example
195    /// ```
196    /// # use nalgebra::Quaternion;
197    /// let q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
198    /// assert_eq!(q.vector()[0], 2.0);
199    /// assert_eq!(q.vector()[1], 3.0);
200    /// assert_eq!(q.vector()[2], 4.0);
201    /// ```
202    #[inline]
203    #[must_use]
204    pub fn vector(&self) -> MatrixView<'_, T, U3, U1, RStride<T, U4, U1>, CStride<T, U4, U1>> {
205        self.coords.fixed_rows::<3>(0)
206    }
207
208    /// The scalar part `w` of this quaternion.
209    ///
210    /// # Example
211    /// ```
212    /// # use nalgebra::Quaternion;
213    /// let q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
214    /// assert_eq!(q.scalar(), 1.0);
215    /// ```
216    #[inline]
217    #[must_use]
218    pub fn scalar(&self) -> T {
219        self.coords[3].clone()
220    }
221
222    /// Reinterprets this quaternion as a 4D vector.
223    ///
224    /// # Example
225    /// ```
226    /// # use nalgebra::{Vector4, Quaternion};
227    /// let q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
228    /// // Recall that the quaternion is stored internally as (i, j, k, w)
229    /// // while the crate::new constructor takes the arguments as (w, i, j, k).
230    /// assert_eq!(*q.as_vector(), Vector4::new(2.0, 3.0, 4.0, 1.0));
231    /// ```
232    #[inline]
233    #[must_use]
234    pub const fn as_vector(&self) -> &Vector4<T> {
235        &self.coords
236    }
237
238    /// The norm of this quaternion.
239    ///
240    /// # Example
241    /// ```
242    /// # #[macro_use] extern crate approx;
243    /// # use nalgebra::Quaternion;
244    /// let q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
245    /// assert_relative_eq!(q.norm(), 5.47722557, epsilon = 1.0e-6);
246    /// ```
247    #[inline]
248    #[must_use]
249    pub fn norm(&self) -> T {
250        self.coords.norm()
251    }
252
253    /// A synonym for the norm of this quaternion.
254    ///
255    /// Aka the length.
256    /// This is the same as `.norm()`
257    ///
258    /// # Example
259    /// ```
260    /// # #[macro_use] extern crate approx;
261    /// # use nalgebra::Quaternion;
262    /// let q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
263    /// assert_relative_eq!(q.magnitude(), 5.47722557, epsilon = 1.0e-6);
264    /// ```
265    #[inline]
266    #[must_use]
267    pub fn magnitude(&self) -> T {
268        self.norm()
269    }
270
271    /// The squared norm of this quaternion.
272    ///
273    /// # Example
274    /// ```
275    /// # use nalgebra::Quaternion;
276    /// let q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
277    /// assert_eq!(q.norm_squared(), 30.0);
278    /// ```
279    #[inline]
280    #[must_use]
281    pub fn norm_squared(&self) -> T {
282        self.coords.norm_squared()
283    }
284
285    /// A synonym for the squared norm of this quaternion.
286    ///
287    /// Aka the squared length.
288    /// This is the same as `.norm_squared()`
289    ///
290    /// # Example
291    /// ```
292    /// # use nalgebra::Quaternion;
293    /// let q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
294    /// assert_eq!(q.magnitude_squared(), 30.0);
295    /// ```
296    #[inline]
297    #[must_use]
298    pub fn magnitude_squared(&self) -> T {
299        self.norm_squared()
300    }
301
302    /// The dot product of two quaternions.
303    ///
304    /// # Example
305    /// ```
306    /// # use nalgebra::Quaternion;
307    /// let q1 = Quaternion::new(1.0, 2.0, 3.0, 4.0);
308    /// let q2 = Quaternion::new(5.0, 6.0, 7.0, 8.0);
309    /// assert_eq!(q1.dot(&q2), 70.0);
310    /// ```
311    #[inline]
312    #[must_use]
313    pub fn dot(&self, rhs: &Self) -> T {
314        self.coords.dot(&rhs.coords)
315    }
316}
317
318impl<T: SimdRealField> Quaternion<T>
319where
320    T::Element: SimdRealField,
321{
322    /// Inverts this quaternion if it is not zero.
323    ///
324    /// This method also does not works with SIMD components (see `simd_try_inverse` instead).
325    ///
326    /// # Example
327    /// ```
328    /// # #[macro_use] extern crate approx;
329    /// # use nalgebra::Quaternion;
330    /// let q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
331    /// let inv_q = q.try_inverse();
332    ///
333    /// assert!(inv_q.is_some());
334    /// assert_relative_eq!(inv_q.unwrap() * q, Quaternion::identity());
335    ///
336    /// //Non-invertible case
337    /// let q = Quaternion::new(0.0, 0.0, 0.0, 0.0);
338    /// let inv_q = q.try_inverse();
339    ///
340    /// assert!(inv_q.is_none());
341    /// ```
342    #[inline]
343    #[must_use = "Did you mean to use try_inverse_mut()?"]
344    pub fn try_inverse(&self) -> Option<Self>
345    where
346        T: RealField,
347    {
348        let mut res = self.clone();
349
350        if res.try_inverse_mut() {
351            Some(res)
352        } else {
353            None
354        }
355    }
356
357    /// Attempt to inverse this quaternion.
358    ///
359    /// This method also works with SIMD components.
360    #[inline]
361    #[must_use = "Did you mean to use try_inverse_mut()?"]
362    pub fn simd_try_inverse(&self) -> SimdOption<Self> {
363        let norm_squared = self.norm_squared();
364        let ge = norm_squared.clone().simd_ge(T::simd_default_epsilon());
365        SimdOption::new(self.conjugate() / norm_squared, ge)
366    }
367
368    /// Calculates the inner product (also known as the dot product).
369    /// See "Foundations of Game Engine Development, Volume 1: Mathematics" by Lengyel
370    /// Formula 4.89.
371    ///
372    /// # Example
373    /// ```
374    /// # #[macro_use] extern crate approx;
375    /// # use nalgebra::Quaternion;
376    /// let a = Quaternion::new(0.0, 2.0, 3.0, 4.0);
377    /// let b = Quaternion::new(0.0, 5.0, 2.0, 1.0);
378    /// let expected = Quaternion::new(-20.0, 0.0, 0.0, 0.0);
379    /// let result = a.inner(&b);
380    /// assert_relative_eq!(expected, result, epsilon = 1.0e-5);
381    /// ```
382    #[inline]
383    #[must_use]
384    pub fn inner(&self, other: &Self) -> Self {
385        (self * other + other * self).half()
386    }
387
388    /// Calculates the outer product (also known as the wedge product).
389    /// See "Foundations of Game Engine Development, Volume 1: Mathematics" by Lengyel
390    /// Formula 4.89.
391    ///
392    /// # Example
393    /// ```
394    /// # #[macro_use] extern crate approx;
395    /// # use nalgebra::Quaternion;
396    /// let a = Quaternion::new(0.0, 2.0, 3.0, 4.0);
397    /// let b = Quaternion::new(0.0, 5.0, 2.0, 1.0);
398    /// let expected = Quaternion::new(0.0, -5.0, 18.0, -11.0);
399    /// let result = a.outer(&b);
400    /// assert_relative_eq!(expected, result, epsilon = 1.0e-5);
401    /// ```
402    #[inline]
403    #[must_use]
404    pub fn outer(&self, other: &Self) -> Self {
405        #[allow(clippy::eq_op)]
406        (self * other - other * self).half()
407    }
408
409    /// Calculates the projection of `self` onto `other` (also known as the parallel).
410    /// See "Foundations of Game Engine Development, Volume 1: Mathematics" by Lengyel
411    /// Formula 4.94.
412    ///
413    /// # Example
414    /// ```
415    /// # #[macro_use] extern crate approx;
416    /// # use nalgebra::Quaternion;
417    /// let a = Quaternion::new(0.0, 2.0, 3.0, 4.0);
418    /// let b = Quaternion::new(0.0, 5.0, 2.0, 1.0);
419    /// let expected = Quaternion::new(0.0, 3.333333333333333, 1.3333333333333333, 0.6666666666666666);
420    /// let result = a.project(&b).unwrap();
421    /// assert_relative_eq!(expected, result, epsilon = 1.0e-5);
422    /// ```
423    #[inline]
424    #[must_use]
425    pub fn project(&self, other: &Self) -> Option<Self>
426    where
427        T: RealField,
428    {
429        self.inner(other).right_div(other)
430    }
431
432    /// Calculates the rejection of `self` from `other` (also known as the perpendicular).
433    /// See "Foundations of Game Engine Development, Volume 1: Mathematics" by Lengyel
434    /// Formula 4.94.
435    ///
436    /// # Example
437    /// ```
438    /// # #[macro_use] extern crate approx;
439    /// # use nalgebra::Quaternion;
440    /// let a = Quaternion::new(0.0, 2.0, 3.0, 4.0);
441    /// let b = Quaternion::new(0.0, 5.0, 2.0, 1.0);
442    /// let expected = Quaternion::new(0.0, -1.3333333333333333, 1.6666666666666665, 3.3333333333333335);
443    /// let result = a.reject(&b).unwrap();
444    /// assert_relative_eq!(expected, result, epsilon = 1.0e-5);
445    /// ```
446    #[inline]
447    #[must_use]
448    pub fn reject(&self, other: &Self) -> Option<Self>
449    where
450        T: RealField,
451    {
452        self.outer(other).right_div(other)
453    }
454
455    /// The polar decomposition of this quaternion.
456    ///
457    /// Returns, from left to right: the quaternion norm, the half rotation angle, the rotation
458    /// axis. If the rotation angle is zero, the rotation axis is set to `None`.
459    ///
460    /// # Example
461    /// ```
462    /// # use std::f32;
463    /// # use nalgebra::{Vector3, Quaternion};
464    /// let q = Quaternion::new(0.0, 5.0, 0.0, 0.0);
465    /// let (norm, half_ang, axis) = q.polar_decomposition();
466    /// assert_eq!(norm, 5.0);
467    /// assert_eq!(half_ang, f32::consts::FRAC_PI_2);
468    /// assert_eq!(axis, Some(Vector3::x_axis()));
469    /// ```
470    #[must_use]
471    pub fn polar_decomposition(&self) -> (T, T, Option<Unit<Vector3<T>>>)
472    where
473        T: RealField,
474    {
475        match Unit::try_new_and_get(self.clone(), T::zero()) {
476            Some((q, n)) => match Unit::try_new(self.vector().clone_owned(), T::zero()) {
477                Some(axis) => {
478                    let angle = q.angle() / crate::convert(2.0f64);
479
480                    (n, angle, Some(axis))
481                }
482                None => (n, T::zero(), None),
483            },
484            None => (T::zero(), T::zero(), None),
485        }
486    }
487
488    /// Compute the natural logarithm of a quaternion.
489    ///
490    /// # Example
491    /// ```
492    /// # #[macro_use] extern crate approx;
493    /// # use nalgebra::Quaternion;
494    /// let q = Quaternion::new(2.0, 5.0, 0.0, 0.0);
495    /// assert_relative_eq!(q.ln(), Quaternion::new(1.683647, 1.190289, 0.0, 0.0), epsilon = 1.0e-6)
496    /// ```
497    #[inline]
498    #[must_use]
499    pub fn ln(&self) -> Self {
500        let n = self.norm();
501        let v = self.vector();
502        let s = self.scalar();
503
504        Self::from_parts(n.clone().simd_ln(), v.normalize() * (s / n).simd_acos())
505    }
506
507    /// Compute the exponential of a quaternion.
508    ///
509    /// # Example
510    /// ```
511    /// # #[macro_use] extern crate approx;
512    /// # use nalgebra::Quaternion;
513    /// let q = Quaternion::new(1.683647, 1.190289, 0.0, 0.0);
514    /// assert_relative_eq!(q.exp(), Quaternion::new(2.0, 5.0, 0.0, 0.0), epsilon = 1.0e-5)
515    /// ```
516    #[inline]
517    #[must_use]
518    pub fn exp(&self) -> Self {
519        self.exp_eps(T::simd_default_epsilon())
520    }
521
522    /// Compute the exponential of a quaternion. Returns the identity if the vector part of this quaternion
523    /// has a norm smaller than `eps`.
524    ///
525    /// # Example
526    /// ```
527    /// # #[macro_use] extern crate approx;
528    /// # use nalgebra::Quaternion;
529    /// let q = Quaternion::new(1.683647, 1.190289, 0.0, 0.0);
530    /// assert_relative_eq!(q.exp_eps(1.0e-6), Quaternion::new(2.0, 5.0, 0.0, 0.0), epsilon = 1.0e-5);
531    ///
532    /// // Singular case.
533    /// let q = Quaternion::new(0.0000001, 0.0, 0.0, 0.0);
534    /// assert_eq!(q.exp_eps(1.0e-6), Quaternion::identity());
535    /// ```
536    #[inline]
537    #[must_use]
538    pub fn exp_eps(&self, eps: T) -> Self {
539        let v = self.vector();
540        let nn = v.norm_squared();
541        let le = nn.clone().simd_le(eps.clone() * eps);
542        le.if_else(Self::identity, || {
543            let w_exp = self.scalar().simd_exp();
544            let n = nn.simd_sqrt();
545            let nv = v * (w_exp.clone() * n.clone().simd_sin() / n.clone());
546
547            Self::from_parts(w_exp * n.simd_cos(), nv)
548        })
549    }
550
551    /// Raise the quaternion to a given floating power.
552    ///
553    /// # Example
554    /// ```
555    /// # #[macro_use] extern crate approx;
556    /// # use nalgebra::Quaternion;
557    /// let q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
558    /// assert_relative_eq!(q.powf(1.5), Quaternion::new( -6.2576659, 4.1549037, 6.2323556, 8.3098075), epsilon = 1.0e-6);
559    /// ```
560    #[inline]
561    #[must_use]
562    pub fn powf(&self, n: T) -> Self {
563        (self.ln() * n).exp()
564    }
565
566    /// Transforms this quaternion into its 4D vector form (Vector part, Scalar part).
567    ///
568    /// # Example
569    /// ```
570    /// # use nalgebra::{Quaternion, Vector4};
571    /// let mut q = Quaternion::identity();
572    /// *q.as_vector_mut() = Vector4::new(1.0, 2.0, 3.0, 4.0);
573    /// assert!(q.i == 1.0 && q.j == 2.0 && q.k == 3.0 && q.w == 4.0);
574    /// ```
575    #[inline]
576    pub const fn as_vector_mut(&mut self) -> &mut Vector4<T> {
577        &mut self.coords
578    }
579
580    /// The mutable vector part `(i, j, k)` of this quaternion.
581    ///
582    /// # Example
583    /// ```
584    /// # use nalgebra::{Quaternion, Vector4};
585    /// let mut q = Quaternion::identity();
586    /// {
587    ///     let mut v = q.vector_mut();
588    ///     v[0] = 2.0;
589    ///     v[1] = 3.0;
590    ///     v[2] = 4.0;
591    /// }
592    /// assert!(q.i == 2.0 && q.j == 3.0 && q.k == 4.0 && q.w == 1.0);
593    /// ```
594    #[inline]
595    pub fn vector_mut(
596        &mut self,
597    ) -> MatrixViewMut<'_, T, U3, U1, RStride<T, U4, U1>, CStride<T, U4, U1>> {
598        self.coords.fixed_rows_mut::<3>(0)
599    }
600
601    /// Replaces this quaternion by its conjugate.
602    ///
603    /// # Example
604    /// ```
605    /// # use nalgebra::Quaternion;
606    /// let mut q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
607    /// q.conjugate_mut();
608    /// assert!(q.i == -2.0 && q.j == -3.0 && q.k == -4.0 && q.w == 1.0);
609    /// ```
610    #[inline]
611    pub fn conjugate_mut(&mut self) {
612        self.coords[0] = -self.coords[0].clone();
613        self.coords[1] = -self.coords[1].clone();
614        self.coords[2] = -self.coords[2].clone();
615    }
616
617    /// Inverts this quaternion in-place if it is not zero.
618    ///
619    /// # Example
620    /// ```
621    /// # #[macro_use] extern crate approx;
622    /// # use nalgebra::Quaternion;
623    /// let mut q = Quaternion::new(1.0f32, 2.0, 3.0, 4.0);
624    ///
625    /// assert!(q.try_inverse_mut());
626    /// assert_relative_eq!(q * Quaternion::new(1.0, 2.0, 3.0, 4.0), Quaternion::identity());
627    ///
628    /// //Non-invertible case
629    /// let mut q = Quaternion::new(0.0f32, 0.0, 0.0, 0.0);
630    /// assert!(!q.try_inverse_mut());
631    /// ```
632    #[inline]
633    pub fn try_inverse_mut(&mut self) -> T::SimdBool {
634        let norm_squared = self.norm_squared();
635        let ge = norm_squared.clone().simd_ge(T::simd_default_epsilon());
636        *self = ge.if_else(|| self.conjugate() / norm_squared, || self.clone());
637        ge
638    }
639
640    /// Normalizes this quaternion.
641    ///
642    /// # Example
643    /// ```
644    /// # #[macro_use] extern crate approx;
645    /// # use nalgebra::Quaternion;
646    /// let mut q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
647    /// q.normalize_mut();
648    /// assert_relative_eq!(q.norm(), 1.0);
649    /// ```
650    #[inline]
651    pub fn normalize_mut(&mut self) -> T {
652        self.coords.normalize_mut()
653    }
654
655    /// Calculates square of a quaternion.
656    #[inline]
657    #[must_use]
658    pub fn squared(&self) -> Self {
659        self * self
660    }
661
662    /// Divides quaternion into two.
663    #[inline]
664    #[must_use]
665    pub fn half(&self) -> Self {
666        self / crate::convert(2.0f64)
667    }
668
669    /// Calculates square root.
670    #[inline]
671    #[must_use]
672    pub fn sqrt(&self) -> Self {
673        self.powf(crate::convert(0.5))
674    }
675
676    /// Check if the quaternion is pure.
677    ///
678    /// A quaternion is pure if it has no real part (`self.w == 0.0`).
679    #[inline]
680    #[must_use]
681    pub fn is_pure(&self) -> bool {
682        self.w.is_zero()
683    }
684
685    /// Convert quaternion to pure quaternion.
686    #[inline]
687    #[must_use]
688    pub fn pure(&self) -> Self {
689        Self::from_imag(self.imag())
690    }
691
692    /// Left quaternionic division.
693    ///
694    /// Calculates B<sup>-1</sup> * A where A = self, B = other.
695    #[inline]
696    #[must_use]
697    pub fn left_div(&self, other: &Self) -> Option<Self>
698    where
699        T: RealField,
700    {
701        other.try_inverse().map(|inv| inv * self)
702    }
703
704    /// Right quaternionic division.
705    ///
706    /// Calculates A * B<sup>-1</sup> where A = self, B = other.
707    ///
708    /// # Example
709    /// ```
710    /// # #[macro_use] extern crate approx;
711    /// # use nalgebra::Quaternion;
712    /// let a = Quaternion::new(0.0, 1.0, 2.0, 3.0);
713    /// let b = Quaternion::new(0.0, 5.0, 2.0, 1.0);
714    /// let result = a.right_div(&b).unwrap();
715    /// let expected = Quaternion::new(0.4, 0.13333333333333336, -0.4666666666666667, 0.26666666666666666);
716    /// assert_relative_eq!(expected, result, epsilon = 1.0e-7);
717    /// ```
718    #[inline]
719    #[must_use]
720    pub fn right_div(&self, other: &Self) -> Option<Self>
721    where
722        T: RealField,
723    {
724        other.try_inverse().map(|inv| self * inv)
725    }
726
727    /// Calculates the quaternionic cosinus.
728    ///
729    /// # Example
730    /// ```
731    /// # #[macro_use] extern crate approx;
732    /// # use nalgebra::Quaternion;
733    /// let input = Quaternion::new(1.0, 2.0, 3.0, 4.0);
734    /// let expected = Quaternion::new(58.93364616794395, -34.086183690465596, -51.1292755356984, -68.17236738093119);
735    /// let result = input.cos();
736    /// assert_relative_eq!(expected, result, epsilon = 1.0e-7);
737    /// ```
738    #[inline]
739    #[must_use]
740    pub fn cos(&self) -> Self {
741        let z = self.imag().magnitude();
742        let w = -self.w.clone().simd_sin() * z.clone().simd_sinhc();
743        Self::from_parts(self.w.clone().simd_cos() * z.simd_cosh(), self.imag() * w)
744    }
745
746    /// Calculates the quaternionic arccosinus.
747    ///
748    /// # Example
749    /// ```
750    /// # #[macro_use] extern crate approx;
751    /// # use nalgebra::Quaternion;
752    /// let input = Quaternion::new(1.0, 2.0, 3.0, 4.0);
753    /// let result = input.cos().acos();
754    /// assert_relative_eq!(input, result, epsilon = 1.0e-7);
755    /// ```
756    #[inline]
757    #[must_use]
758    pub fn acos(&self) -> Self {
759        let u = Self::from_imag(self.imag().normalize());
760        let identity = Self::identity();
761
762        let z = (self + (self.squared() - identity).sqrt()).ln();
763
764        -(u * z)
765    }
766
767    /// Calculates the quaternionic sinus.
768    ///
769    /// # Example
770    /// ```
771    /// # #[macro_use] extern crate approx;
772    /// # use nalgebra::Quaternion;
773    /// let input = Quaternion::new(1.0, 2.0, 3.0, 4.0);
774    /// let expected = Quaternion::new(91.78371578403467, 21.886486853029176, 32.82973027954377, 43.77297370605835);
775    /// let result = input.sin();
776    /// assert_relative_eq!(expected, result, epsilon = 1.0e-7);
777    /// ```
778    #[inline]
779    #[must_use]
780    pub fn sin(&self) -> Self {
781        let z = self.imag().magnitude();
782        let w = self.w.clone().simd_cos() * z.clone().simd_sinhc();
783        Self::from_parts(self.w.clone().simd_sin() * z.simd_cosh(), self.imag() * w)
784    }
785
786    /// Calculates the quaternionic arcsinus.
787    ///
788    /// # Example
789    /// ```
790    /// # #[macro_use] extern crate approx;
791    /// # use nalgebra::Quaternion;
792    /// let input = Quaternion::new(1.0, 2.0, 3.0, 4.0);
793    /// let result = input.sin().asin();
794    /// assert_relative_eq!(input, result, epsilon = 1.0e-7);
795    /// ```
796    #[inline]
797    #[must_use]
798    pub fn asin(&self) -> Self {
799        let u = Self::from_imag(self.imag().normalize());
800        let identity = Self::identity();
801
802        let z = ((u.clone() * self) + (identity - self.squared()).sqrt()).ln();
803
804        -(u * z)
805    }
806
807    /// Calculates the quaternionic tangent.
808    ///
809    /// # Example
810    /// ```
811    /// # #[macro_use] extern crate approx;
812    /// # use nalgebra::Quaternion;
813    /// let input = Quaternion::new(1.0, 2.0, 3.0, 4.0);
814    /// let expected = Quaternion::new(0.00003821631725009489, 0.3713971716439371, 0.5570957574659058, 0.7427943432878743);
815    /// let result = input.tan();
816    /// assert_relative_eq!(expected, result, epsilon = 1.0e-7);
817    /// ```
818    #[inline]
819    #[must_use]
820    pub fn tan(&self) -> Self
821    where
822        T: RealField,
823    {
824        self.sin().right_div(&self.cos()).unwrap()
825    }
826
827    /// Calculates the quaternionic arctangent.
828    ///
829    /// # Example
830    /// ```
831    /// # #[macro_use] extern crate approx;
832    /// # use nalgebra::Quaternion;
833    /// let input = Quaternion::new(1.0, 2.0, 3.0, 4.0);
834    /// let result = input.tan().atan();
835    /// assert_relative_eq!(input, result, epsilon = 1.0e-7);
836    /// ```
837    #[inline]
838    #[must_use]
839    pub fn atan(&self) -> Self
840    where
841        T: RealField,
842    {
843        let u = Self::from_imag(self.imag().normalize());
844        let num = u.clone() + self;
845        let den = u.clone() - self;
846        let fr = num.right_div(&den).unwrap();
847        let ln = fr.ln();
848        (u.half()) * ln
849    }
850
851    /// Calculates the hyperbolic quaternionic sinus.
852    ///
853    /// # Example
854    /// ```
855    /// # #[macro_use] extern crate approx;
856    /// # use nalgebra::Quaternion;
857    /// let input = Quaternion::new(1.0, 2.0, 3.0, 4.0);
858    /// let expected = Quaternion::new(0.7323376060463428, -0.4482074499805421, -0.6723111749708133, -0.8964148999610843);
859    /// let result = input.sinh();
860    /// assert_relative_eq!(expected, result, epsilon = 1.0e-7);
861    /// ```
862    #[inline]
863    #[must_use]
864    pub fn sinh(&self) -> Self {
865        (self.exp() - (-self).exp()).half()
866    }
867
868    /// Calculates the hyperbolic quaternionic arcsinus.
869    ///
870    /// # Example
871    /// ```
872    /// # #[macro_use] extern crate approx;
873    /// # use nalgebra::Quaternion;
874    /// let input = Quaternion::new(1.0, 2.0, 3.0, 4.0);
875    /// let expected = Quaternion::new(2.385889902585242, 0.514052600662788, 0.7710789009941821, 1.028105201325576);
876    /// let result = input.asinh();
877    /// assert_relative_eq!(expected, result, epsilon = 1.0e-7);
878    /// ```
879    #[inline]
880    #[must_use]
881    pub fn asinh(&self) -> Self {
882        let identity = Self::identity();
883        (self + (identity + self.squared()).sqrt()).ln()
884    }
885
886    /// Calculates the hyperbolic quaternionic cosinus.
887    ///
888    /// # Example
889    /// ```
890    /// # #[macro_use] extern crate approx;
891    /// # use nalgebra::Quaternion;
892    /// let input = Quaternion::new(1.0, 2.0, 3.0, 4.0);
893    /// let expected = Quaternion::new(0.9615851176369566, -0.3413521745610167, -0.5120282618415251, -0.6827043491220334);
894    /// let result = input.cosh();
895    /// assert_relative_eq!(expected, result, epsilon = 1.0e-7);
896    /// ```
897    #[inline]
898    #[must_use]
899    pub fn cosh(&self) -> Self {
900        (self.exp() + (-self).exp()).half()
901    }
902
903    /// Calculates the hyperbolic quaternionic arccosinus.
904    ///
905    /// # Example
906    /// ```
907    /// # #[macro_use] extern crate approx;
908    /// # use nalgebra::Quaternion;
909    /// let input = Quaternion::new(1.0, 2.0, 3.0, 4.0);
910    /// let expected = Quaternion::new(2.4014472020074007, 0.5162761016176176, 0.7744141524264264, 1.0325522032352352);
911    /// let result = input.acosh();
912    /// assert_relative_eq!(expected, result, epsilon = 1.0e-7);
913    /// ```
914    #[inline]
915    #[must_use]
916    pub fn acosh(&self) -> Self {
917        let identity = Self::identity();
918        (self + (self + identity.clone()).sqrt() * (self - identity).sqrt()).ln()
919    }
920
921    /// Calculates the hyperbolic quaternionic tangent.
922    ///
923    /// # Example
924    /// ```
925    /// # #[macro_use] extern crate approx;
926    /// # use nalgebra::Quaternion;
927    /// let input = Quaternion::new(1.0, 2.0, 3.0, 4.0);
928    /// let expected = Quaternion::new(1.0248695360556623, -0.10229568178876419, -0.1534435226831464, -0.20459136357752844);
929    /// let result = input.tanh();
930    /// assert_relative_eq!(expected, result, epsilon = 1.0e-7);
931    /// ```
932    #[inline]
933    #[must_use]
934    pub fn tanh(&self) -> Self
935    where
936        T: RealField,
937    {
938        self.sinh().right_div(&self.cosh()).unwrap()
939    }
940
941    /// Calculates the hyperbolic quaternionic arctangent.
942    ///
943    /// # Example
944    /// ```
945    /// # #[macro_use] extern crate approx;
946    /// # use nalgebra::Quaternion;
947    /// let input = Quaternion::new(1.0, 2.0, 3.0, 4.0);
948    /// let expected = Quaternion::new(0.03230293287000163, 0.5173453683196951, 0.7760180524795426, 1.0346907366393903);
949    /// let result = input.atanh();
950    /// assert_relative_eq!(expected, result, epsilon = 1.0e-7);
951    /// ```
952    #[inline]
953    #[must_use]
954    pub fn atanh(&self) -> Self {
955        let identity = Self::identity();
956        ((identity.clone() + self).ln() - (identity - self).ln()).half()
957    }
958}
959
960impl<T: RealField + AbsDiffEq<Epsilon = T>> AbsDiffEq for Quaternion<T> {
961    type Epsilon = T;
962
963    #[inline]
964    fn default_epsilon() -> Self::Epsilon {
965        T::default_epsilon()
966    }
967
968    #[inline]
969    fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
970        self.as_vector().abs_diff_eq(other.as_vector(), epsilon.clone()) ||
971        // Account for the double-covering of S², i.e. q = -q
972        self.as_vector().iter().zip(other.as_vector().iter()).all(|(a, b)| a.abs_diff_eq(&-b.clone(), epsilon.clone()))
973    }
974}
975
976impl<T: RealField + RelativeEq<Epsilon = T>> RelativeEq for Quaternion<T> {
977    #[inline]
978    fn default_max_relative() -> Self::Epsilon {
979        T::default_max_relative()
980    }
981
982    #[inline]
983    fn relative_eq(
984        &self,
985        other: &Self,
986        epsilon: Self::Epsilon,
987        max_relative: Self::Epsilon,
988    ) -> bool {
989        self.as_vector().relative_eq(other.as_vector(), epsilon.clone(), max_relative.clone()) ||
990        // Account for the double-covering of S², i.e. q = -q
991        self.as_vector().iter().zip(other.as_vector().iter()).all(|(a, b)| a.relative_eq(&-b.clone(), epsilon.clone(), max_relative.clone()))
992    }
993}
994
995impl<T: RealField + UlpsEq<Epsilon = T>> UlpsEq for Quaternion<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_vector().ulps_eq(other.as_vector(), epsilon.clone(), max_ulps) ||
1004        // Account for the double-covering of S², i.e. q = -q.
1005        self.as_vector().iter().zip(other.as_vector().iter()).all(|(a, b)| a.ulps_eq(&-b.clone(), epsilon.clone(), max_ulps))
1006    }
1007}
1008
1009impl<T: RealField + fmt::Display> fmt::Display for Quaternion<T> {
1010    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
1011        write!(
1012            f,
1013            "Quaternion {} − ({}, {}, {})",
1014            self[3], self[0], self[1], self[2]
1015        )
1016    }
1017}
1018
1019/// A unit quaternions. May be used to represent a rotation.
1020pub type UnitQuaternion<T> = Unit<Quaternion<T>>;
1021
1022impl<T: Scalar + ClosedNeg + PartialEq> PartialEq for UnitQuaternion<T> {
1023    #[inline]
1024    fn eq(&self, rhs: &Self) -> bool {
1025        self.coords == rhs.coords ||
1026        // Account for the double-covering of S², i.e. q = -q
1027        self.coords.iter().zip(rhs.coords.iter()).all(|(a, b)| *a == -b.clone())
1028    }
1029}
1030
1031impl<T: Scalar + ClosedNeg + Eq> Eq for UnitQuaternion<T> {}
1032
1033impl<T: SimdRealField> Normed for Quaternion<T> {
1034    type Norm = T::SimdRealField;
1035
1036    #[inline]
1037    fn norm(&self) -> T::SimdRealField {
1038        self.coords.norm()
1039    }
1040
1041    #[inline]
1042    fn norm_squared(&self) -> T::SimdRealField {
1043        self.coords.norm_squared()
1044    }
1045
1046    #[inline]
1047    fn scale_mut(&mut self, n: Self::Norm) {
1048        self.coords.scale_mut(n)
1049    }
1050
1051    #[inline]
1052    fn unscale_mut(&mut self, n: Self::Norm) {
1053        self.coords.unscale_mut(n)
1054    }
1055}
1056
1057impl<T: SimdRealField> UnitQuaternion<T>
1058where
1059    T::Element: SimdRealField,
1060{
1061    /// The rotation angle in [0; pi] of this unit quaternion.
1062    ///
1063    /// # Example
1064    /// ```
1065    /// # use nalgebra::{Unit, UnitQuaternion, Vector3};
1066    /// let axis = Unit::new_normalize(Vector3::new(1.0, 2.0, 3.0));
1067    /// let rot = UnitQuaternion::from_axis_angle(&axis, 1.78);
1068    /// assert_eq!(rot.angle(), 1.78);
1069    /// ```
1070    #[inline]
1071    #[must_use]
1072    pub fn angle(&self) -> T {
1073        let w = self.quaternion().scalar().simd_abs();
1074        self.quaternion().imag().norm().simd_atan2(w) * crate::convert(2.0f64)
1075    }
1076
1077    /// The underlying quaternion.
1078    ///
1079    /// Same as `self.as_ref()`.
1080    ///
1081    /// # Example
1082    /// ```
1083    /// # use nalgebra::{UnitQuaternion, Quaternion};
1084    /// let axis = UnitQuaternion::identity();
1085    /// assert_eq!(*axis.quaternion(), Quaternion::new(1.0, 0.0, 0.0, 0.0));
1086    /// ```
1087    #[inline]
1088    #[must_use]
1089    pub fn quaternion(&self) -> &Quaternion<T> {
1090        self.as_ref()
1091    }
1092
1093    /// Compute the conjugate of this unit quaternion.
1094    ///
1095    /// # Example
1096    /// ```
1097    /// # use nalgebra::{Unit, UnitQuaternion, Vector3};
1098    /// let axis = Unit::new_normalize(Vector3::new(1.0, 2.0, 3.0));
1099    /// let rot = UnitQuaternion::from_axis_angle(&axis, 1.78);
1100    /// let conj = rot.conjugate();
1101    /// assert_eq!(conj, UnitQuaternion::from_axis_angle(&-axis, 1.78));
1102    /// ```
1103    #[inline]
1104    #[must_use = "Did you mean to use conjugate_mut()?"]
1105    pub fn conjugate(&self) -> Self {
1106        Self::new_unchecked(self.as_ref().conjugate())
1107    }
1108
1109    /// Inverts this quaternion if it is not zero.
1110    ///
1111    /// # Example
1112    /// ```
1113    /// # use nalgebra::{Unit, UnitQuaternion, Vector3};
1114    /// let axis = Unit::new_normalize(Vector3::new(1.0, 2.0, 3.0));
1115    /// let rot = UnitQuaternion::from_axis_angle(&axis, 1.78);
1116    /// let inv = rot.inverse();
1117    /// assert_eq!(rot * inv, UnitQuaternion::identity());
1118    /// assert_eq!(inv * rot, UnitQuaternion::identity());
1119    /// ```
1120    #[inline]
1121    #[must_use = "Did you mean to use inverse_mut()?"]
1122    pub fn inverse(&self) -> Self {
1123        self.conjugate()
1124    }
1125
1126    /// The rotation angle needed to make `self` and `other` coincide.
1127    ///
1128    /// # Example
1129    /// ```
1130    /// # #[macro_use] extern crate approx;
1131    /// # use nalgebra::{UnitQuaternion, Vector3};
1132    /// let rot1 = UnitQuaternion::from_axis_angle(&Vector3::y_axis(), 1.0);
1133    /// let rot2 = UnitQuaternion::from_axis_angle(&Vector3::x_axis(), 0.1);
1134    /// assert_relative_eq!(rot1.angle_to(&rot2), 1.0045657, epsilon = 1.0e-6);
1135    /// ```
1136    #[inline]
1137    #[must_use]
1138    pub fn angle_to(&self, other: &Self) -> T {
1139        let delta = self.rotation_to(other);
1140        delta.angle()
1141    }
1142
1143    /// The unit quaternion needed to make `self` and `other` coincide.
1144    ///
1145    /// The result is such that: `self.rotation_to(other) * self == other`.
1146    ///
1147    /// # Example
1148    /// ```
1149    /// # #[macro_use] extern crate approx;
1150    /// # use nalgebra::{UnitQuaternion, Vector3};
1151    /// let rot1 = UnitQuaternion::from_axis_angle(&Vector3::y_axis(), 1.0);
1152    /// let rot2 = UnitQuaternion::from_axis_angle(&Vector3::x_axis(), 0.1);
1153    /// let rot_to = rot1.rotation_to(&rot2);
1154    /// assert_relative_eq!(rot_to * rot1, rot2, epsilon = 1.0e-6);
1155    /// ```
1156    #[inline]
1157    #[must_use]
1158    pub fn rotation_to(&self, other: &Self) -> Self {
1159        other / self
1160    }
1161
1162    /// Linear interpolation between two unit quaternions.
1163    ///
1164    /// The result is not normalized.
1165    ///
1166    /// # Example
1167    /// ```
1168    /// # use nalgebra::{UnitQuaternion, Quaternion};
1169    /// let q1 = UnitQuaternion::new_normalize(Quaternion::new(1.0, 0.0, 0.0, 0.0));
1170    /// let q2 = UnitQuaternion::new_normalize(Quaternion::new(0.0, 1.0, 0.0, 0.0));
1171    /// assert_eq!(q1.lerp(&q2, 0.1), Quaternion::new(0.9, 0.1, 0.0, 0.0));
1172    /// ```
1173    #[inline]
1174    #[must_use]
1175    pub fn lerp(&self, other: &Self, t: T) -> Quaternion<T> {
1176        self.as_ref().lerp(other.as_ref(), t)
1177    }
1178
1179    /// Normalized linear interpolation between two unit quaternions.
1180    ///
1181    /// This is the same as `self.lerp` except that the result is normalized.
1182    ///
1183    /// # Example
1184    /// ```
1185    /// # use nalgebra::{UnitQuaternion, Quaternion};
1186    /// let q1 = UnitQuaternion::new_normalize(Quaternion::new(1.0, 0.0, 0.0, 0.0));
1187    /// let q2 = UnitQuaternion::new_normalize(Quaternion::new(0.0, 1.0, 0.0, 0.0));
1188    /// assert_eq!(q1.nlerp(&q2, 0.1), UnitQuaternion::new_normalize(Quaternion::new(0.9, 0.1, 0.0, 0.0)));
1189    /// ```
1190    #[inline]
1191    #[must_use]
1192    pub fn nlerp(&self, other: &Self, t: T) -> Self {
1193        let mut res = self.lerp(other, t);
1194        let _ = res.normalize_mut();
1195
1196        Self::new_unchecked(res)
1197    }
1198
1199    /// Spherical linear interpolation between two unit quaternions.
1200    ///
1201    /// Panics if the angle between both quaternion is 180 degrees (in which case the interpolation
1202    /// is not well-defined). Use `.try_slerp` instead to avoid the panic.
1203    ///
1204    /// # Example
1205    /// ```
1206    /// # use nalgebra::geometry::UnitQuaternion;
1207    ///
1208    /// let q1 = UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_4, 0.0, 0.0);
1209    /// let q2 = UnitQuaternion::from_euler_angles(-std::f32::consts::PI, 0.0, 0.0);
1210    ///
1211    /// let q = q1.slerp(&q2, 1.0 / 3.0);
1212    ///
1213    /// assert_eq!(q.euler_angles(), (std::f32::consts::FRAC_PI_2, 0.0, 0.0));
1214    /// ```
1215    #[inline]
1216    #[must_use]
1217    pub fn slerp(&self, other: &Self, t: T) -> Self
1218    where
1219        T: RealField,
1220    {
1221        self.try_slerp(other, t, T::default_epsilon())
1222            .expect("Quaternion slerp: ambiguous configuration.")
1223    }
1224
1225    /// Computes the spherical linear interpolation between two unit quaternions or returns `None`
1226    /// if both quaternions are approximately 180 degrees apart (in which case the interpolation is
1227    /// not well-defined).
1228    ///
1229    /// # Arguments
1230    /// * `self`: the first quaternion to interpolate from.
1231    /// * `other`: the second quaternion to interpolate toward.
1232    /// * `t`: the interpolation parameter. Should be between 0 and 1.
1233    /// * `epsilon`: the value below which the sinus of the angle separating both quaternion
1234    ///   must be to return `None`.
1235    #[inline]
1236    #[must_use]
1237    pub fn try_slerp(&self, other: &Self, t: T, epsilon: T) -> Option<Self>
1238    where
1239        T: RealField,
1240    {
1241        let coords = if self.coords.dot(&other.coords) < T::zero() {
1242            Unit::new_unchecked(self.coords.clone()).try_slerp(
1243                &Unit::new_unchecked(-other.coords.clone()),
1244                t,
1245                epsilon,
1246            )
1247        } else {
1248            Unit::new_unchecked(self.coords.clone()).try_slerp(
1249                &Unit::new_unchecked(other.coords.clone()),
1250                t,
1251                epsilon,
1252            )
1253        };
1254
1255        coords.map(|q| Unit::new_unchecked(Quaternion::from(q.into_inner())))
1256    }
1257
1258    /// Compute the conjugate of this unit quaternion in-place.
1259    #[inline]
1260    pub fn conjugate_mut(&mut self) {
1261        self.as_mut_unchecked().conjugate_mut()
1262    }
1263
1264    /// Inverts this quaternion if it is not zero.
1265    ///
1266    /// # Example
1267    /// ```
1268    /// # #[macro_use] extern crate approx;
1269    /// # use nalgebra::{UnitQuaternion, Vector3, Unit};
1270    /// let axisangle = Vector3::new(0.1, 0.2, 0.3);
1271    /// let mut rot = UnitQuaternion::new(axisangle);
1272    /// rot.inverse_mut();
1273    /// assert_relative_eq!(rot * UnitQuaternion::new(axisangle), UnitQuaternion::identity());
1274    /// assert_relative_eq!(UnitQuaternion::new(axisangle) * rot, UnitQuaternion::identity());
1275    /// ```
1276    #[inline]
1277    pub fn inverse_mut(&mut self) {
1278        self.as_mut_unchecked().conjugate_mut()
1279    }
1280
1281    /// The rotation axis of this unit quaternion or `None` if the rotation is zero.
1282    ///
1283    /// # Example
1284    /// ```
1285    /// # use nalgebra::{UnitQuaternion, Vector3, Unit};
1286    /// let axis = Unit::new_normalize(Vector3::new(1.0, 2.0, 3.0));
1287    /// let angle = 1.2;
1288    /// let rot = UnitQuaternion::from_axis_angle(&axis, angle);
1289    /// assert_eq!(rot.axis(), Some(axis));
1290    ///
1291    /// // Case with a zero angle.
1292    /// let rot = UnitQuaternion::from_axis_angle(&axis, 0.0);
1293    /// assert!(rot.axis().is_none());
1294    /// ```
1295    #[inline]
1296    #[must_use]
1297    pub fn axis(&self) -> Option<Unit<Vector3<T>>>
1298    where
1299        T: RealField,
1300    {
1301        let v = if self.quaternion().scalar() >= T::zero() {
1302            self.as_ref().vector().clone_owned()
1303        } else {
1304            -self.as_ref().vector()
1305        };
1306
1307        Unit::try_new(v, T::zero())
1308    }
1309
1310    /// The rotation axis of this unit quaternion multiplied by the rotation angle.
1311    ///
1312    /// # Example
1313    /// ```
1314    /// # #[macro_use] extern crate approx;
1315    /// # use nalgebra::{UnitQuaternion, Vector3, Unit};
1316    /// let axisangle = Vector3::new(0.1, 0.2, 0.3);
1317    /// let rot = UnitQuaternion::new(axisangle);
1318    /// assert_relative_eq!(rot.scaled_axis(), axisangle, epsilon = 1.0e-6);
1319    /// ```
1320    #[inline]
1321    #[must_use]
1322    pub fn scaled_axis(&self) -> Vector3<T>
1323    where
1324        T: RealField,
1325    {
1326        match self.axis() {
1327            Some(axis) => axis.into_inner() * self.angle(),
1328            None => Vector3::zero(),
1329        }
1330    }
1331
1332    /// The rotation axis and angle in (0, pi] of this unit quaternion.
1333    ///
1334    /// Returns `None` if the angle is zero.
1335    ///
1336    /// # Example
1337    /// ```
1338    /// # use nalgebra::{UnitQuaternion, Vector3, Unit};
1339    /// let axis = Unit::new_normalize(Vector3::new(1.0, 2.0, 3.0));
1340    /// let angle = 1.2;
1341    /// let rot = UnitQuaternion::from_axis_angle(&axis, angle);
1342    /// assert_eq!(rot.axis_angle(), Some((axis, angle)));
1343    ///
1344    /// // Case with a zero angle.
1345    /// let rot = UnitQuaternion::from_axis_angle(&axis, 0.0);
1346    /// assert!(rot.axis_angle().is_none());
1347    /// ```
1348    #[inline]
1349    #[must_use]
1350    pub fn axis_angle(&self) -> Option<(Unit<Vector3<T>>, T)>
1351    where
1352        T: RealField,
1353    {
1354        self.axis().map(|axis| (axis, self.angle()))
1355    }
1356
1357    /// Compute the exponential of a quaternion.
1358    ///
1359    /// Note that this function yields a `Quaternion<T>` because it loses the unit property.
1360    #[inline]
1361    #[must_use]
1362    pub fn exp(&self) -> Quaternion<T> {
1363        self.as_ref().exp()
1364    }
1365
1366    /// Compute the natural logarithm of a quaternion.
1367    ///
1368    /// Note that this function yields a `Quaternion<T>` because it loses the unit property.
1369    /// The vector part of the return value corresponds to the axis-angle representation (divided
1370    /// by 2.0) of this unit quaternion.
1371    ///
1372    /// # Example
1373    /// ```
1374    /// # #[macro_use] extern crate approx;
1375    /// # use nalgebra::{Vector3, UnitQuaternion};
1376    /// let axisangle = Vector3::new(0.1, 0.2, 0.3);
1377    /// let q = UnitQuaternion::new(axisangle);
1378    /// assert_relative_eq!(q.ln().vector().into_owned(), axisangle, epsilon = 1.0e-6);
1379    /// ```
1380    #[inline]
1381    #[must_use]
1382    pub fn ln(&self) -> Quaternion<T>
1383    where
1384        T: RealField,
1385    {
1386        match self.axis() {
1387            Some(v) => Quaternion::from_imag(v.into_inner() * self.angle()),
1388            None => Quaternion::zero(),
1389        }
1390    }
1391
1392    /// Raise the quaternion to a given floating power.
1393    ///
1394    /// This returns the unit quaternion that identifies a rotation with axis `self.axis()` and
1395    /// angle `self.angle() × n`.
1396    ///
1397    /// # Example
1398    /// ```
1399    /// # #[macro_use] extern crate approx;
1400    /// # use nalgebra::{UnitQuaternion, Vector3, Unit};
1401    /// let axis = Unit::new_normalize(Vector3::new(1.0, 2.0, 3.0));
1402    /// let angle = 1.2;
1403    /// let rot = UnitQuaternion::from_axis_angle(&axis, angle);
1404    /// let pow = rot.powf(2.0);
1405    /// assert_relative_eq!(pow.axis().unwrap(), axis, epsilon = 1.0e-6);
1406    /// assert_eq!(pow.angle(), 2.4);
1407    /// ```
1408    #[inline]
1409    #[must_use]
1410    pub fn powf(&self, n: T) -> Self
1411    where
1412        T: RealField,
1413    {
1414        match self.axis() {
1415            Some(v) => Self::from_axis_angle(&v, self.angle() * n),
1416            None => Self::identity(),
1417        }
1418    }
1419
1420    /// Builds a rotation matrix from this unit quaternion.
1421    ///
1422    /// # Example
1423    /// ```
1424    /// # #[macro_use] extern crate approx;
1425    /// # use std::f32;
1426    /// # use nalgebra::{UnitQuaternion, Vector3, Matrix3};
1427    /// let q = UnitQuaternion::from_axis_angle(&Vector3::z_axis(), f32::consts::FRAC_PI_6);
1428    /// let rot = q.to_rotation_matrix();
1429    /// let expected = Matrix3::new(0.8660254, -0.5,      0.0,
1430    ///                             0.5,       0.8660254, 0.0,
1431    ///                             0.0,       0.0,       1.0);
1432    ///
1433    /// assert_relative_eq!(*rot.matrix(), expected, epsilon = 1.0e-6);
1434    /// ```
1435    #[inline]
1436    #[must_use]
1437    pub fn to_rotation_matrix(self) -> Rotation<T, 3> {
1438        let i = self.as_ref()[0].clone();
1439        let j = self.as_ref()[1].clone();
1440        let k = self.as_ref()[2].clone();
1441        let w = self.as_ref()[3].clone();
1442
1443        let ww = w.clone() * w.clone();
1444        let ii = i.clone() * i.clone();
1445        let jj = j.clone() * j.clone();
1446        let kk = k.clone() * k.clone();
1447        let ij = i.clone() * j.clone() * crate::convert(2.0f64);
1448        let wk = w.clone() * k.clone() * crate::convert(2.0f64);
1449        let wj = w.clone() * j.clone() * crate::convert(2.0f64);
1450        let ik = i.clone() * k.clone() * crate::convert(2.0f64);
1451        let jk = j * k * crate::convert(2.0f64);
1452        let wi = w * i * crate::convert(2.0f64);
1453
1454        Rotation::from_matrix_unchecked(Matrix3::new(
1455            ww.clone() + ii.clone() - jj.clone() - kk.clone(),
1456            ij.clone() - wk.clone(),
1457            wj.clone() + ik.clone(),
1458            wk + ij,
1459            ww.clone() - ii.clone() + jj.clone() - kk.clone(),
1460            jk.clone() - wi.clone(),
1461            ik - wj,
1462            wi + jk,
1463            ww - ii - jj + kk,
1464        ))
1465    }
1466
1467    /// Converts this unit quaternion into its equivalent Euler angles.
1468    ///
1469    /// The angles are produced in the form (roll, pitch, yaw).
1470    #[inline]
1471    #[deprecated(note = "This is renamed to use `.euler_angles()`.")]
1472    pub fn to_euler_angles(self) -> (T, T, T)
1473    where
1474        T: RealField,
1475    {
1476        self.euler_angles()
1477    }
1478
1479    /// Retrieves the euler angles corresponding to this unit quaternion.
1480    ///
1481    /// The angles are produced in the form (roll, pitch, yaw).
1482    ///
1483    /// # Example
1484    /// ```
1485    /// # #[macro_use] extern crate approx;
1486    /// # use nalgebra::UnitQuaternion;
1487    /// let rot = UnitQuaternion::from_euler_angles(0.1, 0.2, 0.3);
1488    /// let euler = rot.euler_angles();
1489    /// assert_relative_eq!(euler.0, 0.1, epsilon = 1.0e-6);
1490    /// assert_relative_eq!(euler.1, 0.2, epsilon = 1.0e-6);
1491    /// assert_relative_eq!(euler.2, 0.3, epsilon = 1.0e-6);
1492    /// ```
1493    #[inline]
1494    #[must_use]
1495    pub fn euler_angles(&self) -> (T, T, T)
1496    where
1497        T: RealField,
1498    {
1499        self.clone().to_rotation_matrix().euler_angles()
1500    }
1501
1502    /// Converts this unit quaternion into its equivalent homogeneous transformation matrix.
1503    ///
1504    /// # Example
1505    /// ```
1506    /// # #[macro_use] extern crate approx;
1507    /// # use std::f32;
1508    /// # use nalgebra::{UnitQuaternion, Vector3, Matrix4};
1509    /// let rot = UnitQuaternion::from_axis_angle(&Vector3::z_axis(), f32::consts::FRAC_PI_6);
1510    /// let expected = Matrix4::new(0.8660254, -0.5,      0.0, 0.0,
1511    ///                             0.5,       0.8660254, 0.0, 0.0,
1512    ///                             0.0,       0.0,       1.0, 0.0,
1513    ///                             0.0,       0.0,       0.0, 1.0);
1514    ///
1515    /// assert_relative_eq!(rot.to_homogeneous(), expected, epsilon = 1.0e-6);
1516    /// ```
1517    #[inline]
1518    #[must_use]
1519    pub fn to_homogeneous(self) -> Matrix4<T> {
1520        self.to_rotation_matrix().to_homogeneous()
1521    }
1522
1523    /// Rotate a point by this unit quaternion.
1524    ///
1525    /// This is the same as the multiplication `self * pt`.
1526    ///
1527    /// # Example
1528    /// ```
1529    /// # #[macro_use] extern crate approx;
1530    /// # use std::f32;
1531    /// # use nalgebra::{UnitQuaternion, Vector3, Point3};
1532    /// let rot = UnitQuaternion::from_axis_angle(&Vector3::y_axis(), f32::consts::FRAC_PI_2);
1533    /// let transformed_point = rot.transform_point(&Point3::new(1.0, 2.0, 3.0));
1534    ///
1535    /// assert_relative_eq!(transformed_point, Point3::new(3.0, 2.0, -1.0), epsilon = 1.0e-6);
1536    /// ```
1537    #[inline]
1538    #[must_use]
1539    pub fn transform_point(&self, pt: &Point3<T>) -> Point3<T> {
1540        self * pt
1541    }
1542
1543    /// Rotate a vector by this unit quaternion.
1544    ///
1545    /// This is the same as the multiplication `self * v`.
1546    ///
1547    /// # Example
1548    /// ```
1549    /// # #[macro_use] extern crate approx;
1550    /// # use std::f32;
1551    /// # use nalgebra::{UnitQuaternion, Vector3};
1552    /// let rot = UnitQuaternion::from_axis_angle(&Vector3::y_axis(), f32::consts::FRAC_PI_2);
1553    /// let transformed_vector = rot.transform_vector(&Vector3::new(1.0, 2.0, 3.0));
1554    ///
1555    /// assert_relative_eq!(transformed_vector, Vector3::new(3.0, 2.0, -1.0), epsilon = 1.0e-6);
1556    /// ```
1557    #[inline]
1558    #[must_use]
1559    pub fn transform_vector(&self, v: &Vector3<T>) -> Vector3<T> {
1560        self * v
1561    }
1562
1563    /// Rotate a point by the inverse of this unit quaternion. This may be
1564    /// cheaper than inverting the unit quaternion and transforming the
1565    /// point.
1566    ///
1567    /// # Example
1568    /// ```
1569    /// # #[macro_use] extern crate approx;
1570    /// # use std::f32;
1571    /// # use nalgebra::{UnitQuaternion, Vector3, Point3};
1572    /// let rot = UnitQuaternion::from_axis_angle(&Vector3::y_axis(), f32::consts::FRAC_PI_2);
1573    /// let transformed_point = rot.inverse_transform_point(&Point3::new(1.0, 2.0, 3.0));
1574    ///
1575    /// assert_relative_eq!(transformed_point, Point3::new(-3.0, 2.0, 1.0), epsilon = 1.0e-6);
1576    /// ```
1577    #[inline]
1578    #[must_use]
1579    pub fn inverse_transform_point(&self, pt: &Point3<T>) -> Point3<T> {
1580        // TODO: would it be useful performance-wise not to call inverse explicitly (i-e. implement
1581        // the inverse transformation explicitly here) ?
1582        self.inverse() * pt
1583    }
1584
1585    /// Rotate a vector by the inverse of this unit quaternion. This may be
1586    /// cheaper than inverting the unit quaternion and transforming the
1587    /// vector.
1588    ///
1589    /// # Example
1590    /// ```
1591    /// # #[macro_use] extern crate approx;
1592    /// # use std::f32;
1593    /// # use nalgebra::{UnitQuaternion, Vector3};
1594    /// let rot = UnitQuaternion::from_axis_angle(&Vector3::y_axis(), f32::consts::FRAC_PI_2);
1595    /// let transformed_vector = rot.inverse_transform_vector(&Vector3::new(1.0, 2.0, 3.0));
1596    ///
1597    /// assert_relative_eq!(transformed_vector, Vector3::new(-3.0, 2.0, 1.0), epsilon = 1.0e-6);
1598    /// ```
1599    #[inline]
1600    #[must_use]
1601    pub fn inverse_transform_vector(&self, v: &Vector3<T>) -> Vector3<T> {
1602        self.inverse() * v
1603    }
1604
1605    /// Rotate a vector by the inverse of this unit quaternion. This may be
1606    /// cheaper than inverting the unit quaternion and transforming the
1607    /// vector.
1608    ///
1609    /// # Example
1610    /// ```
1611    /// # #[macro_use] extern crate approx;
1612    /// # use std::f32;
1613    /// # use nalgebra::{UnitQuaternion, Vector3};
1614    /// let rot = UnitQuaternion::from_axis_angle(&Vector3::z_axis(), f32::consts::FRAC_PI_2);
1615    /// let transformed_vector = rot.inverse_transform_unit_vector(&Vector3::x_axis());
1616    ///
1617    /// assert_relative_eq!(transformed_vector, -Vector3::y_axis(), epsilon = 1.0e-6);
1618    /// ```
1619    #[inline]
1620    #[must_use]
1621    pub fn inverse_transform_unit_vector(&self, v: &Unit<Vector3<T>>) -> Unit<Vector3<T>> {
1622        self.inverse() * v
1623    }
1624
1625    /// Appends to `self` a rotation given in the axis-angle form, using a linearized formulation.
1626    ///
1627    /// This is faster, but approximate, way to compute `UnitQuaternion::new(axisangle) * self`.
1628    #[inline]
1629    #[must_use]
1630    pub fn append_axisangle_linearized(&self, axisangle: &Vector3<T>) -> Self {
1631        let half: T = crate::convert(0.5);
1632        let q1 = self.clone().into_inner();
1633        let q2 = Quaternion::from_imag(axisangle * half);
1634        Unit::new_normalize(&q1 + q2 * &q1)
1635    }
1636}
1637
1638impl<T: RealField> Default for UnitQuaternion<T> {
1639    fn default() -> Self {
1640        Self::identity()
1641    }
1642}
1643
1644impl<T: RealField + fmt::Display> fmt::Display for UnitQuaternion<T> {
1645    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
1646        match self.axis() {
1647            Some(axis) => {
1648                let axis = axis.into_inner();
1649                write!(
1650                    f,
1651                    "UnitQuaternion angle: {} − axis: ({}, {}, {})",
1652                    self.angle(),
1653                    axis[0],
1654                    axis[1],
1655                    axis[2]
1656                )
1657            }
1658            None => {
1659                write!(
1660                    f,
1661                    "UnitQuaternion angle: {} − axis: (undefined)",
1662                    self.angle()
1663                )
1664            }
1665        }
1666    }
1667}
1668
1669impl<T: RealField + AbsDiffEq<Epsilon = T>> AbsDiffEq for UnitQuaternion<T> {
1670    type Epsilon = T;
1671
1672    #[inline]
1673    fn default_epsilon() -> Self::Epsilon {
1674        T::default_epsilon()
1675    }
1676
1677    #[inline]
1678    fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
1679        self.as_ref().abs_diff_eq(other.as_ref(), epsilon)
1680    }
1681}
1682
1683impl<T: RealField + RelativeEq<Epsilon = T>> RelativeEq for UnitQuaternion<T> {
1684    #[inline]
1685    fn default_max_relative() -> Self::Epsilon {
1686        T::default_max_relative()
1687    }
1688
1689    #[inline]
1690    fn relative_eq(
1691        &self,
1692        other: &Self,
1693        epsilon: Self::Epsilon,
1694        max_relative: Self::Epsilon,
1695    ) -> bool {
1696        self.as_ref()
1697            .relative_eq(other.as_ref(), epsilon, max_relative)
1698    }
1699}
1700
1701impl<T: RealField + UlpsEq<Epsilon = T>> UlpsEq for UnitQuaternion<T> {
1702    #[inline]
1703    fn default_max_ulps() -> u32 {
1704        T::default_max_ulps()
1705    }
1706
1707    #[inline]
1708    fn ulps_eq(&self, other: &Self, epsilon: Self::Epsilon, max_ulps: u32) -> bool {
1709        self.as_ref().ulps_eq(other.as_ref(), epsilon, max_ulps)
1710    }
1711}