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}