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