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

use core::ops::{Div, Mul, Neg, Sub};

use num_traits::{One, Zero};

use super::set_identity;

/// # Example
/// ```
/// let mut m = mat32::new_identity::<f32>();
/// mat32::inv(&mut m, &mat32::new_identity());
/// assert_eq!(m, mat32::new_identity::<f32>());
/// ```
#[inline]
pub fn inv<'out, T>(out: &'out mut [T; 6], m: &[T; 6]) -> &'out mut [T; 6]
where
    T: One + Zero + Sub<T, Output = T>,
    for<'a, 'b> &'a T: Neg<Output = T> + Mul<&'b T, Output = T> + Div<&'b T, Output = T>,
{
    let m11 = &m[0];
    let m12 = &m[2];
    let m13 = &m[4];
    let m21 = &m[1];
    let m22 = &m[3];
    let m23 = &m[5];

    let d = m11 * m22 - m12 * m21;

    if d.is_zero() {
        set_identity(out)
    } else {
        let inv_d = &T::one() / &d;

        out[0] = m22 * &inv_d;
        out[1] = &-m12 * &inv_d;
        out[2] = &-m21 * &inv_d;
        out[3] = m11 * &inv_d;
        out[4] = &(m21 * m23 - m22 * m13) * &inv_d;
        out[5] = &-&(m11 * m23 - m12 * m13) * &inv_d;
        out
    }
}
/// # Example
/// ```
/// let mut m = mat32::new_identity::<f32>();
/// mat32::inv_mut(&mut m);
/// assert_eq!(m, mat32::new_identity::<f32>());
/// ```
#[inline]
pub fn inv_mut<'out, T>(out: &'out mut [T; 6]) -> &'out mut [T; 6]
where
    T: Clone + One + Zero + Sub<T, Output = T>,
    for<'a, 'b> &'a T: Neg<Output = T> + Mul<&'b T, Output = T> + Div<&'b T, Output = T>,
{
    let tmp = out.clone();
    inv(out, &tmp)
}