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
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158

use std::ops::*;
use serde::{Serialize, Deserialize};
use crate::traits::*;
use crate::Vec4;

#[repr(C)]
#[derive(Copy, Clone, PartialEq, Debug, Default)]
#[derive(Serialize, Deserialize)]
pub struct Mat4<T> {
    pub x: Vec4<T>,
    pub y: Vec4<T>,
    pub z: Vec4<T>,
    pub w: Vec4<T>,
}

impl<T: Number> Mat4<T> {
    pub const ZERO: Self = Self {
        x: Vec4::ZERO,
        y: Vec4::ZERO,
        z: Vec4::ZERO,
        w: Vec4::ZERO,
    };

    pub const ONE: Self = Self {
        x: Vec4::X,
        y: Vec4::Y,
        z: Vec4::Z,
        w: Vec4::W,
    };

    pub fn transpose(&self) -> Self {
        Self {
            x: Vec4::new(self.x.x, self.y.x, self.z.x, self.w.x),
            y: Vec4::new(self.x.y, self.y.y, self.z.y, self.w.y),
            z: Vec4::new(self.x.z, self.y.z, self.z.z, self.w.z),
            w: Vec4::new(self.x.w, self.y.w, self.z.w, self.w.w),
        }
    }
}

impl<T: Float> Mat4<T> {
    #[inline(always)]
    pub fn inverse(&self) -> Self {
        let a2323 = self.z.z * self.w.w - self.z.w * self.w.z;
        let a1323 = self.z.y * self.w.w - self.z.w * self.w.y;
        let a1223 = self.z.y * self.w.z - self.z.z * self.w.y;
        let a0323 = self.z.x * self.w.w - self.z.w * self.w.x;
        let a0223 = self.z.x * self.w.z - self.z.z * self.w.x;
        let a0123 = self.z.x * self.w.y - self.z.y * self.w.x;
        let a2313 = self.y.z * self.w.w - self.y.w * self.w.z;
        let a1313 = self.y.y * self.w.w - self.y.w * self.w.y;
        let a1213 = self.y.y * self.w.z - self.y.z * self.w.y;
        let a2312 = self.y.z * self.z.w - self.y.w * self.z.z;
        let a1312 = self.y.y * self.z.w - self.y.w * self.z.y;
        let a1212 = self.y.y * self.z.z - self.y.z * self.z.y;
        let a0313 = self.y.x * self.w.w - self.y.w * self.w.x;
        let a0213 = self.y.x * self.w.z - self.y.z * self.w.x;
        let a0312 = self.y.x * self.z.w - self.y.w * self.z.x;
        let a0212 = self.y.x * self.z.z - self.y.z * self.z.x;
        let a0113 = self.y.x * self.w.y - self.y.y * self.w.x;
        let a0112 = self.y.x * self.z.y - self.y.y * self.z.x;

        let det = self.x.x * (self.y.y * a2323 - self.y.z * a1323 + self.y.w * a1223) 
            - self.x.y * (self.y.x * a2323 - self.y.z * a0323 + self.y.w * a0223) 
            + self.x.z * (self.y.x * a1323 - self.y.y * a0323 + self.y.w * a0123) 
            - self.x.w * (self.y.x * a1223 - self.y.y * a0223 + self.y.z * a0123);
        let det = T::ONE / det;

        Self {
            x: Vec4 {
                x: det *   (self.y.y * a2323 - self.y.z * a1323 + self.y.w * a1223),
                y: det * - (self.x.y * a2323 - self.x.z * a1323 + self.x.w * a1223),
                z: det *   (self.x.y * a2313 - self.x.z * a1313 + self.x.w * a1213),
                w: det * - (self.x.y * a2312 - self.x.z * a1312 + self.x.w * a1212),
            },
            y: Vec4 {
                x: det * - (self.y.x * a2323 - self.y.z * a0323 + self.y.w * a0223),
                y: det *   (self.x.x * a2323 - self.x.z * a0323 + self.x.w * a0223),
                z: det * - (self.x.x * a2313 - self.x.z * a0313 + self.x.w * a0213),
                w: det *   (self.x.x * a2312 - self.x.z * a0312 + self.x.w * a0212),
            },
            z: Vec4 {
                x: det *   (self.y.x * a1323 - self.y.y * a0323 + self.y.w * a0123),
                y: det * - (self.x.x * a1323 - self.x.y * a0323 + self.x.w * a0123),
                z: det *   (self.x.x * a1313 - self.x.y * a0313 + self.x.w * a0113),
                w: det * - (self.x.x * a1312 - self.x.y * a0312 + self.x.w * a0112),
            },
            w: Vec4 {
                x: det * - (self.y.x * a1223 - self.y.y * a0223 + self.y.z * a0123),
                y: det *   (self.x.x * a1223 - self.x.y * a0223 + self.x.z * a0123),
                z: det * - (self.x.x * a1213 - self.x.y * a0213 + self.x.z * a0113),
                w: det *   (self.x.x * a1212 - self.x.y * a0212 + self.x.z * a0112),
            }
        }
    }
}

impl<T: Number> Add for Mat4<T> {
    type Output = Self;

    #[inline(always)]
    fn add(self, other: Self) -> Self::Output { 
        Self {
            x: self.x + other.x,
            y: self.y + other.y,
            z: self.z + other.z,
            w: self.w + other.w,
        }
    }
}

impl<T: Number> Mul for Mat4<T> {
    type Output = Self;

    #[inline(always)]
    fn mul(self, other: Self) -> Self::Output {
        Self {
            x: self.x * other.x.x + self.y * other.x.y + self.z * other.x.z + self.w * self.x.w,
            y: self.x * other.y.x + self.y * other.y.y + self.z * other.y.z + self.w * self.y.w,
            z: self.x * other.z.x + self.y * other.z.y + self.z * other.z.z + self.w * self.z.w,
            w: self.x * other.w.x + self.y * other.w.y + self.z * other.w.z + self.w * self.w.w,
        }
    }
}

impl<T: Number> Mul<Vec4<T>> for Mat4<T> {
    type Output = Vec4<T>;

    #[inline(always)]
    fn mul(self, other: Vec4<T>) -> Self::Output {
        Vec4 {
            x: self.x.x * other.x + self.y.x * other.y + self.z.x * other.z + self.w.x * other.w,
            y: self.x.y * other.x + self.y.y * other.y + self.z.y * other.z + self.w.y * other.w,
            z: self.x.z * other.x + self.y.z * other.y + self.z.z * other.z + self.w.z * other.w,
            w: self.x.w * other.x + self.y.w * other.y + self.z.w * other.z + self.w.w * other.w,
        }
    }
}

impl<T: Number> Mul<T> for Mat4<T> {
    type Output = Self;

    #[inline(always)]
    fn mul(self, other: T) -> Self {
        Self {
            x: self.x * other,
            y: self.y * other,
            z: self.z * other,
            w: self.w * other,
        }
    }
}

pub type Mat4b = Mat4<bool>;
pub type Mat4i = Mat4<i32>;
pub type Mat4u = Mat4<u32>;
pub type Mat4f = Mat4<f32>;
pub type Mat4d = Mat4<f64>;