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