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

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

use num_traits::{One, Zero};

use super::set_identity;

/// # Example
/// ```
/// let mut m = mat4::new_identity::<f32>();
/// mat4::inv(&mut m, &mat4::new_identity::<f32>());
/// assert_eq!(m, mat4::new_identity::<f32>());
/// ```
#[inline]
pub fn inv<'out, T>(out: &'out mut [T; 16], a: &[T; 16]) -> &'out mut [T; 16]
where
    T: One + Zero + Add<T, Output = T> + Sub<T, Output = T>,
    for<'a, 'b> &'a T: Mul<&'b T, Output = T> + Div<&'b T, Output = T>,
{
    let a00 = &a[0];
    let a01 = &a[1];
    let a02 = &a[2];
    let a03 = &a[3];
    let a10 = &a[4];
    let a11 = &a[5];
    let a12 = &a[6];
    let a13 = &a[7];
    let a20 = &a[8];
    let a21 = &a[9];
    let a22 = &a[10];
    let a23 = &a[11];
    let a30 = &a[12];
    let a31 = &a[13];
    let a32 = &a[14];
    let a33 = &a[15];

    let b00 = a00 * a11 - a01 * a10;
    let b01 = a00 * a12 - a02 * a10;
    let b02 = a00 * a13 - a03 * a10;
    let b03 = a01 * a12 - a02 * a11;
    let b04 = a01 * a13 - a03 * a11;
    let b05 = a02 * a13 - a03 * a12;
    let b06 = a20 * a31 - a21 * a30;
    let b07 = a20 * a32 - a22 * a30;
    let b08 = a20 * a33 - a23 * a30;
    let b09 = a21 * a32 - a22 * a31;
    let b10 = a21 * a33 - a23 * a31;
    let b11 = a22 * a33 - a23 * a32;

    let det = &b00 * &b11 - &b01 * &b10 + &b02 * &b09 + &b03 * &b08 - &b04 * &b07 + &b05 * &b06;

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

        out[0] = &(a11 * &b11 - a12 * &b10 + a13 * &b09) * &inv_det;
        out[1] = &(a02 * &b10 - a01 * &b11 - a03 * &b09) * &inv_det;
        out[2] = &(a31 * &b05 - a32 * &b04 + a33 * &b03) * &inv_det;
        out[3] = &(a22 * &b04 - a21 * &b05 - a23 * &b03) * &inv_det;
        out[4] = &(a12 * &b08 - a10 * &b11 - a13 * &b07) * &inv_det;
        out[5] = &(a00 * &b11 - a02 * &b08 + a03 * &b07) * &inv_det;
        out[6] = &(a32 * &b02 - a30 * &b05 - a33 * &b01) * &inv_det;
        out[7] = &(a20 * &b05 - a22 * &b02 + a23 * &b01) * &inv_det;
        out[8] = &(a10 * &b10 - a11 * &b08 + a13 * &b06) * &inv_det;
        out[9] = &(a01 * &b08 - a00 * &b10 - a03 * &b06) * &inv_det;
        out[10] = &(a30 * &b04 - a31 * &b02 + a33 * &b00) * &inv_det;
        out[11] = &(a21 * &b02 - a20 * &b04 - a23 * &b00) * &inv_det;
        out[12] = &(a11 * &b07 - a10 * &b09 - a12 * &b06) * &inv_det;
        out[13] = &(a00 * &b09 - a01 * &b07 + a02 * &b06) * &inv_det;
        out[14] = &(a31 * &b01 - a30 * &b03 - a32 * &b00) * &inv_det;
        out[15] = &(a20 * &b03 - a21 * &b01 + a22 * &b00) * &inv_det;

        out
    }
}
/// # Example
/// ```
/// let mut m = mat4::new_identity::<f32>();
/// mat4::inv_mut(&mut m);
/// assert_eq!(m, mat4::new_identity::<f32>());
/// ```
#[inline]
pub fn inv_mut<'out, T>(out: &'out mut [T; 16]) -> &'out mut [T; 16]
where
    T: Clone + One + Zero + Add<T, Output = T> + Sub<T, Output = T>,
    for<'a, 'b> &'a T: Mul<&'b T, Output = T> + Div<&'b T, Output = T>,
{
    let tmp = out.clone();
    inv(out, &tmp)
}