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 distributions::{uniform::SampleUniform, Distribution, OpenClosed01, Standard, Uniform},
11 Rng,
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: Scalar>(self) -> Quaternion<To>
63 where
64 T: Scalar,
65 To: 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 Standard
175where
176 Standard: Distribution<T>,
177{
178 #[inline]
179 fn sample<R: Rng + ?Sized>(&self, rng: &mut R) -> Quaternion<T> {
180 Quaternion::new(rng.gen(), rng.gen(), rng.gen(), rng.gen())
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: Scalar>(self) -> UnitQuaternion<To>
235 where
236 To: 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 if let (Some(na), Some(nb)) = (
491 Unit::try_new(a.clone_owned(), T::zero()),
492 Unit::try_new(b.clone_owned(), T::zero()),
493 ) {
494 Self::scaled_rotation_between_axis(&na, &nb, s)
495 } else {
496 Some(Self::identity())
497 }
498 }
499
500 /// The unit quaternion needed to make `a` and `b` be collinear and point toward the same
501 /// direction.
502 ///
503 /// # Example
504 /// ```
505 /// # #[macro_use] extern crate approx;
506 /// # use nalgebra::{Unit, Vector3, UnitQuaternion};
507 /// let a = Unit::new_normalize(Vector3::new(1.0, 2.0, 3.0));
508 /// let b = Unit::new_normalize(Vector3::new(3.0, 1.0, 2.0));
509 /// let q = UnitQuaternion::rotation_between(&a, &b).unwrap();
510 /// assert_relative_eq!(q * a, b);
511 /// assert_relative_eq!(q.inverse() * b, a);
512 /// ```
513 #[inline]
514 pub fn rotation_between_axis<SB, SC>(
515 a: &Unit<Vector<T, U3, SB>>,
516 b: &Unit<Vector<T, U3, SC>>,
517 ) -> Option<Self>
518 where
519 T: RealField,
520 SB: Storage<T, U3>,
521 SC: Storage<T, U3>,
522 {
523 Self::scaled_rotation_between_axis(a, b, T::one())
524 }
525
526 /// The smallest rotation needed to make `a` and `b` collinear and point toward the same
527 /// direction, raised to the power `s`.
528 ///
529 /// # Example
530 /// ```
531 /// # #[macro_use] extern crate approx;
532 /// # use nalgebra::{Unit, Vector3, UnitQuaternion};
533 /// let a = Unit::new_normalize(Vector3::new(1.0, 2.0, 3.0));
534 /// let b = Unit::new_normalize(Vector3::new(3.0, 1.0, 2.0));
535 /// let q2 = UnitQuaternion::scaled_rotation_between(&a, &b, 0.2).unwrap();
536 /// let q5 = UnitQuaternion::scaled_rotation_between(&a, &b, 0.5).unwrap();
537 /// assert_relative_eq!(q2 * q2 * q2 * q2 * q2 * a, b, epsilon = 1.0e-6);
538 /// assert_relative_eq!(q5 * q5 * a, b, epsilon = 1.0e-6);
539 /// ```
540 #[inline]
541 pub fn scaled_rotation_between_axis<SB, SC>(
542 na: &Unit<Vector<T, U3, SB>>,
543 nb: &Unit<Vector<T, U3, SC>>,
544 s: T,
545 ) -> Option<Self>
546 where
547 T: RealField,
548 SB: Storage<T, U3>,
549 SC: Storage<T, U3>,
550 {
551 // TODO: code duplication with Rotation.
552 let c = na.cross(nb);
553
554 if let Some(axis) = Unit::try_new(c, T::default_epsilon()) {
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 } else if na.dot(nb) < T::zero() {
566 // PI
567 //
568 // The rotation axis is undefined but the angle not zero. This is not a
569 // simple rotation.
570 None
571 } else {
572 // Zero
573 Some(Self::identity())
574 }
575 }
576
577 /// Creates an unit quaternion that corresponds to the local frame of an observer standing at the
578 /// origin and looking toward `dir`.
579 ///
580 /// It maps the `z` axis to the direction `dir`.
581 ///
582 /// # Arguments
583 /// * dir - The look direction. It does not need to be normalized.
584 /// * up - The vertical direction. It does not need to be normalized.
585 /// The only requirement of this parameter is to not be collinear to `dir`. Non-collinearity
586 /// is not checked.
587 ///
588 /// # Example
589 /// ```
590 /// # #[macro_use] extern crate approx;
591 /// # use std::f32;
592 /// # use nalgebra::{UnitQuaternion, Vector3};
593 /// let dir = Vector3::new(1.0, 2.0, 3.0);
594 /// let up = Vector3::y();
595 ///
596 /// let q = UnitQuaternion::face_towards(&dir, &up);
597 /// assert_relative_eq!(q * Vector3::z(), dir.normalize());
598 /// ```
599 #[inline]
600 pub fn face_towards<SB, SC>(dir: &Vector<T, U3, SB>, up: &Vector<T, U3, SC>) -> Self
601 where
602 SB: Storage<T, U3>,
603 SC: Storage<T, U3>,
604 {
605 Self::from_rotation_matrix(&Rotation3::face_towards(dir, up))
606 }
607
608 /// Deprecated: Use [`UnitQuaternion::face_towards`] instead.
609 #[deprecated(note = "renamed to `face_towards`")]
610 pub fn new_observer_frames<SB, SC>(dir: &Vector<T, U3, SB>, up: &Vector<T, U3, SC>) -> Self
611 where
612 SB: Storage<T, U3>,
613 SC: Storage<T, U3>,
614 {
615 Self::face_towards(dir, up)
616 }
617
618 /// Builds a right-handed look-at view matrix without translation.
619 ///
620 /// It maps the view direction `dir` to the **negative** `z` axis.
621 /// This conforms to the common notion of right handed look-at matrix from the computer
622 /// graphics community.
623 ///
624 /// # Arguments
625 /// * dir − The view direction. It does not need to be normalized.
626 /// * up - A vector approximately aligned with required the vertical axis. It does not need
627 /// to be normalized. The only requirement of this parameter is to not be collinear to `dir`.
628 ///
629 /// # Example
630 /// ```
631 /// # #[macro_use] extern crate approx;
632 /// # use std::f32;
633 /// # use nalgebra::{UnitQuaternion, Vector3};
634 /// let dir = Vector3::new(1.0, 2.0, 3.0);
635 /// let up = Vector3::y();
636 ///
637 /// let q = UnitQuaternion::look_at_rh(&dir, &up);
638 /// assert_relative_eq!(q * dir.normalize(), -Vector3::z());
639 /// ```
640 #[inline]
641 pub fn look_at_rh<SB, SC>(dir: &Vector<T, U3, SB>, up: &Vector<T, U3, SC>) -> Self
642 where
643 SB: Storage<T, U3>,
644 SC: Storage<T, U3>,
645 {
646 Self::face_towards(&-dir, up).inverse()
647 }
648
649 /// Builds a left-handed look-at view matrix without translation.
650 ///
651 /// It maps the view direction `dir` to the **positive** `z` axis.
652 /// This conforms to the common notion of left handed look-at matrix from the computer
653 /// graphics community.
654 ///
655 /// # Arguments
656 /// * dir − The view direction. It does not need to be normalized.
657 /// * up - A vector approximately aligned with required the vertical axis. The only
658 /// requirement of this parameter is to not be collinear to `dir`.
659 ///
660 /// # Example
661 /// ```
662 /// # #[macro_use] extern crate approx;
663 /// # use std::f32;
664 /// # use nalgebra::{UnitQuaternion, Vector3};
665 /// let dir = Vector3::new(1.0, 2.0, 3.0);
666 /// let up = Vector3::y();
667 ///
668 /// let q = UnitQuaternion::look_at_lh(&dir, &up);
669 /// assert_relative_eq!(q * dir.normalize(), Vector3::z());
670 /// ```
671 #[inline]
672 pub fn look_at_lh<SB, SC>(dir: &Vector<T, U3, SB>, up: &Vector<T, U3, SC>) -> Self
673 where
674 SB: Storage<T, U3>,
675 SC: Storage<T, U3>,
676 {
677 Self::face_towards(dir, up).inverse()
678 }
679
680 /// Creates a new unit quaternion rotation from a rotation axis scaled by the rotation angle.
681 ///
682 /// If `axisangle` has a magnitude smaller than `T::default_epsilon()`, this returns the identity rotation.
683 ///
684 /// # Example
685 /// ```
686 /// # #[macro_use] extern crate approx;
687 /// # use std::f32;
688 /// # use nalgebra::{UnitQuaternion, Point3, Vector3};
689 /// let axisangle = Vector3::y() * f32::consts::FRAC_PI_2;
690 /// // Point and vector being transformed in the tests.
691 /// let pt = Point3::new(4.0, 5.0, 6.0);
692 /// let vec = Vector3::new(4.0, 5.0, 6.0);
693 /// let q = UnitQuaternion::new(axisangle);
694 ///
695 /// assert_relative_eq!(q * pt, Point3::new(6.0, 5.0, -4.0), epsilon = 1.0e-6);
696 /// assert_relative_eq!(q * vec, Vector3::new(6.0, 5.0, -4.0), epsilon = 1.0e-6);
697 ///
698 /// // A zero vector yields an identity.
699 /// assert_eq!(UnitQuaternion::new(Vector3::<f32>::zeros()), UnitQuaternion::identity());
700 /// ```
701 #[inline]
702 pub fn new<SB>(axisangle: Vector<T, U3, SB>) -> Self
703 where
704 SB: Storage<T, U3>,
705 {
706 let two: T = crate::convert(2.0f64);
707 let q = Quaternion::<T>::from_imag(axisangle / two).exp();
708 Self::new_unchecked(q)
709 }
710
711 /// Creates a new unit quaternion rotation from a rotation axis scaled by the rotation angle.
712 ///
713 /// If `axisangle` has a magnitude smaller than `eps`, this returns the identity rotation.
714 ///
715 /// # Example
716 /// ```
717 /// # #[macro_use] extern crate approx;
718 /// # use std::f32;
719 /// # use nalgebra::{UnitQuaternion, Point3, Vector3};
720 /// let axisangle = Vector3::y() * f32::consts::FRAC_PI_2;
721 /// // Point and vector being transformed in the tests.
722 /// let pt = Point3::new(4.0, 5.0, 6.0);
723 /// let vec = Vector3::new(4.0, 5.0, 6.0);
724 /// let q = UnitQuaternion::new_eps(axisangle, 1.0e-6);
725 ///
726 /// assert_relative_eq!(q * pt, Point3::new(6.0, 5.0, -4.0), epsilon = 1.0e-6);
727 /// assert_relative_eq!(q * vec, Vector3::new(6.0, 5.0, -4.0), epsilon = 1.0e-6);
728 ///
729 /// // An almost zero vector yields an identity.
730 /// assert_eq!(UnitQuaternion::new_eps(Vector3::new(1.0e-8, 1.0e-9, 1.0e-7), 1.0e-6), UnitQuaternion::identity());
731 /// ```
732 #[inline]
733 pub fn new_eps<SB>(axisangle: Vector<T, U3, SB>, eps: T) -> Self
734 where
735 SB: Storage<T, U3>,
736 {
737 let two: T = crate::convert(2.0f64);
738 let q = Quaternion::<T>::from_imag(axisangle / two).exp_eps(eps);
739 Self::new_unchecked(q)
740 }
741
742 /// Creates a new unit quaternion rotation from a rotation axis scaled by the rotation angle.
743 ///
744 /// If `axisangle` has a magnitude smaller than `T::default_epsilon()`, this returns the identity rotation.
745 /// Same as `Self::new(axisangle)`.
746 ///
747 /// # Example
748 /// ```
749 /// # #[macro_use] extern crate approx;
750 /// # use std::f32;
751 /// # use nalgebra::{UnitQuaternion, Point3, Vector3};
752 /// let axisangle = Vector3::y() * f32::consts::FRAC_PI_2;
753 /// // Point and vector being transformed in the tests.
754 /// let pt = Point3::new(4.0, 5.0, 6.0);
755 /// let vec = Vector3::new(4.0, 5.0, 6.0);
756 /// let q = UnitQuaternion::from_scaled_axis(axisangle);
757 ///
758 /// assert_relative_eq!(q * pt, Point3::new(6.0, 5.0, -4.0), epsilon = 1.0e-6);
759 /// assert_relative_eq!(q * vec, Vector3::new(6.0, 5.0, -4.0), epsilon = 1.0e-6);
760 ///
761 /// // A zero vector yields an identity.
762 /// assert_eq!(UnitQuaternion::from_scaled_axis(Vector3::<f32>::zeros()), UnitQuaternion::identity());
763 /// ```
764 #[inline]
765 pub fn from_scaled_axis<SB>(axisangle: Vector<T, U3, SB>) -> Self
766 where
767 SB: Storage<T, U3>,
768 {
769 Self::new(axisangle)
770 }
771
772 /// Creates a new unit quaternion rotation from a rotation axis scaled by the rotation angle.
773 ///
774 /// If `axisangle` has a magnitude smaller than `eps`, this returns the identity rotation.
775 /// Same as `Self::new_eps(axisangle, eps)`.
776 ///
777 /// # Example
778 /// ```
779 /// # #[macro_use] extern crate approx;
780 /// # use std::f32;
781 /// # use nalgebra::{UnitQuaternion, Point3, Vector3};
782 /// let axisangle = Vector3::y() * f32::consts::FRAC_PI_2;
783 /// // Point and vector being transformed in the tests.
784 /// let pt = Point3::new(4.0, 5.0, 6.0);
785 /// let vec = Vector3::new(4.0, 5.0, 6.0);
786 /// let q = UnitQuaternion::from_scaled_axis_eps(axisangle, 1.0e-6);
787 ///
788 /// assert_relative_eq!(q * pt, Point3::new(6.0, 5.0, -4.0), epsilon = 1.0e-6);
789 /// assert_relative_eq!(q * vec, Vector3::new(6.0, 5.0, -4.0), epsilon = 1.0e-6);
790 ///
791 /// // An almost zero vector yields an identity.
792 /// assert_eq!(UnitQuaternion::from_scaled_axis_eps(Vector3::new(1.0e-8, 1.0e-9, 1.0e-7), 1.0e-6), UnitQuaternion::identity());
793 /// ```
794 #[inline]
795 pub fn from_scaled_axis_eps<SB>(axisangle: Vector<T, U3, SB>, eps: T) -> Self
796 where
797 SB: Storage<T, U3>,
798 {
799 Self::new_eps(axisangle, eps)
800 }
801
802 /// Create the mean unit quaternion from a data structure implementing `IntoIterator`
803 /// returning unit quaternions.
804 ///
805 /// The method will panic if the iterator does not return any quaternions.
806 ///
807 /// Algorithm from: Oshman, Yaakov, and Avishy Carmi. "Attitude estimation from vector
808 /// observations using a genetic-algorithm-embedded quaternion particle filter." Journal of
809 /// Guidance, Control, and Dynamics 29.4 (2006): 879-891.
810 ///
811 /// # Example
812 /// ```
813 /// # #[macro_use] extern crate approx;
814 /// # use std::f32;
815 /// # use nalgebra::{UnitQuaternion};
816 /// let q1 = UnitQuaternion::from_euler_angles(0.0, 0.0, 0.0);
817 /// let q2 = UnitQuaternion::from_euler_angles(-0.1, 0.0, 0.0);
818 /// let q3 = UnitQuaternion::from_euler_angles(0.1, 0.0, 0.0);
819 ///
820 /// let quat_vec = vec![q1, q2, q3];
821 /// let q_mean = UnitQuaternion::mean_of(quat_vec);
822 ///
823 /// let euler_angles_mean = q_mean.euler_angles();
824 /// assert_relative_eq!(euler_angles_mean.0, 0.0, epsilon = 1.0e-7)
825 /// ```
826 #[inline]
827 pub fn mean_of(unit_quaternions: impl IntoIterator<Item = Self>) -> Self
828 where
829 T: RealField,
830 {
831 let quaternions_matrix: Matrix4<T> = unit_quaternions
832 .into_iter()
833 .map(|q| q.as_vector() * q.as_vector().transpose())
834 .sum();
835
836 assert!(!quaternions_matrix.is_zero());
837
838 let eigen_matrix = quaternions_matrix
839 .try_symmetric_eigen(T::RealField::default_epsilon(), 10)
840 .expect("Quaternions matrix could not be diagonalized. This behavior should not be possible.");
841
842 let max_eigenvalue_index = eigen_matrix
843 .eigenvalues
844 .iter()
845 .position(|v| *v == eigen_matrix.eigenvalues.max())
846 .unwrap();
847
848 let max_eigenvector = eigen_matrix.eigenvectors.column(max_eigenvalue_index);
849 UnitQuaternion::from_quaternion(Quaternion::new(
850 max_eigenvector[0].clone(),
851 max_eigenvector[1].clone(),
852 max_eigenvector[2].clone(),
853 max_eigenvector[3].clone(),
854 ))
855 }
856}
857
858impl<T: SimdRealField> One for UnitQuaternion<T>
859where
860 T::Element: SimdRealField,
861{
862 #[inline]
863 fn one() -> Self {
864 Self::identity()
865 }
866}
867
868#[cfg(feature = "rand-no-std")]
869impl<T: SimdRealField> Distribution<UnitQuaternion<T>> for Standard
870where
871 T::Element: SimdRealField,
872 OpenClosed01: Distribution<T>,
873 T: SampleUniform,
874{
875 /// Generate a uniformly distributed random rotation quaternion.
876 #[inline]
877 fn sample<R: Rng + ?Sized>(&self, rng: &mut R) -> UnitQuaternion<T> {
878 // Ken Shoemake's Subgroup Algorithm
879 // Uniform random rotations.
880 // In D. Kirk, editor, Graphics Gems III, pages 124-132. Academic, New York, 1992.
881 let x0 = rng.sample(OpenClosed01);
882 let twopi = Uniform::new(T::zero(), T::simd_two_pi());
883 let theta1 = rng.sample(&twopi);
884 let theta2 = rng.sample(&twopi);
885 let s1 = theta1.clone().simd_sin();
886 let c1 = theta1.simd_cos();
887 let s2 = theta2.clone().simd_sin();
888 let c2 = theta2.simd_cos();
889 let r1 = (T::one() - x0.clone()).simd_sqrt();
890 let r2 = x0.simd_sqrt();
891 Unit::new_unchecked(Quaternion::new(
892 s1 * r1.clone(),
893 c1 * r1,
894 s2 * r2.clone(),
895 c2 * r2,
896 ))
897 }
898}
899
900#[cfg(feature = "arbitrary")]
901impl<T: RealField + Arbitrary> Arbitrary for UnitQuaternion<T>
902where
903 Owned<T, U4>: Send,
904 Owned<T, U3>: Send,
905{
906 #[inline]
907 fn arbitrary(g: &mut Gen) -> Self {
908 let axisangle = Vector3::arbitrary(g);
909 Self::from_scaled_axis(axisangle)
910 }
911}
912
913#[cfg(test)]
914#[cfg(feature = "rand")]
915mod tests {
916 use super::*;
917 use rand::SeedableRng;
918 use rand_xorshift;
919
920 #[test]
921 fn random_unit_quats_are_unit() {
922 let mut rng = rand_xorshift::XorShiftRng::from_seed([0xAB; 16]);
923 for _ in 0..1000 {
924 let x = rng.gen::<UnitQuaternion<f32>>();
925 assert!(relative_eq!(x.into_inner().norm(), 1.0))
926 }
927 }
928}