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