quat 0.2.1

quat
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100

use core::ops::{AddAssign, Div, Mul, MulAssign, Sub};

use cast_trait::Cast;
use num_traits::real::Real;

use super::norm;

pub use vec4::lerp;

/// # Example
/// ```
/// use std::f32::consts::FRAC_2_PI;
/// let mut q = quat::new_one::<f32>();
///
/// let a = quat::new_identity();
/// let mut b = quat::new_identity();
/// quat::rotate_z_mut(&mut b, &FRAC_2_PI);
///
/// quat::nlerp(&mut q, &a, &b, &0.5_f32);
/// assert_eq!(q, [0.4829259, 0.4829259, 0.55849457, 0.47079617]);
/// ```
#[inline]
pub fn nlerp<'out, T, F>(out: &'out mut [T; 4], a: &[T; 4], b: &[T; 4], t: &F) -> &'out mut [T; 4]
where
    T: Real + Cast<F>,
    F: Clone + Cast<T>,
    for<'a> T: AddAssign<&'a T>,
    for<'a> F: MulAssign<&'a F>,
    for<'a, 'b> &'a T: Sub<&'b T, Output = T> + Mul<&'b T, Output = T> + Div<&'b T, Output = T>,
{
    let mut tmp = out.clone();
    lerp::<T, F>(&mut tmp, a, b, t);
    norm::<T>(out, &tmp);
    out
}
/// # Example
/// ```
/// use std::f32::consts::FRAC_2_PI;
/// let mut q = quat::new_one::<f32>();
///
/// let a = quat::new_identity();
/// let mut b = quat::new_identity();
/// quat::rotate_z_mut(&mut b, &FRAC_2_PI);
///
/// quat::slerp(&mut q, &a, &b, &0.5_f32);
/// assert_eq!(q, [0.0, 0.0, 0.15848388, 0.98736155]);
/// ```
#[inline]
pub fn slerp<'a, T, F>(out: &'a mut [T; 4], a: &[T; 4], b: &[T; 4], t: &F) -> &'a mut [T; 4]
where
    T: Clone + Cast<F>,
    F: Real + Cast<T>,
{
    let ax = a[0].clone().cast();
    let ay = a[1].clone().cast();
    let az = a[2].clone().cast();
    let aw = a[3].clone().cast();
    let mut bx = b[0].clone().cast();
    let mut by = b[1].clone().cast();
    let mut bz = b[2].clone().cast();
    let mut bw = b[3].clone().cast();

    let mut cosom = ax * bx + ay * by + az * bz + aw * bw;
    let mut sinom;
    let omega;
    let scale0;
    let scale1;

    if cosom < F::zero() {
        cosom = -cosom;
        bx = -bx;
        by = -by;
        bz = -bz;
        bw = -bw;
    }

    if F::one() - cosom > F::zero() {
        omega = cosom.acos();

        sinom = omega.sin();
        sinom = if sinom != F::zero() {
            F::one() / sinom
        } else {
            sinom
        };

        scale0 = ((F::one() - *t) * omega).sin() * sinom;
        scale1 = (*t * omega).sin() * sinom;
    } else {
        scale0 = F::one() - *t;
        scale1 = *t;
    }

    out[0] = (scale0 * ax + scale1 * bx).cast();
    out[1] = (scale0 * ay + scale1 * by).cast();
    out[2] = (scale0 * az + scale1 * bz).cast();
    out[3] = (scale0 * aw + scale1 * bw).cast();

    out
}