use core::ops::{Add, Div, Mul, Sub};
use num_traits::{One, Zero};
use super::set_identity;
#[inline]
pub fn inv_mat3_values<'out, T>(
out: &'out mut [T; 9],
m00: &T,
m01: &T,
m02: &T,
m10: &T,
m11: &T,
m12: &T,
m20: &T,
m21: &T,
m22: &T,
) -> &'out mut [T; 9]
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 m0 = m11 * m22 - m12 * m21;
let m3 = m02 * m21 - m01 * m22;
let m6 = m01 * m12 - m02 * m11;
let d = m00 * &m0 + m10 * &m3 + m20 * &m6;
if d.is_zero() {
set_identity(out)
} else {
let inv_d = &T::one() / &d;
out[0] = &m0 * &inv_d;
out[1] = &(m12 * m20 - m10 * m22) * &inv_d;
out[2] = &(m10 * m21 - m11 * m20) * &inv_d;
out[3] = &m3 * &inv_d;
out[4] = &(m00 * m22 - m02 * m20) * &inv_d;
out[5] = &(m01 * m20 - m00 * m21) * &inv_d;
out[6] = &m6 * &inv_d;
out[7] = &(m02 * m10 - m00 * m12) * &inv_d;
out[8] = &(m00 * m11 - m01 * m10) * &inv_d;
out
}
}
#[inline]
pub fn inv<'out, T>(out: &'out mut [T; 9], a: &[T; 9]) -> &'out mut [T; 9]
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>,
{
inv_mat3_values(
out,
&a[0],
&a[3],
&a[6],
&a[1],
&a[4],
&a[7],
&a[2],
&a[5],
&a[8],
)
}
#[inline]
pub fn inv_mut<'out, T>(out: &'out mut [T; 9]) -> &'out mut [T; 9]
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)
}
#[inline]
pub fn inv_mat2<'out, T>(out: &'out mut [T; 9], a: &[T; 4]) -> &'out mut [T; 9]
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>,
{
inv_mat3_values(
out,
&a[0],
&a[2],
&T::zero(),
&a[1],
&a[3],
&T::zero(),
&T::zero(),
&T::zero(),
&T::one(),
)
}
#[inline]
pub fn inv_mat32<'out, T>(out: &'out mut [T; 9], a: &[T; 6]) -> &'out mut [T; 9]
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>,
{
inv_mat3_values(
out,
&a[0],
&a[2],
&T::zero(),
&a[1],
&a[3],
&T::zero(),
&T::zero(),
&T::zero(),
&T::one(),
)
}
#[inline]
pub fn inv_mat4<'out, T>(out: &'out mut [T; 9], a: &[T; 16]) -> &'out mut [T; 9]
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>,
{
inv_mat3_values(
out,
&a[0],
&a[4],
&a[8],
&a[1],
&a[5],
&a[9],
&a[2],
&a[6],
&a[10],
)
}