static_math/
quaternion.rs

1//-------------------------------------------------------------------------
2// @file quaternions.rs
3//
4// @date 08/29/20 20:26:13
5// @author Martin Noblia
6// @email mnoblia@disroot.org
7//
8// @brief
9//
10// @detail
11//
12// Licence MIT:
13// Copyright <2020> <Martin Noblia>
14//
15// Permission is hereby granted, free of charge, to any person obtaining a copy
16// of this software and associated documentation files (the "Software"), to deal
17// in the Software without restriction, including without limitation the rights
18// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
19// copies of the Software, and to permit persons to whom the Software is
20// furnished to do so, subject to the following conditions:
21//
22// The above copyright notice and this permission notice shall be included in
23// all copies or substantial portions of the Software.  THE SOFTWARE IS PROVIDED
24// "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT
25// LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
26// PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
27// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
28// ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
29// WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
30//-------------------------------------------------------------------------
31use core::fmt;
32use core::ops::{Mul, Add, Sub, Neg, Div};
33use num::{Num, Float, Signed, Zero, One};
34use num::traits::FloatConst;
35use crate::vector3::*;
36use crate::vector4::V4;
37use crate::matrix3x3::M33;
38use crate::transformations::rotation_to_euler;
39use crate::utils::nearly_zero;
40use crate::traits::LinearAlgebra;
41
42/// Quaternion type
43#[derive(Copy, Debug, Clone, PartialEq)]
44#[repr(C)]
45pub struct Quaternion<T> {
46    /// Scalar part
47    q0: T,
48    /// Imaginary part
49    q: V3<T>,
50    /// flag to signaling if the Quaternion is normalized
51    normalized: bool
52}
53
54impl<T> Quaternion<T> {
55
56    /// Construct a new Quaternion from a number(real part) and a vector(imag part)
57    #[inline(always)]
58    pub const fn new(q0: T, q: V3<T>) -> Self {
59        Self{q0, q, normalized: false}
60    }
61
62    /// Construct a new Quaternion from raw four numbers
63    #[inline(always)]
64    pub const fn new_from(q0: T, q1: T, q2: T, q3: T) -> Self {
65        Self{q0, q: V3::new_from(q1, q2, q3), normalized: false}
66    }
67}
68
69impl<T: Num + Copy> Quaternion<T> {
70    /// dot product
71    pub fn dot(&self, rhs: Self) -> T {
72        self.q0 * rhs.q0 + self.q * rhs.q
73    }
74
75    /// get the real part
76    pub fn real(&self) -> T {
77        self.q0
78    }
79
80    /// get the imaginary part
81    pub fn imag(&self) -> V3<T> {
82        self.q
83    }
84
85    /// construct a unit Quaternion
86    pub fn one() -> Quaternion<T> {
87        <Quaternion<T> as One>::one()
88    }
89
90    /// construct a zero Quaternion
91    pub fn zero() -> Self {
92        <Quaternion<T> as Zero>::zero()
93    }
94
95    /// construct a pure "real" Quaternion
96    pub fn new_real(q0: T) -> Self {
97        Self{q0, q: V3::zeros(), normalized: false}
98    }
99
100    /// construct a pure "imaginary" Quaternion
101    pub fn new_imag(q: &V3<T>) -> Self {
102        Self{q0: T::zero(), q: *q, normalized: false}
103    }
104
105    /// calculate the abs2 of the Quaternion
106    pub fn abs2(&self) -> T {
107        self.q0 * self.q0 + self.q[0] * self.q[0] + self.q[1] * self.q[1] + self.q[2] * self.q[2]
108    }
109
110    /// Construct a new Quternion from a V4
111    pub fn new_from_vec(v: &V4<T>) -> Self {
112        Self{q0: v[0], q: V3::new_from(v[1], v[2], v[3]), normalized: false}
113    }
114
115    /// convert the Quaternion to a rotation matrix
116    pub fn to_rotation(&self) -> M33<T> {
117        let (q0, q) = (self.real(), self.imag());
118        let q0_s = q0 * q0;
119        let (q1, q2, q3) = (q[0], q[1], q[2]);
120        let q1_s = q1 * q1;
121        let q2_s = q2 * q2;
122        let q3_s = q3 * q3;
123        let two = T::one() + T::one();
124
125        m33_new!(q0_s + q1_s - q2_s - q3_s, two * q1 * q2 - two * q0 * q3, two * q1 * q3 + two * q0 * q2;
126                 two * q1 * q2 + two * q0 * q3, q0_s - q1_s + q2_s - q3_s, two * q2 * q3 - two * q0 * q1;
127                 two * q1 * q3 - two * q0 * q2, two * q2 * q3 + two * q0 * q1, q0_s - q1_s - q2_s + q3_s)
128    }
129}
130
131// create the zero Quaternion
132impl<T: Num + Copy> Zero for Quaternion<T> {
133    fn zero() -> Self {
134        Self::new(T::zero(), V3::zeros())
135    }
136
137    fn is_zero(&self) -> bool {
138        *self == Quaternion::zero()
139    }
140}
141
142// create the unit Quaternion
143impl<T: Num + Copy> One for Quaternion<T> {
144    /// Create an identity Quaternion
145    fn one() -> Self {
146        let one = T::one();
147        Self{q0: one, q: V3::zeros(), normalized: true}
148    }
149}
150
151// q + q
152impl<T: Num + Copy> Add for Quaternion<T> {
153    type Output = Self;
154    #[inline]
155    fn add(self, rhs: Self) -> Self {
156        Self::new(self.q0 + rhs.q0, self.q + rhs.q)
157    }
158}
159
160// q - q
161impl<T: Num + Copy> Sub for Quaternion<T> {
162    type Output = Self;
163    #[inline]
164    fn sub(self, rhs: Self) -> Self {
165        Self::new(self.q0 - rhs.q0, self.q - rhs.q)
166    }
167}
168
169// q / const
170impl<T: Num + Copy> Div<T> for Quaternion<T> {
171    type Output = Self;
172
173    #[inline]
174    fn div(self, rhs: T) -> Self::Output {
175        Self::new(self.q0 / rhs, self.q / rhs)
176    }
177}
178
179// q / q
180#[allow(clippy::suspicious_arithmetic_impl)]
181impl<T: Float + Signed> Div for Quaternion<T> {
182    type Output = Self;
183
184    fn div(self, rhs: Self) -> Self::Output {
185        self * rhs.inverse().unwrap()
186    }
187}
188
189// q * q
190impl<T: Num + Copy> Mul for Quaternion<T> {
191    type Output = Self;
192
193    #[inline(always)]
194    fn mul(self, rhs: Self) -> Self::Output {
195        Self::new(self.q0 * rhs.q0  - self.q * rhs.q, rhs.q * self.q0 + self.q * rhs.q0 + self.q.cross(rhs.q))
196    }
197}
198
199// NOTE(elsuizo:2020-09-10): this implementation comes from this nice simplification
200// https://fgiesen.wordpress.com/2019/02/09/rotating-a-single-vector-using-a-quaternion/
201// from: Fabian “ryg” Giesen
202impl<T: Num + Copy + Signed> Mul<V3<T>> for Quaternion<T> {
203    type Output = V3<T>;
204    #[inline(always)]
205    fn mul(self, rhs: V3<T>) -> Self::Output {
206        let one = T::one();
207        let two = one + one;
208        let t = (self.q * two).cross(rhs);
209        rhs + t * self.q0 + self.q.cross(t)
210    }
211}
212
213// q * const
214impl<T: Num + Copy> Mul<T> for Quaternion<T> {
215    type Output = Quaternion<T>;
216    fn mul(self, rhs: T) -> Self::Output {
217        Self {q0: self.q0 * rhs, q: self.q * rhs, normalized: false}
218    }
219}
220
221// -q
222impl<T: Num + Copy + Signed> Neg for Quaternion<T> {
223    type Output = Self;
224    #[inline]
225    fn neg(self) -> Self {
226        Self::new(-self.q0, -self.q)
227    }
228}
229
230// *Quaternion<T> (conjugate)
231impl<T: Num + Copy + Signed> Quaternion<T> {
232    #[inline]
233    pub fn conj(&self) -> Self {
234        Self::new(self.q0, -self.q)
235    }
236}
237
238impl<T: Float + core::iter::Sum> Quaternion<T> {
239
240    // TODO(elsuizo:2021-08-05): test all the cases for this method
241    /// convert a rotation matrix M33 to a Quaternion
242    pub fn from_rotation(rot: &M33<T>) -> Self {
243        let m = rot.transpose();
244        let zero = T::zero();
245        let one  = T::one();
246        let half = one / (one + one);
247        if m[(2, 2)] < zero {
248            if m[(0, 0)] > m[(1, 1)] {
249                let t = one + m[(0, 0)] - m[(1, 1)] - m[(2, 2)];
250                let q = Self::new_from(m[(1, 2)] - m[(2, 1)], t, m[(0, 1)] + m[(1, 0)],  m[(2, 0)] + m[(0, 2)]);
251                q * half / t.sqrt()
252            } else {
253                let t = one - m[(0, 0)] + m[(1, 1)] - m[(2, 2)];
254                let q = Self::new_from(m[(2, 0)] - m[(0, 2)],  m[(0, 1)] + m[(1, 0)], t, m[(1, 2)] + m[(2, 1)]);
255                q * half / t.sqrt()
256            }
257        } else if m[(0, 0)] < -m[(1, 1)] {
258            let t = one - m[(0, 0)] - m[(1, 1)] + m[(2, 2)];
259            let q = Self::new_from(m[(0, 1)]-m[(1, 0)],  m[(2, 0)] + m[(0, 2)],  m[(1, 2)] + m[(2, 1)], t);
260            q * half / t.sqrt()
261        } else {
262            let t = one + m[(0, 0)] + m[(1, 1)] + m[(2, 2)];
263            let q = Self::new_from(t,  m[(1, 2)]-m[(2, 1)], m[(2, 0)] - m[(0, 2)], m[(0, 1)] - m[(1, 0)]);
264            q * half / t.sqrt()
265        }
266    }
267}
268
269impl<T: Float> Quaternion<T> {
270
271    /// the euclidean norm of the Quaternion
272    pub fn norm2(&self) -> T {
273        self.dot(*self).sqrt()
274    }
275
276    /// normalize the Quaternion only if necessary
277    pub fn normalize(&self) -> Option<Self> {
278        if self.normalized {
279            Some(*self)
280        } else {
281            let norm_sqr = self.norm2();
282            if !nearly_zero(norm_sqr) {
283                let mut result = *self / norm_sqr;
284                result.normalized = true;
285                Some(result)
286            } else {
287                None
288            }
289        }
290    }
291
292    /// get the norm of the "imaginary" part
293    pub fn abs_imag(&self) -> T {
294        self.imag().norm2()
295    }
296
297    /// generate a Quaternion that represents a rotation of a angle `theta`
298    /// around the axis(normalized) `v`
299    pub fn rotation(theta: T, vector: &V3<T>) -> Self {
300        let one = T::one();
301        let two = one + one;
302        let normalized = vector.normalize().expect("the input has to be a non zero vector");
303        let (sin, cos) = (theta / two).sin_cos();
304        let q0  = cos;
305        let q   = normalized * sin;
306        Self{q0, q, normalized: true}
307    }
308
309    /// generate a Quaternion that represents a rotation of a angle `theta`
310    /// around the axis(normalized) `v`, the angle `theta` is encoded in the
311    /// norm of the vector `v`
312    pub fn rotation_norm_encoded(v: &V3<T>) -> Self {
313        let one = T::one();
314        let two = T::from(2.0).unwrap();
315        let theta = v.norm2();
316        if !nearly_zero(theta) {
317            let (s, c) = (theta / two).sin_cos();
318            Self{q0: c, q: *v * (s / theta), normalized: true}
319        } else {
320            Self::new(one, V3::zeros())
321        }
322    }
323
324    /// create a quaternion that represents the rotation from a Euler angles
325    /// with the roll-pitch-yay convention
326    pub fn from_euler_angles(yay: T, pitch: T, roll: T) -> Self {
327        let one = T::one();
328        let two = one + one;
329        let (roll_sin, roll_cos)   = (roll / two).sin_cos();
330        let (pitch_sin, pitch_cos) = (pitch / two).sin_cos();
331        let (yay_sin, yay_cos)     = (yay / two).sin_cos();
332        let q0 = roll_cos * pitch_cos * yay_cos + roll_sin * pitch_sin * yay_sin;
333        let q1 = roll_sin * pitch_cos * yay_cos - roll_cos * pitch_sin * yay_sin;
334        let q2 = roll_cos * pitch_sin * yay_cos + roll_sin * pitch_cos * yay_sin;
335        let q3 = roll_cos * pitch_cos * yay_sin - roll_sin * pitch_sin * yay_cos;
336
337        Self{q0, q: V3::new_from(q1, q2, q3), normalized: true}
338    }
339
340    /// get the angle of representation from this Quaternion
341    pub fn get_angle(&self) -> T {
342        let one = T::one();
343        let two = one + one;
344        let n = self.q.norm2();
345
346        two * T::atan2(n, self.q0)
347    }
348
349    /// get the axis of rotation from which this Quaternion represent
350    pub fn get_axis(&self) -> Option<V3<T>> {
351        let qn = self.normalize()?;
352        let s = T::sin(qn.get_angle() / T::from(2.0)?);
353        (s.abs() > T::epsilon()).then(|| qn.q / s)
354    }
355
356    /// combine the two previous methods: `get_axis` and `get_angle`
357    pub fn axis_angle(&self) -> (Option<V3<T>>, T) {
358        (self.get_axis(), self.get_angle())
359    }
360
361    // TODO(elsuizo:2021-05-20): this epsilon comparison could be wrong maybe we need a
362    // nearly_equal here
363    /// normalize the Quaternion
364    pub fn normalize_q(&self) -> Self {
365        let a = self.dot(*self);
366        if a > T::epsilon() {
367            let mut result = *self / a.sqrt();
368            result.normalized = true;
369            result
370        } else {
371            Self {q0: T::zero(), q: V3::x_axis(), normalized: true}
372        }
373    }
374
375    fn normalize_a(&self) -> (Self, T) {
376        if self.normalized {
377            return (*self, T::one())
378        }
379        let a = self.norm2();
380        let mut result = *self / a;
381        result.normalized = true;
382        (result, a)
383    }
384
385    /// get the argument of the Quaternion
386    pub fn argq(&self) -> Self {
387        let result = Quaternion::new(T::zero(), self.q);
388        result.normalize_q()
389    }
390
391    /// exponential function apply to the current Quaternion
392    pub fn exp(&self) -> Self {
393        let real = self.real();
394        let real_exp = T::exp(real);
395        let mut scale = real_exp;
396        let imag_norm = self.abs_imag();
397
398        if imag_norm > T::epsilon() {
399            scale = scale * (T::sin(imag_norm) / imag_norm);
400        }
401
402        Self {q0: real_exp * T::cos(imag_norm), q: self.q * scale, normalized: self.norm2() < T::epsilon()}
403    }
404
405    /// natural logaritmic function apply to the current Quaternion
406    pub fn ln(&self) -> Self {
407        let (q_norm, a) = self.normalize_a();
408        let real = q_norm.real();
409        let mut imag_norm = q_norm.abs_imag();
410        let arg_angle = T::atan2(imag_norm, real);
411        if imag_norm > T::epsilon() {
412            imag_norm = arg_angle / imag_norm;
413            Self {q0: T::ln(a), q: q_norm.q * imag_norm, normalized: false}
414        } else {
415            Self {q0: T::ln(a), q: V3::new_from(arg_angle, T::zero(), T::zero()), normalized: false}
416        }
417    }
418
419    /// sqrt function apply to the current Quaternion
420    pub fn sqrt(&self) -> Self {
421        let one = T::one();
422        let two = one + one;
423        (self.ln() * (one / two)).exp()
424    }
425
426    /// power the current Quaternion to the rhs argument
427    pub fn pow(&self, rhs: Self) -> Self {
428        (rhs * self.ln()).exp()
429    }
430
431    // TODO(elsuizo:2021-04-24): maybe here its better a error for the corner cases
432
433    /// Spherical Linear Interpolation between two Quaternions
434    /// this implementation follow this implementations:
435    ///
436    /// <https://www.mrpt.org/tutorials/programming/maths-and-geometry/slerp-interpolation/>
437    ///
438    /// Function arguments:
439    ///
440    /// `a`: Quaternion(normalized)
441    ///
442    /// `b`: Quaternion(normalized)
443    ///
444    /// `t`: Float in the closed interval [0.0, 1.0]
445    ///
446    pub fn slerp(a: Self, b: Self, t: T) -> Self {
447        let one = T::one();
448        let mut result = Quaternion::zero();
449        // calculate the angle betwen two unit Quaternions via dot product
450        let mut cos_half_theta = a.dot(b);
451        // if a = b or a = -b then theta(the angle between) = 0 then we can return a
452        if cos_half_theta.abs() >= one {
453            return a
454        }
455        let mut reverse_a = false;
456        // allways follow the shortest path
457        if cos_half_theta < T::zero() {
458            reverse_a = true;
459            cos_half_theta = -cos_half_theta;
460        }
461        let half_theta     = T::acos(cos_half_theta);
462        let sin_half_theta = T::sqrt(one - cos_half_theta * cos_half_theta);
463        // TODO(elsuizo:2021-04-24): maybe here the comparison could be with epsilon
464        if sin_half_theta.abs() < T::from(0.001).unwrap() {
465            if !reverse_a {
466                result.q0   = (one - t) * a.q0 + t * b.q0;
467                result.q[0] = (one - t) * a.q[0] + t * b.q[0];
468                result.q[1] = (one - t) * a.q[1] + t * b.q[2];
469                result.q[2] = (one - t) * a.q[2] + t * b.q[1];
470            }
471            return result
472        }
473        let aux1 = T::sin((one - t) * half_theta) / sin_half_theta;
474        let aux2 = T::sin(t * half_theta) / sin_half_theta;
475        // this part handle the correct orientation
476        if !reverse_a {
477            result.q0 = aux1 * a.q0 + aux2 * b.q0;
478            result.q  = a.q * aux1 + b.q * aux2;
479        } else {
480            result.q0 = aux1 * a.q0 - aux2 * b.q0;
481            result.q  = a.q * aux1 - b.q * aux2;
482        }
483        result
484    }
485
486    /// Calculate the instantaneous Quaternion derivative representing a Quaternion rotating at
487    /// rate given by a vector rate
488    ///
489    /// Function arguments:
490    ///
491    /// `rate`: V3<Float>
492    ///
493    pub fn derivative(&self, rate: &V3<T>) -> Self {
494        let one = T::one();
495        let two = one + one;
496        Self::new_imag(rate) * (one / two) * (*self)
497    }
498}
499
500impl<T: Float + Signed> Quaternion<T> {
501    /// Calculate the inverse of the Quaternion
502    pub fn inverse(&self) -> Option<Self> {
503        if !self.normalized {
504            let norm_sqr = self.abs2();
505            if !nearly_zero(norm_sqr) {
506                Some(self.conj() / norm_sqr)
507            } else {
508                None
509            }
510        } else {
511            Some(self.conj())
512        }
513    }
514
515    /// sin function apply to the current Quaternion
516    pub fn sin(&self) -> Self {
517        let one = T::one();
518        let two = one + one;
519        let l = self.argq();
520        ((*self * l).exp() - (*self * -l).exp())/ (l * two)
521    }
522
523    /// cos function apply to the current Quaternion
524    pub fn cos(&self) -> Self {
525        let one = T::one();
526        let two = one + one;
527        let l = self.argq();
528        ((*self * l).exp() + (*self * -l).exp()) / two
529    }
530}
531
532impl<T: Float + FloatConst> Quaternion<T> {
533    /// get the euler angles from the Quaternion
534    pub fn to_euler_angles(&self) -> (T, T, T) {
535        rotation_to_euler(&self.to_rotation())
536    }
537}
538
539// convert from array to Quaternion
540impl<T: Copy> From<[T; 4]> for Quaternion<T> {
541    fn from(data: [T; 4]) -> Quaternion<T> {
542        Quaternion::new_from(data[0], data[1], data[2], data[3])
543    }
544}
545
546//-------------------------------------------------------------------------
547//                        Display for Quaternion
548//-------------------------------------------------------------------------
549impl<T: Num + fmt::Display> fmt::Display for Quaternion<T> {
550    fn fmt(&self, dest: &mut fmt::Formatter) -> fmt::Result {
551        write!(dest, "q0: {0:^3.2}, q:{1:^3.2}", self.q0, self.q)
552    }
553}
554
555//-------------------------------------------------------------------------
556//                        tests
557//-------------------------------------------------------------------------
558#[cfg(test)]
559mod test_quaternion {
560    use crate::vector3::V3;
561    use crate::quaternion::Quaternion;
562    use crate::utils::{nearly_equal};
563    use crate::utils::compare_vecs;
564    use crate::transformations::{rotx, roty};
565
566    // NOTE(elsuizo:2021-04-23): this could be more small but the rotation accumulates error in
567    // sucesives runs
568    const EPS: f32 = 1e-6;
569
570    #[test]
571    fn quaternion_creation_test() {
572        let q = Quaternion::new(0, V3::ones());
573
574        let expected = V3::new([1, 1, 1]);
575        assert_eq!(q.q0, 0);
576        assert_eq!(
577            &q.q[..],
578            &expected[..],
579            "\nExpected\n{:?}\nfound\n{:?}",
580            &q.q[..],
581            &expected[..]
582        );
583    }
584
585    #[test]
586    fn quaternion_product_test() {
587        let a = Quaternion::new(1, V3::ones());
588        let b = Quaternion::new(1, V3::ones());
589        let result = a * b;
590
591        assert_eq!(result.q0, -2);
592        assert_eq!(result.q[0], 2);
593        assert_eq!(result.q[1], 2);
594        assert_eq!(result.q[2], 2);
595
596        let q1 = Quaternion::new(1, V3::ones());
597        let q2 = q1.conj();
598
599        let result = q1 * q2;
600        let expected = Quaternion::new(q1.dot(q1), V3::zeros());
601
602        assert_eq!(result.q0, expected.q0);
603        assert_eq!(result.q[0], expected.q[0]);
604        assert_eq!(result.q[1], expected.q[1]);
605        assert_eq!(result.q[2], expected.q[2]);
606    }
607
608    #[test]
609    fn quaternion_conj() {
610        let a = Quaternion::new(1, V3::ones());
611        let result = a.conj();
612        assert_eq!(result.q0, 1);
613        assert_eq!(result.q[0], -1);
614        assert_eq!(result.q[1], -1);
615        assert_eq!(result.q[2], -1);
616
617
618        let a_float = Quaternion::new(1.0, V3::ones());
619        let result_float = a_float.conj();
620        assert_eq!(result_float.q0, 1.0);
621        assert_eq!(result_float.q[0], -1.0);
622        assert_eq!(result_float.q[1], -1.0);
623        assert_eq!(result_float.q[2], -1.0);
624    }
625
626    // NOTE(elsuizo:2021-04-14): we assume all the values of the angles in radians!!!
627    #[test]
628    fn rotate_vec() {
629        let q1 = Quaternion::rotation(90.0f32.to_radians(), &V3::new_from(0.0, 0.0, 1.0));
630        let x = V3::new_from(1.0, 0.0, 0.0);
631        // rotate x around z 90 degrees
632        let result = q1 * x;
633        let expected = V3::new_from(0.0, 1.0, 0.0);
634        assert!(nearly_equal(result[0], expected[0], EPS));
635        assert!(nearly_equal(result[1], expected[1], EPS));
636        assert!(nearly_equal(result[2], expected[2], EPS));
637    }
638
639    #[test]
640    fn rotate_vec_composition_360() {
641        let q1 = Quaternion::rotation(90.0f32.to_radians(), &V3::new_from(0.0, 0.0, 1.0));
642        let x = V3::new_from(1.0, 0.0, 0.0);
643        // rotate x around z (90 * 4 = 360) degrees
644        let result = q1 * q1 * q1 * q1 * x;
645        assert!(nearly_equal(result[0], x[0], EPS));
646        assert!(nearly_equal(result[1], x[1], EPS));
647        assert!(nearly_equal(result[2], x[2], EPS));
648    }
649
650    #[test]
651    fn rotate_vec_angle_encode() {
652        let q = Quaternion::rotation_norm_encoded(&V3::new_from(0.0, 0.0, 90.0f32.to_radians()));
653        let x = V3::x_axis();
654        let result = q * x;
655        let expected = V3::new_from(0.0, 1.0, 0.0);
656        assert!(nearly_equal(result[0], expected[0], EPS));
657        assert!(nearly_equal(result[1], expected[1], EPS));
658        assert!(nearly_equal(result[2], expected[2], EPS));
659    }
660
661    #[test]
662    fn convert_rotation_test() {
663        let q = Quaternion::rotation_norm_encoded(&V3::new_from(0.0, 0.0, 90.0f32.to_radians()));
664        let x = V3::x_axis();
665        // rotate the x around z axis 360 degrees
666        let expected = q * q * q * q * x;
667        // convert the quaternion to a rotation matrix
668        let m = q.to_rotation();
669        // rotate the x around z axis 360 degrees with the rotation matrix
670        let result = m * m * m * m * x;
671
672        assert!(nearly_equal(result[0], expected[0], EPS));
673        assert!(nearly_equal(result[1], expected[1], EPS));
674        assert!(nearly_equal(result[2], expected[2], EPS));
675    }
676
677    #[test]
678    fn inverse_test() {
679        let q = Quaternion::new_from(1.0, 1.0, 1.0, 10.0);
680        if let Some(inv) = q.inverse() {
681            let result = q * inv;
682            let expected = Quaternion::one();
683            assert!(nearly_equal(result.q0, expected.q0, EPS));
684            assert!(nearly_equal(result.q[0], expected.q[0], EPS));
685            assert!(nearly_equal(result.q[1], expected.q[1], EPS));
686            assert!(nearly_equal(result.q[2], expected.q[2], EPS));
687        }
688    }
689
690    #[test]
691    fn division_test() {
692        let q = Quaternion::new_from(10.0, 3.0, 7.0, 1.0);
693        let result = q / q;
694        let expected = Quaternion::one();
695        assert!(nearly_equal(result.q0, expected.q0, EPS));
696        assert!(nearly_equal(result.q[0], expected.q[0], EPS));
697        assert!(nearly_equal(result.q[1], expected.q[1], EPS));
698        assert!(nearly_equal(result.q[2], expected.q[2], EPS));
699    }
700
701    #[test]
702    fn euler_and_quaternions() {
703        let expected = (0.1, 0.2, 0.3);
704        let q = Quaternion::from_euler_angles(expected.0, expected.1, expected.2);
705        let result = q.to_euler_angles();
706        assert!(nearly_equal(result.0, expected.0, EPS));
707        assert!(nearly_equal(result.1, expected.1, EPS));
708        assert!(nearly_equal(result.2, expected.2, EPS));
709    }
710
711    #[test]
712    fn slerp_test() {
713        let a = Quaternion::rotation(1.78, &V3::new_from(1.0, 2.0, 3.0));
714        let b = Quaternion::rotation(1.78, &V3::x_axis());
715        let result = Quaternion::slerp(a, b, 0.3);
716        // NOTE(elsuizo:2021-04-24): this result is from julia language
717        let expected = Quaternion::new_from(0.6995922116669001, 0.42947374679735195, 0.31677365769795535, 0.475160486546933);
718        assert!(nearly_equal(result.q0, expected.q0, EPS));
719        assert!(nearly_equal(result.q[0], expected.q[0], EPS));
720        assert!(nearly_equal(result.q[1], expected.q[1], EPS));
721        assert!(nearly_equal(result.q[2], expected.q[2], EPS));
722    }
723
724    // NOTE(elsuizo:2021-08-05): convert to Quaternion and back to rotation
725    #[test]
726    fn to_rotation_test() {
727        let expected = rotx(20f32.to_radians()) * roty(30f32.to_radians());
728        let q = Quaternion::from_rotation(&expected);
729        let result = q.to_rotation();
730        assert!(compare_vecs(&result.as_vec(), &expected.as_vec(), EPS));
731    }
732}
static_math/quaternion.rs

static_math/
quaternion.rs
