nalgebra/geometry/
quaternion_construction.rs

1#[cfg(feature = "arbitrary")]
2use crate::base::dimension::U4;
3#[cfg(feature = "arbitrary")]
4use crate::base::storage::Owned;
5#[cfg(feature = "arbitrary")]
6use quickcheck::{Arbitrary, Gen};
7
8#[cfg(feature = "rand-no-std")]
9use rand::{
10    Rng,
11    distr::{Distribution, OpenClosed01, StandardUniform, Uniform, uniform::SampleUniform},
12};
13
14use num::{One, Zero};
15
16use simba::scalar::{RealField, SupersetOf};
17use simba::simd::SimdBool;
18
19use crate::base::dimension::U3;
20use crate::base::storage::Storage;
21use crate::base::{Matrix3, Matrix4, Unit, Vector, Vector3, Vector4};
22use crate::{Scalar, SimdRealField};
23
24use crate::geometry::{Quaternion, Rotation3, UnitQuaternion};
25
26impl<T> Quaternion<T> {
27    /// Creates a quaternion from a 4D vector. The quaternion scalar part corresponds to the `w`
28    /// vector component.
29    #[inline]
30    // #[deprecated(note = "Use `::from` instead.")] // Don't deprecate because this one can be a const-fn.
31    pub const fn from_vector(vector: Vector4<T>) -> Self {
32        Self { coords: vector }
33    }
34
35    /// Creates a new quaternion from its individual components. Note that the arguments order does
36    /// **not** follow the storage order.
37    ///
38    /// The storage order is `[ i, j, k, w ]` while the arguments for this functions are in the
39    /// order `(w, i, j, k)`.
40    ///
41    /// # Example
42    /// ```
43    /// # use nalgebra::{Quaternion, Vector4};
44    /// let q = Quaternion::new(1.0, 2.0, 3.0, 4.0);
45    /// assert!(q.i == 2.0 && q.j == 3.0 && q.k == 4.0 && q.w == 1.0);
46    /// assert_eq!(*q.as_vector(), Vector4::new(2.0, 3.0, 4.0, 1.0));
47    /// ```
48    #[inline]
49    pub const fn new(w: T, i: T, j: T, k: T) -> Self {
50        Self::from_vector(Vector4::new(i, j, k, w))
51    }
52
53    /// Cast the components of `self` to another type.
54    ///
55    /// # Example
56    /// ```
57    /// # use nalgebra::Quaternion;
58    /// let q = Quaternion::new(1.0f64, 2.0, 3.0, 4.0);
59    /// let q2 = q.cast::<f32>();
60    /// assert_eq!(q2, Quaternion::new(1.0f32, 2.0, 3.0, 4.0));
61    /// ```
62    pub fn cast<To>(self) -> Quaternion<To>
63    where
64        T: Scalar,
65        To: Scalar + SupersetOf<T>,
66    {
67        crate::convert(self)
68    }
69}
70
71impl<T: SimdRealField> Quaternion<T> {
72    /// Constructs a pure quaternion.
73    #[inline]
74    pub fn from_imag(vector: Vector3<T>) -> Self {
75        Self::from_parts(T::zero(), vector)
76    }
77
78    /// Creates a new quaternion from its scalar and vector parts. Note that the arguments order does
79    /// **not** follow the storage order.
80    ///
81    /// The storage order is [ vector, scalar ].
82    ///
83    /// # Example
84    /// ```
85    /// # use nalgebra::{Quaternion, Vector3, Vector4};
86    /// let w = 1.0;
87    /// let ijk = Vector3::new(2.0, 3.0, 4.0);
88    /// let q = Quaternion::from_parts(w, ijk);
89    /// assert!(q.i == 2.0 && q.j == 3.0 && q.k == 4.0 && q.w == 1.0);
90    /// assert_eq!(*q.as_vector(), Vector4::new(2.0, 3.0, 4.0, 1.0));
91    /// ```
92    #[inline]
93    // TODO: take a reference to `vector`?
94    pub fn from_parts<SB>(scalar: T, vector: Vector<T, U3, SB>) -> Self
95    where
96        SB: Storage<T, U3>,
97    {
98        Self::new(
99            scalar,
100            vector[0].clone(),
101            vector[1].clone(),
102            vector[2].clone(),
103        )
104    }
105
106    /// Constructs a real quaternion.
107    #[inline]
108    pub fn from_real(r: T) -> Self {
109        Self::from_parts(r, Vector3::zero())
110    }
111
112    /// The quaternion multiplicative identity.
113    ///
114    /// # Example
115    /// ```
116    /// # use nalgebra::Quaternion;
117    /// let q = Quaternion::identity();
118    /// let q2 = Quaternion::new(1.0, 2.0, 3.0, 4.0);
119    ///
120    /// assert_eq!(q * q2, q2);
121    /// assert_eq!(q2 * q, q2);
122    /// ```
123    #[inline]
124    pub fn identity() -> Self {
125        Self::from_real(T::one())
126    }
127}
128
129// TODO: merge with the previous block.
130impl<T: SimdRealField> Quaternion<T>
131where
132    T::Element: SimdRealField,
133{
134    /// Creates a new quaternion from its polar decomposition.
135    ///
136    /// Note that `axis` is assumed to be a unit vector.
137    // TODO: take a reference to `axis`?
138    pub fn from_polar_decomposition<SB>(scale: T, theta: T, axis: Unit<Vector<T, U3, SB>>) -> Self
139    where
140        SB: Storage<T, U3>,
141    {
142        let rot = UnitQuaternion::<T>::from_axis_angle(&axis, theta * crate::convert(2.0f64));
143
144        rot.into_inner() * scale
145    }
146}
147
148impl<T: SimdRealField> One for Quaternion<T>
149where
150    T::Element: SimdRealField,
151{
152    #[inline]
153    fn one() -> Self {
154        Self::identity()
155    }
156}
157
158impl<T: SimdRealField> Zero for Quaternion<T>
159where
160    T::Element: SimdRealField,
161{
162    #[inline]
163    fn zero() -> Self {
164        Self::from(Vector4::zero())
165    }
166
167    #[inline]
168    fn is_zero(&self) -> bool {
169        self.coords.is_zero()
170    }
171}
172
173#[cfg(feature = "rand-no-std")]
174impl<T: SimdRealField> Distribution<Quaternion<T>> for StandardUniform
175where
176    StandardUniform: Distribution<T>,
177{
178    #[inline]
179    fn sample<R: Rng + ?Sized>(&self, rng: &mut R) -> Quaternion<T> {
180        Quaternion::new(rng.random(), rng.random(), rng.random(), rng.random())
181    }
182}
183
184#[cfg(feature = "arbitrary")]
185impl<T: SimdRealField + Arbitrary> Arbitrary for Quaternion<T>
186where
187    Owned<T, U4>: Send,
188{
189    #[inline]
190    fn arbitrary(g: &mut Gen) -> Self {
191        Self::new(
192            T::arbitrary(g),
193            T::arbitrary(g),
194            T::arbitrary(g),
195            T::arbitrary(g),
196        )
197    }
198}
199
200impl<T: SimdRealField> UnitQuaternion<T>
201where
202    T::Element: SimdRealField,
203{
204    /// The rotation identity.
205    ///
206    /// # Example
207    /// ```
208    /// # use nalgebra::{UnitQuaternion, Vector3, Point3};
209    /// let q = UnitQuaternion::identity();
210    /// let q2 = UnitQuaternion::new(Vector3::new(1.0, 2.0, 3.0));
211    /// let v = Vector3::new_random();
212    /// let p = Point3::from(v);
213    ///
214    /// assert_eq!(q * q2, q2);
215    /// assert_eq!(q2 * q, q2);
216    /// assert_eq!(q * v, v);
217    /// assert_eq!(q * p, p);
218    /// ```
219    #[inline]
220    pub fn identity() -> Self {
221        Self::new_unchecked(Quaternion::identity())
222    }
223
224    /// Cast the components of `self` to another type.
225    ///
226    /// # Example
227    /// ```
228    /// # use nalgebra::UnitQuaternion;
229    /// # use approx::assert_relative_eq;
230    /// let q = UnitQuaternion::from_euler_angles(1.0f64, 2.0, 3.0);
231    /// let q2 = q.cast::<f32>();
232    /// assert_relative_eq!(q2, UnitQuaternion::from_euler_angles(1.0f32, 2.0, 3.0), epsilon = 1.0e-6);
233    /// ```
234    pub fn cast<To>(self) -> UnitQuaternion<To>
235    where
236        To: Scalar + SupersetOf<T>,
237    {
238        crate::convert(self)
239    }
240
241    /// Creates a new quaternion from a unit vector (the rotation axis) and an angle
242    /// (the rotation angle).
243    ///
244    /// # Example
245    /// ```
246    /// # #[macro_use] extern crate approx;
247    /// # use std::f32;
248    /// # use nalgebra::{UnitQuaternion, Point3, Vector3};
249    /// let axis = Vector3::y_axis();
250    /// let angle = f32::consts::FRAC_PI_2;
251    /// // Point and vector being transformed in the tests.
252    /// let pt = Point3::new(4.0, 5.0, 6.0);
253    /// let vec = Vector3::new(4.0, 5.0, 6.0);
254    /// let q = UnitQuaternion::from_axis_angle(&axis, angle);
255    ///
256    /// assert_eq!(q.axis().unwrap(), axis);
257    /// assert_eq!(q.angle(), angle);
258    /// assert_relative_eq!(q * pt, Point3::new(6.0, 5.0, -4.0), epsilon = 1.0e-6);
259    /// assert_relative_eq!(q * vec, Vector3::new(6.0, 5.0, -4.0), epsilon = 1.0e-6);
260    ///
261    /// // A zero vector yields an identity.
262    /// assert_eq!(UnitQuaternion::from_scaled_axis(Vector3::<f32>::zeros()), UnitQuaternion::identity());
263    /// ```
264    #[inline]
265    pub fn from_axis_angle<SB>(axis: &Unit<Vector<T, U3, SB>>, angle: T) -> Self
266    where
267        SB: Storage<T, U3>,
268    {
269        let (sang, cang) = (angle / crate::convert(2.0f64)).simd_sin_cos();
270
271        let q = Quaternion::from_parts(cang, axis.as_ref() * sang);
272        Self::new_unchecked(q)
273    }
274
275    /// Creates a new unit quaternion from a quaternion.
276    ///
277    /// The input quaternion will be normalized.
278    #[inline]
279    pub fn from_quaternion(q: Quaternion<T>) -> Self {
280        Self::new_normalize(q)
281    }
282
283    /// Creates a new unit quaternion from Euler angles.
284    ///
285    /// The primitive rotations are applied in order: 1 roll − 2 pitch − 3 yaw.
286    ///
287    /// # Example
288    /// ```
289    /// # #[macro_use] extern crate approx;
290    /// # use nalgebra::UnitQuaternion;
291    /// let rot = UnitQuaternion::from_euler_angles(0.1, 0.2, 0.3);
292    /// let euler = rot.euler_angles();
293    /// assert_relative_eq!(euler.0, 0.1, epsilon = 1.0e-6);
294    /// assert_relative_eq!(euler.1, 0.2, epsilon = 1.0e-6);
295    /// assert_relative_eq!(euler.2, 0.3, epsilon = 1.0e-6);
296    /// ```
297    #[inline]
298    pub fn from_euler_angles(roll: T, pitch: T, yaw: T) -> Self {
299        let (sr, cr) = (roll * crate::convert(0.5f64)).simd_sin_cos();
300        let (sp, cp) = (pitch * crate::convert(0.5f64)).simd_sin_cos();
301        let (sy, cy) = (yaw * crate::convert(0.5f64)).simd_sin_cos();
302
303        let q = Quaternion::new(
304            cr.clone() * cp.clone() * cy.clone() + sr.clone() * sp.clone() * sy.clone(),
305            sr.clone() * cp.clone() * cy.clone() - cr.clone() * sp.clone() * sy.clone(),
306            cr.clone() * sp.clone() * cy.clone() + sr.clone() * cp.clone() * sy.clone(),
307            cr * cp * sy - sr * sp * cy,
308        );
309
310        Self::new_unchecked(q)
311    }
312
313    /// Builds an unit quaternion from a basis assumed to be orthonormal.
314    ///
315    /// In order to get a valid unit-quaternion, the input must be an
316    /// orthonormal basis, i.e., all vectors are normalized, and the are
317    /// all orthogonal to each other. These invariants are not checked
318    /// by this method.
319    pub fn from_basis_unchecked(basis: &[Vector3<T>; 3]) -> Self {
320        let rot = Rotation3::from_basis_unchecked(basis);
321        Self::from_rotation_matrix(&rot)
322    }
323
324    /// Builds an unit quaternion from a rotation matrix.
325    ///
326    /// # Example
327    /// ```
328    /// # #[macro_use] extern crate approx;
329    /// # use nalgebra::{Rotation3, UnitQuaternion, Vector3};
330    /// let axis = Vector3::y_axis();
331    /// let angle = 0.1;
332    /// let rot = Rotation3::from_axis_angle(&axis, angle);
333    /// let q = UnitQuaternion::from_rotation_matrix(&rot);
334    /// assert_relative_eq!(q.to_rotation_matrix(), rot, epsilon = 1.0e-6);
335    /// assert_relative_eq!(q.axis().unwrap(), rot.axis().unwrap(), epsilon = 1.0e-6);
336    /// assert_relative_eq!(q.angle(), rot.angle(), epsilon = 1.0e-6);
337    /// ```
338    #[inline]
339    pub fn from_rotation_matrix(rotmat: &Rotation3<T>) -> Self {
340        // Robust matrix to quaternion transformation.
341        // See https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion
342        let tr = rotmat[(0, 0)].clone() + rotmat[(1, 1)].clone() + rotmat[(2, 2)].clone();
343        let quarter: T = crate::convert(0.25);
344
345        let res = tr.clone().simd_gt(T::zero()).if_else3(
346            || {
347                let denom = (tr.clone() + T::one()).simd_sqrt() * crate::convert(2.0);
348                Quaternion::new(
349                    quarter.clone() * denom.clone(),
350                    (rotmat[(2, 1)].clone() - rotmat[(1, 2)].clone()) / denom.clone(),
351                    (rotmat[(0, 2)].clone() - rotmat[(2, 0)].clone()) / denom.clone(),
352                    (rotmat[(1, 0)].clone() - rotmat[(0, 1)].clone()) / denom,
353                )
354            },
355            (
356                || {
357                    rotmat[(0, 0)].clone().simd_gt(rotmat[(1, 1)].clone())
358                        & rotmat[(0, 0)].clone().simd_gt(rotmat[(2, 2)].clone())
359                },
360                || {
361                    let denom = (T::one() + rotmat[(0, 0)].clone()
362                        - rotmat[(1, 1)].clone()
363                        - rotmat[(2, 2)].clone())
364                    .simd_sqrt()
365                        * crate::convert(2.0);
366                    Quaternion::new(
367                        (rotmat[(2, 1)].clone() - rotmat[(1, 2)].clone()) / denom.clone(),
368                        quarter.clone() * denom.clone(),
369                        (rotmat[(0, 1)].clone() + rotmat[(1, 0)].clone()) / denom.clone(),
370                        (rotmat[(0, 2)].clone() + rotmat[(2, 0)].clone()) / denom,
371                    )
372                },
373            ),
374            (
375                || rotmat[(1, 1)].clone().simd_gt(rotmat[(2, 2)].clone()),
376                || {
377                    let denom = (T::one() + rotmat[(1, 1)].clone()
378                        - rotmat[(0, 0)].clone()
379                        - rotmat[(2, 2)].clone())
380                    .simd_sqrt()
381                        * crate::convert(2.0);
382                    Quaternion::new(
383                        (rotmat[(0, 2)].clone() - rotmat[(2, 0)].clone()) / denom.clone(),
384                        (rotmat[(0, 1)].clone() + rotmat[(1, 0)].clone()) / denom.clone(),
385                        quarter.clone() * denom.clone(),
386                        (rotmat[(1, 2)].clone() + rotmat[(2, 1)].clone()) / denom,
387                    )
388                },
389            ),
390            || {
391                let denom = (T::one() + rotmat[(2, 2)].clone()
392                    - rotmat[(0, 0)].clone()
393                    - rotmat[(1, 1)].clone())
394                .simd_sqrt()
395                    * crate::convert(2.0);
396                Quaternion::new(
397                    (rotmat[(1, 0)].clone() - rotmat[(0, 1)].clone()) / denom.clone(),
398                    (rotmat[(0, 2)].clone() + rotmat[(2, 0)].clone()) / denom.clone(),
399                    (rotmat[(1, 2)].clone() + rotmat[(2, 1)].clone()) / denom.clone(),
400                    quarter.clone() * denom,
401                )
402            },
403        );
404
405        Self::new_unchecked(res)
406    }
407
408    /// Builds an unit quaternion by extracting the rotation part of the given transformation `m`.
409    ///
410    /// This is an iterative method. See `.from_matrix_eps` to provide mover
411    /// convergence parameters and starting solution.
412    /// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al.
413    pub fn from_matrix(m: &Matrix3<T>) -> Self
414    where
415        T: RealField,
416    {
417        Rotation3::from_matrix(m).into()
418    }
419
420    /// Builds an unit quaternion by extracting the rotation part of the given transformation `m`.
421    ///
422    /// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al.
423    ///
424    /// # Parameters
425    ///
426    /// * `m`: the matrix from which the rotational part is to be extracted.
427    /// * `eps`: the angular errors tolerated between the current rotation and the optimal one.
428    /// * `max_iter`: the maximum number of iterations. Loops indefinitely until convergence if set to `0`.
429    /// * `guess`: an estimate of the solution. Convergence will be significantly faster if an initial solution close
430    ///   to the actual solution is provided. Can be set to `UnitQuaternion::identity()` if no other
431    ///   guesses come to mind.
432    pub fn from_matrix_eps(m: &Matrix3<T>, eps: T, max_iter: usize, guess: Self) -> Self
433    where
434        T: RealField,
435    {
436        let guess = Rotation3::from(guess);
437        Rotation3::from_matrix_eps(m, eps, max_iter, guess).into()
438    }
439
440    /// The unit quaternion needed to make `a` and `b` be collinear and point toward the same
441    /// direction. Returns `None` if both `a` and `b` are collinear and point to opposite directions, as then the
442    /// rotation desired is not unique.
443    ///
444    /// # Example
445    /// ```
446    /// # #[macro_use] extern crate approx;
447    /// # use nalgebra::{Vector3, UnitQuaternion};
448    /// let a = Vector3::new(1.0, 2.0, 3.0);
449    /// let b = Vector3::new(3.0, 1.0, 2.0);
450    /// let q = UnitQuaternion::rotation_between(&a, &b).unwrap();
451    /// assert_relative_eq!(q * a, b);
452    /// assert_relative_eq!(q.inverse() * b, a);
453    /// ```
454    #[inline]
455    pub fn rotation_between<SB, SC>(a: &Vector<T, U3, SB>, b: &Vector<T, U3, SC>) -> Option<Self>
456    where
457        T: RealField,
458        SB: Storage<T, U3>,
459        SC: Storage<T, U3>,
460    {
461        Self::scaled_rotation_between(a, b, T::one())
462    }
463
464    /// The smallest rotation needed to make `a` and `b` collinear and point toward the same
465    /// direction, raised to the power `s`.
466    ///
467    /// # Example
468    /// ```
469    /// # #[macro_use] extern crate approx;
470    /// # use nalgebra::{Vector3, UnitQuaternion};
471    /// let a = Vector3::new(1.0, 2.0, 3.0);
472    /// let b = Vector3::new(3.0, 1.0, 2.0);
473    /// let q2 = UnitQuaternion::scaled_rotation_between(&a, &b, 0.2).unwrap();
474    /// let q5 = UnitQuaternion::scaled_rotation_between(&a, &b, 0.5).unwrap();
475    /// assert_relative_eq!(q2 * q2 * q2 * q2 * q2 * a, b, epsilon = 1.0e-6);
476    /// assert_relative_eq!(q5 * q5 * a, b, epsilon = 1.0e-6);
477    /// ```
478    #[inline]
479    pub fn scaled_rotation_between<SB, SC>(
480        a: &Vector<T, U3, SB>,
481        b: &Vector<T, U3, SC>,
482        s: T,
483    ) -> Option<Self>
484    where
485        T: RealField,
486        SB: Storage<T, U3>,
487        SC: Storage<T, U3>,
488    {
489        // TODO: code duplication with Rotation.
490        match (
491            Unit::try_new(a.clone_owned(), T::zero()),
492            Unit::try_new(b.clone_owned(), T::zero()),
493        ) {
494            (Some(na), Some(nb)) => Self::scaled_rotation_between_axis(&na, &nb, s),
495            _ => Some(Self::identity()),
496        }
497    }
498
499    /// The unit quaternion needed to make `a` and `b` be collinear and point toward the same
500    /// direction.
501    ///
502    /// # Example
503    /// ```
504    /// # #[macro_use] extern crate approx;
505    /// # use nalgebra::{Unit, Vector3, UnitQuaternion};
506    /// let a = Unit::new_normalize(Vector3::new(1.0, 2.0, 3.0));
507    /// let b = Unit::new_normalize(Vector3::new(3.0, 1.0, 2.0));
508    /// let q = UnitQuaternion::rotation_between(&a, &b).unwrap();
509    /// assert_relative_eq!(q * a, b);
510    /// assert_relative_eq!(q.inverse() * b, a);
511    /// ```
512    #[inline]
513    pub fn rotation_between_axis<SB, SC>(
514        a: &Unit<Vector<T, U3, SB>>,
515        b: &Unit<Vector<T, U3, SC>>,
516    ) -> Option<Self>
517    where
518        T: RealField,
519        SB: Storage<T, U3>,
520        SC: Storage<T, U3>,
521    {
522        Self::scaled_rotation_between_axis(a, b, T::one())
523    }
524
525    /// The smallest rotation needed to make `a` and `b` collinear and point toward the same
526    /// direction, raised to the power `s`.
527    ///
528    /// # Example
529    /// ```
530    /// # #[macro_use] extern crate approx;
531    /// # use nalgebra::{Unit, Vector3, UnitQuaternion};
532    /// let a = Unit::new_normalize(Vector3::new(1.0, 2.0, 3.0));
533    /// let b = Unit::new_normalize(Vector3::new(3.0, 1.0, 2.0));
534    /// let q2 = UnitQuaternion::scaled_rotation_between(&a, &b, 0.2).unwrap();
535    /// let q5 = UnitQuaternion::scaled_rotation_between(&a, &b, 0.5).unwrap();
536    /// assert_relative_eq!(q2 * q2 * q2 * q2 * q2 * a, b, epsilon = 1.0e-6);
537    /// assert_relative_eq!(q5 * q5 * a, b, epsilon = 1.0e-6);
538    /// ```
539    #[inline]
540    pub fn scaled_rotation_between_axis<SB, SC>(
541        na: &Unit<Vector<T, U3, SB>>,
542        nb: &Unit<Vector<T, U3, SC>>,
543        s: T,
544    ) -> Option<Self>
545    where
546        T: RealField,
547        SB: Storage<T, U3>,
548        SC: Storage<T, U3>,
549    {
550        // TODO: code duplication with Rotation.
551        let c = na.cross(nb);
552
553        match Unit::try_new(c, T::default_epsilon()) {
554            Some(axis) => {
555                let cos = na.dot(nb);
556
557                // The cosinus may be out of [-1, 1] because of inaccuracies.
558                if cos <= -T::one() {
559                    None
560                } else if cos >= T::one() {
561                    Some(Self::identity())
562                } else {
563                    Some(Self::from_axis_angle(&axis, cos.acos() * s))
564                }
565            }
566            None => {
567                if na.dot(nb) < T::zero() {
568                    // PI
569                    //
570                    // The rotation axis is undefined but the angle not zero. This is not a
571                    // simple rotation.
572                    None
573                } else {
574                    // Zero
575                    Some(Self::identity())
576                }
577            }
578        }
579    }
580
581    /// Creates an unit quaternion that corresponds to the local frame of an observer standing at the
582    /// origin and looking toward `dir`.
583    ///
584    /// It maps the `z` axis to the direction `dir`.
585    ///
586    /// # Arguments
587    ///   * dir - The look direction. It does not need to be normalized.
588    ///   * up - The vertical direction. It does not need to be normalized.
589    ///     The only requirement of this parameter is to not be collinear to `dir`. Non-collinearity
590    ///     is not checked.
591    ///
592    /// # Example
593    /// ```
594    /// # #[macro_use] extern crate approx;
595    /// # use std::f32;
596    /// # use nalgebra::{UnitQuaternion, Vector3};
597    /// let dir = Vector3::new(1.0, 2.0, 3.0);
598    /// let up = Vector3::y();
599    ///
600    /// let q = UnitQuaternion::face_towards(&dir, &up);
601    /// assert_relative_eq!(q * Vector3::z(), dir.normalize());
602    /// ```
603    #[inline]
604    pub fn face_towards<SB, SC>(dir: &Vector<T, U3, SB>, up: &Vector<T, U3, SC>) -> Self
605    where
606        SB: Storage<T, U3>,
607        SC: Storage<T, U3>,
608    {
609        Self::from_rotation_matrix(&Rotation3::face_towards(dir, up))
610    }
611
612    /// Deprecated: Use [`UnitQuaternion::face_towards`] instead.
613    #[deprecated(note = "renamed to `face_towards`")]
614    pub fn new_observer_frames<SB, SC>(dir: &Vector<T, U3, SB>, up: &Vector<T, U3, SC>) -> Self
615    where
616        SB: Storage<T, U3>,
617        SC: Storage<T, U3>,
618    {
619        Self::face_towards(dir, up)
620    }
621
622    /// Builds a right-handed look-at view matrix without translation.
623    ///
624    /// It maps the view direction `dir` to the **negative** `z` axis.
625    /// This conforms to the common notion of right handed look-at matrix from the computer
626    /// graphics community.
627    ///
628    /// # Arguments
629    ///   * dir − The view direction. It does not need to be normalized.
630    ///   * up - A vector approximately aligned with required the vertical axis. It does not need
631    ///     to be normalized. The only requirement of this parameter is to not be collinear to `dir`.
632    ///
633    /// # Example
634    /// ```
635    /// # #[macro_use] extern crate approx;
636    /// # use std::f32;
637    /// # use nalgebra::{UnitQuaternion, Vector3};
638    /// let dir = Vector3::new(1.0, 2.0, 3.0);
639    /// let up = Vector3::y();
640    ///
641    /// let q = UnitQuaternion::look_at_rh(&dir, &up);
642    /// assert_relative_eq!(q * dir.normalize(), -Vector3::z());
643    /// ```
644    #[inline]
645    pub fn look_at_rh<SB, SC>(dir: &Vector<T, U3, SB>, up: &Vector<T, U3, SC>) -> Self
646    where
647        SB: Storage<T, U3>,
648        SC: Storage<T, U3>,
649    {
650        Self::face_towards(&-dir, up).inverse()
651    }
652
653    /// Builds a left-handed look-at view matrix without translation.
654    ///
655    /// It maps the view direction `dir` to the **positive** `z` axis.
656    /// This conforms to the common notion of left handed look-at matrix from the computer
657    /// graphics community.
658    ///
659    /// # Arguments
660    ///   * dir − The view direction. It does not need to be normalized.
661    ///   * up - A vector approximately aligned with required the vertical axis. The only
662    ///     requirement of this parameter is to not be collinear to `dir`.
663    ///
664    /// # Example
665    /// ```
666    /// # #[macro_use] extern crate approx;
667    /// # use std::f32;
668    /// # use nalgebra::{UnitQuaternion, Vector3};
669    /// let dir = Vector3::new(1.0, 2.0, 3.0);
670    /// let up = Vector3::y();
671    ///
672    /// let q = UnitQuaternion::look_at_lh(&dir, &up);
673    /// assert_relative_eq!(q * dir.normalize(), Vector3::z());
674    /// ```
675    #[inline]
676    pub fn look_at_lh<SB, SC>(dir: &Vector<T, U3, SB>, up: &Vector<T, U3, SC>) -> Self
677    where
678        SB: Storage<T, U3>,
679        SC: Storage<T, U3>,
680    {
681        Self::face_towards(dir, up).inverse()
682    }
683
684    /// Creates a new unit quaternion rotation from a rotation axis scaled by the rotation angle.
685    ///
686    /// If `axisangle` has a magnitude smaller than `T::default_epsilon()`, this returns the identity rotation.
687    ///
688    /// # Example
689    /// ```
690    /// # #[macro_use] extern crate approx;
691    /// # use std::f32;
692    /// # use nalgebra::{UnitQuaternion, Point3, Vector3};
693    /// let axisangle = Vector3::y() * f32::consts::FRAC_PI_2;
694    /// // Point and vector being transformed in the tests.
695    /// let pt = Point3::new(4.0, 5.0, 6.0);
696    /// let vec = Vector3::new(4.0, 5.0, 6.0);
697    /// let q = UnitQuaternion::new(axisangle);
698    ///
699    /// assert_relative_eq!(q * pt, Point3::new(6.0, 5.0, -4.0), epsilon = 1.0e-6);
700    /// assert_relative_eq!(q * vec, Vector3::new(6.0, 5.0, -4.0), epsilon = 1.0e-6);
701    ///
702    /// // A zero vector yields an identity.
703    /// assert_eq!(UnitQuaternion::new(Vector3::<f32>::zeros()), UnitQuaternion::identity());
704    /// ```
705    #[inline]
706    pub fn new<SB>(axisangle: Vector<T, U3, SB>) -> Self
707    where
708        SB: Storage<T, U3>,
709    {
710        let two: T = crate::convert(2.0f64);
711        let q = Quaternion::<T>::from_imag(axisangle / two).exp();
712        Self::new_unchecked(q)
713    }
714
715    /// Creates a new unit quaternion rotation from a rotation axis scaled by the rotation angle.
716    ///
717    /// If `axisangle` has a magnitude smaller than `eps`, this returns the identity rotation.
718    ///
719    /// # Example
720    /// ```
721    /// # #[macro_use] extern crate approx;
722    /// # use std::f32;
723    /// # use nalgebra::{UnitQuaternion, Point3, Vector3};
724    /// let axisangle = Vector3::y() * f32::consts::FRAC_PI_2;
725    /// // Point and vector being transformed in the tests.
726    /// let pt = Point3::new(4.0, 5.0, 6.0);
727    /// let vec = Vector3::new(4.0, 5.0, 6.0);
728    /// let q = UnitQuaternion::new_eps(axisangle, 1.0e-6);
729    ///
730    /// assert_relative_eq!(q * pt, Point3::new(6.0, 5.0, -4.0), epsilon = 1.0e-6);
731    /// assert_relative_eq!(q * vec, Vector3::new(6.0, 5.0, -4.0), epsilon = 1.0e-6);
732    ///
733    /// // An almost zero vector yields an identity.
734    /// assert_eq!(UnitQuaternion::new_eps(Vector3::new(1.0e-8, 1.0e-9, 1.0e-7), 1.0e-6), UnitQuaternion::identity());
735    /// ```
736    #[inline]
737    pub fn new_eps<SB>(axisangle: Vector<T, U3, SB>, eps: T) -> Self
738    where
739        SB: Storage<T, U3>,
740    {
741        let two: T = crate::convert(2.0f64);
742        let q = Quaternion::<T>::from_imag(axisangle / two).exp_eps(eps);
743        Self::new_unchecked(q)
744    }
745
746    /// Creates a new unit quaternion rotation from a rotation axis scaled by the rotation angle.
747    ///
748    /// If `axisangle` has a magnitude smaller than `T::default_epsilon()`, this returns the identity rotation.
749    /// Same as `Self::new(axisangle)`.
750    ///
751    /// # Example
752    /// ```
753    /// # #[macro_use] extern crate approx;
754    /// # use std::f32;
755    /// # use nalgebra::{UnitQuaternion, Point3, Vector3};
756    /// let axisangle = Vector3::y() * f32::consts::FRAC_PI_2;
757    /// // Point and vector being transformed in the tests.
758    /// let pt = Point3::new(4.0, 5.0, 6.0);
759    /// let vec = Vector3::new(4.0, 5.0, 6.0);
760    /// let q = UnitQuaternion::from_scaled_axis(axisangle);
761    ///
762    /// assert_relative_eq!(q * pt, Point3::new(6.0, 5.0, -4.0), epsilon = 1.0e-6);
763    /// assert_relative_eq!(q * vec, Vector3::new(6.0, 5.0, -4.0), epsilon = 1.0e-6);
764    ///
765    /// // A zero vector yields an identity.
766    /// assert_eq!(UnitQuaternion::from_scaled_axis(Vector3::<f32>::zeros()), UnitQuaternion::identity());
767    /// ```
768    #[inline]
769    pub fn from_scaled_axis<SB>(axisangle: Vector<T, U3, SB>) -> Self
770    where
771        SB: Storage<T, U3>,
772    {
773        Self::new(axisangle)
774    }
775
776    /// Creates a new unit quaternion rotation from a rotation axis scaled by the rotation angle.
777    ///
778    /// If `axisangle` has a magnitude smaller than `eps`, this returns the identity rotation.
779    /// Same as `Self::new_eps(axisangle, eps)`.
780    ///
781    /// # Example
782    /// ```
783    /// # #[macro_use] extern crate approx;
784    /// # use std::f32;
785    /// # use nalgebra::{UnitQuaternion, Point3, Vector3};
786    /// let axisangle = Vector3::y() * f32::consts::FRAC_PI_2;
787    /// // Point and vector being transformed in the tests.
788    /// let pt = Point3::new(4.0, 5.0, 6.0);
789    /// let vec = Vector3::new(4.0, 5.0, 6.0);
790    /// let q = UnitQuaternion::from_scaled_axis_eps(axisangle, 1.0e-6);
791    ///
792    /// assert_relative_eq!(q * pt, Point3::new(6.0, 5.0, -4.0), epsilon = 1.0e-6);
793    /// assert_relative_eq!(q * vec, Vector3::new(6.0, 5.0, -4.0), epsilon = 1.0e-6);
794    ///
795    /// // An almost zero vector yields an identity.
796    /// assert_eq!(UnitQuaternion::from_scaled_axis_eps(Vector3::new(1.0e-8, 1.0e-9, 1.0e-7), 1.0e-6), UnitQuaternion::identity());
797    /// ```
798    #[inline]
799    pub fn from_scaled_axis_eps<SB>(axisangle: Vector<T, U3, SB>, eps: T) -> Self
800    where
801        SB: Storage<T, U3>,
802    {
803        Self::new_eps(axisangle, eps)
804    }
805
806    /// Create the mean unit quaternion from a data structure implementing `IntoIterator`
807    /// returning unit quaternions.
808    ///
809    /// The method will panic if the iterator does not return any quaternions.
810    ///
811    /// Algorithm from: Oshman, Yaakov, and Avishy Carmi. "Attitude estimation from vector
812    /// observations using a genetic-algorithm-embedded quaternion particle filter." Journal of
813    /// Guidance, Control, and Dynamics 29.4 (2006): 879-891.
814    ///
815    /// # Example
816    /// ```
817    /// # #[macro_use] extern crate approx;
818    /// # use std::f32;
819    /// # use nalgebra::{UnitQuaternion};
820    /// let q1 = UnitQuaternion::from_euler_angles(0.0, 0.0, 0.0);
821    /// let q2 = UnitQuaternion::from_euler_angles(-0.1, 0.0, 0.0);
822    /// let q3 = UnitQuaternion::from_euler_angles(0.1, 0.0, 0.0);
823    ///
824    /// let quat_vec = vec![q1, q2, q3];
825    /// let q_mean = UnitQuaternion::mean_of(quat_vec);
826    ///
827    /// let euler_angles_mean = q_mean.euler_angles();
828    /// assert_relative_eq!(euler_angles_mean.0, 0.0, epsilon = 1.0e-7)
829    /// ```
830    #[inline]
831    pub fn mean_of(unit_quaternions: impl IntoIterator<Item = Self>) -> Self
832    where
833        T: RealField,
834    {
835        let quaternions_matrix: Matrix4<T> = unit_quaternions
836            .into_iter()
837            .map(|q| q.as_vector() * q.as_vector().transpose())
838            .sum();
839
840        assert!(!quaternions_matrix.is_zero());
841
842        let eigen_matrix = quaternions_matrix
843            .try_symmetric_eigen(T::RealField::default_epsilon(), 10)
844            .expect("Quaternions matrix could not be diagonalized. This behavior should not be possible.");
845
846        let max_eigenvalue_index = eigen_matrix
847            .eigenvalues
848            .iter()
849            .position(|v| *v == eigen_matrix.eigenvalues.max())
850            .unwrap();
851
852        let max_eigenvector = eigen_matrix.eigenvectors.column(max_eigenvalue_index);
853        UnitQuaternion::from_quaternion(Quaternion::new(
854            max_eigenvector[0].clone(),
855            max_eigenvector[1].clone(),
856            max_eigenvector[2].clone(),
857            max_eigenvector[3].clone(),
858        ))
859    }
860}
861
862impl<T: SimdRealField> One for UnitQuaternion<T>
863where
864    T::Element: SimdRealField,
865{
866    #[inline]
867    fn one() -> Self {
868        Self::identity()
869    }
870}
871
872#[cfg(feature = "rand-no-std")]
873impl<T: SimdRealField> Distribution<UnitQuaternion<T>> for StandardUniform
874where
875    T::Element: SimdRealField,
876    OpenClosed01: Distribution<T>,
877    T: SampleUniform,
878{
879    /// Generate a uniformly distributed random rotation quaternion.
880    #[inline]
881    fn sample<R: Rng + ?Sized>(&self, rng: &mut R) -> UnitQuaternion<T> {
882        // Ken Shoemake's Subgroup Algorithm
883        // Uniform random rotations.
884        // In D. Kirk, editor, Graphics Gems III, pages 124-132. Academic, New York, 1992.
885        let x0 = rng.sample(OpenClosed01);
886        let twopi = Uniform::new(T::zero(), T::simd_two_pi())
887            .expect("Failed to construct `Uniform`, should be unreachable");
888        let theta1 = rng.sample(&twopi);
889        let theta2 = rng.sample(&twopi);
890        let s1 = theta1.clone().simd_sin();
891        let c1 = theta1.simd_cos();
892        let s2 = theta2.clone().simd_sin();
893        let c2 = theta2.simd_cos();
894        let r1 = (T::one() - x0.clone()).simd_sqrt();
895        let r2 = x0.simd_sqrt();
896        Unit::new_unchecked(Quaternion::new(
897            s1 * r1.clone(),
898            c1 * r1,
899            s2 * r2.clone(),
900            c2 * r2,
901        ))
902    }
903}
904
905#[cfg(feature = "arbitrary")]
906impl<T: RealField + Arbitrary> Arbitrary for UnitQuaternion<T>
907where
908    Owned<T, U4>: Send,
909    Owned<T, U3>: Send,
910{
911    #[inline]
912    fn arbitrary(g: &mut Gen) -> Self {
913        let axisangle = Vector3::arbitrary(g);
914        Self::from_scaled_axis(axisangle)
915    }
916}
917
918#[cfg(test)]
919#[cfg(feature = "rand")]
920mod tests {
921    use super::*;
922    use rand::SeedableRng;
923    use rand_xorshift;
924
925    #[test]
926    fn random_unit_quats_are_unit() {
927        let mut rng = rand_xorshift::XorShiftRng::from_seed([0xAB; 16]);
928        for _ in 0..1000 {
929            let x = rng.random::<UnitQuaternion<f32>>();
930            assert!(relative_eq!(x.into_inner().norm(), 1.0))
931        }
932    }
933}