ferravex_math/
matrix.rs

1use crate::{Quat, Vec3, Vec4, vec3, vec4};
2use std::ops::{Add, Index, IndexMut, Mul, MulAssign, Sub};
3
4/// Column-major 4x4 matrix acting on column vectors: `translation * rotation * scale`.
5#[repr(C)]
6#[derive(Clone, Copy, Debug, PartialEq)]
7pub struct Mat4 {
8    pub x_axis: Vec4,
9    pub y_axis: Vec4,
10    pub z_axis: Vec4,
11    pub w_axis: Vec4,
12}
13impl Default for Mat4 {
14    fn default() -> Self {
15        Self::IDENTITY
16    }
17}
18impl Mat4 {
19    pub const IDENTITY: Self = Self::from_cols(Vec4::X, Vec4::Y, Vec4::Z, Vec4::W);
20    pub const ZERO: Self = Self::from_cols(Vec4::ZERO, Vec4::ZERO, Vec4::ZERO, Vec4::ZERO);
21    pub const NAN: Self = Self::from_cols(Vec4::NAN, Vec4::NAN, Vec4::NAN, Vec4::NAN);
22    pub const fn from_cols(x_axis: Vec4, y_axis: Vec4, z_axis: Vec4, w_axis: Vec4) -> Self {
23        Self {
24            x_axis,
25            y_axis,
26            z_axis,
27            w_axis,
28        }
29    }
30    pub const fn from_cols_array(v: &[f32; 16]) -> Self {
31        Self::from_cols(
32            vec4(v[0], v[1], v[2], v[3]),
33            vec4(v[4], v[5], v[6], v[7]),
34            vec4(v[8], v[9], v[10], v[11]),
35            vec4(v[12], v[13], v[14], v[15]),
36        )
37    }
38    pub const fn from_cols_array_2d(v: &[[f32; 4]; 4]) -> Self {
39        Self::from_cols(
40            Vec4::from_array(v[0]),
41            Vec4::from_array(v[1]),
42            Vec4::from_array(v[2]),
43            Vec4::from_array(v[3]),
44        )
45    }
46    pub const fn to_cols_array(self) -> [f32; 16] {
47        let a = self.to_cols_array_2d();
48        [
49            a[0][0], a[0][1], a[0][2], a[0][3], a[1][0], a[1][1], a[1][2], a[1][3], a[2][0],
50            a[2][1], a[2][2], a[2][3], a[3][0], a[3][1], a[3][2], a[3][3],
51        ]
52    }
53    pub const fn to_cols_array_2d(self) -> [[f32; 4]; 4] {
54        [
55            self.x_axis.to_array(),
56            self.y_axis.to_array(),
57            self.z_axis.to_array(),
58            self.w_axis.to_array(),
59        ]
60    }
61    pub fn col(self, index: usize) -> Vec4 {
62        self[index]
63    }
64    pub fn row(self, index: usize) -> Vec4 {
65        vec4(
66            self.x_axis[index],
67            self.y_axis[index],
68            self.z_axis[index],
69            self.w_axis[index],
70        )
71    }
72    pub fn transpose(self) -> Self {
73        Self::from_cols(self.row(0), self.row(1), self.row(2), self.row(3))
74    }
75    pub const fn from_translation(v: Vec3) -> Self {
76        Self::from_cols(Vec4::X, Vec4::Y, Vec4::Z, v.extend(1.0))
77    }
78    pub const fn from_scale(v: Vec3) -> Self {
79        Self::from_diagonal(vec4(v.x, v.y, v.z, 1.0))
80    }
81    pub const fn from_diagonal(v: Vec4) -> Self {
82        Self::from_cols(
83            vec4(v.x, 0.0, 0.0, 0.0),
84            vec4(0.0, v.y, 0.0, 0.0),
85            vec4(0.0, 0.0, v.z, 0.0),
86            vec4(0.0, 0.0, 0.0, v.w),
87        )
88    }
89    /// The quaternion is normalized to avoid embedding accidental scale.
90    pub fn from_quat(rotation: Quat) -> Self {
91        let q = rotation.normalize();
92        Self::from_cols(
93            (q * Vec3::X).extend(0.0),
94            (q * Vec3::Y).extend(0.0),
95            (q * Vec3::Z).extend(0.0),
96            Vec4::W,
97        )
98    }
99    pub fn from_axis_angle(axis: Vec3, angle: f32) -> Self {
100        Self::from_quat(Quat::from_axis_angle(axis, angle))
101    }
102    pub fn from_rotation_x(angle: f32) -> Self {
103        Self::from_quat(Quat::from_rotation_x(angle))
104    }
105    pub fn from_rotation_y(angle: f32) -> Self {
106        Self::from_quat(Quat::from_rotation_y(angle))
107    }
108    pub fn from_rotation_z(angle: f32) -> Self {
109        Self::from_quat(Quat::from_rotation_z(angle))
110    }
111    pub fn from_rotation_translation(rotation: Quat, translation: Vec3) -> Self {
112        Self::from_scale_rotation_translation(Vec3::ONE, rotation, translation)
113    }
114    pub fn from_scale_rotation_translation(scale: Vec3, rotation: Quat, translation: Vec3) -> Self {
115        let mut result = Self::from_quat(rotation);
116        result.x_axis *= scale.x;
117        result.y_axis *= scale.y;
118        result.z_axis *= scale.z;
119        result.w_axis = translation.extend(1.0);
120        result
121    }
122    pub fn transform_point3(self, v: Vec3) -> Vec3 {
123        (self * v.extend(1.0)).truncate()
124    }
125    pub fn transform_vector3(self, v: Vec3) -> Vec3 {
126        (self * v.extend(0.0)).truncate()
127    }
128    pub fn project_point3(self, v: Vec3) -> Vec3 {
129        let result = self * v.extend(1.0);
130        result.truncate() / result.w
131    }
132    pub fn mul_vec4(self, v: Vec4) -> Vec4 {
133        self.x_axis * v.x + self.y_axis * v.y + self.z_axis * v.z + self.w_axis * v.w
134    }
135    pub fn mul_mat4(self, rhs: Self) -> Self {
136        self * rhs
137    }
138    pub fn is_finite(self) -> bool {
139        self.x_axis.is_finite()
140            && self.y_axis.is_finite()
141            && self.z_axis.is_finite()
142            && self.w_axis.is_finite()
143    }
144    pub fn is_nan(self) -> bool {
145        self.x_axis.is_nan() || self.y_axis.is_nan() || self.z_axis.is_nan() || self.w_axis.is_nan()
146    }
147    pub fn abs_diff_eq(self, rhs: Self, tolerance: f32) -> bool {
148        (0..4).all(|i| self[i].abs_diff_eq(rhs[i], tolerance))
149    }
150    /// Partial-pivot Gaussian elimination with f64 intermediates.
151    pub fn determinant(self) -> f32 {
152        let mut rows = std::array::from_fn::<_, 4, _>(|r| self.row(r).to_array().map(f64::from));
153        let mut determinant = 1.0_f64;
154        for column in 0..4 {
155            let pivot = (column..4)
156                .max_by(|&a, &b| rows[a][column].abs().total_cmp(&rows[b][column].abs()))
157                .unwrap();
158            if rows[pivot][column] == 0.0 {
159                return 0.0;
160            }
161            if pivot != column {
162                rows.swap(pivot, column);
163                determinant = -determinant;
164            }
165            let divisor = rows[column][column];
166            determinant *= divisor;
167            let pivot_row = rows[column];
168            for row in rows.iter_mut().skip(column + 1) {
169                let ratio = row[column] / divisor;
170                for (value, pivot_value) in row.iter_mut().zip(pivot_row).skip(column + 1) {
171                    *value -= ratio * pivot_value;
172                }
173            }
174        }
175        determinant as f32
176    }
177    /// Returns None for singular/non-finite matrices or inverses outside f32 range.
178    pub fn try_inverse(self) -> Option<Self> {
179        if !self.is_finite() {
180            return None;
181        }
182        let mut augmented = [[0.0_f64; 8]; 4];
183        for (row, values) in augmented.iter_mut().enumerate() {
184            for (column, value) in values.iter_mut().take(4).enumerate() {
185                *value = f64::from(self[column][row]);
186            }
187            values[row + 4] = 1.0;
188        }
189        for column in 0..4 {
190            let pivot = (column..4)
191                .max_by(|&a, &b| {
192                    augmented[a][column]
193                        .abs()
194                        .total_cmp(&augmented[b][column].abs())
195                })
196                .unwrap();
197            let divisor = augmented[pivot][column];
198            if divisor == 0.0 || !divisor.is_finite() {
199                return None;
200            }
201            augmented.swap(column, pivot);
202            for value in &mut augmented[column] {
203                *value /= divisor;
204            }
205            let pivot_row = augmented[column];
206            for (row, values) in augmented.iter_mut().enumerate() {
207                if row == column {
208                    continue;
209                }
210                let coefficient = values[column];
211                for (value, pivot_value) in values.iter_mut().zip(pivot_row) {
212                    *value -= coefficient * pivot_value;
213                }
214            }
215        }
216        let mut result = Self::ZERO;
217        for row in 0..4 {
218            for column in 0..4 {
219                result[column][row] = augmented[row][column + 4] as f32;
220            }
221        }
222        result.is_finite().then_some(result)
223    }
224    pub fn inverse(self) -> Self {
225        self.try_inverse().unwrap_or(Self::NAN)
226    }
227    /// Right-handed view: the camera looks along its negative Z axis.
228    pub fn look_at_rh(eye: Vec3, target: Vec3, up: Vec3) -> Self {
229        Self::look_to_rh(eye, target - eye, up)
230    }
231    pub fn look_to_rh(eye: Vec3, direction: Vec3, up: Vec3) -> Self {
232        let z = (-direction).normalize();
233        let x = up.cross(z).normalize();
234        let y = z.cross(x);
235        Self::from_cols(
236            vec4(x.x, y.x, z.x, 0.0),
237            vec4(x.y, y.y, z.y, 0.0),
238            vec4(x.z, y.z, z.z, 0.0),
239            vec4(-x.dot(eye), -y.dot(eye), -z.dot(eye), 1.0),
240        )
241    }
242    pub fn look_at_lh(eye: Vec3, target: Vec3, up: Vec3) -> Self {
243        Self::look_to_lh(eye, target - eye, up)
244    }
245    pub fn look_to_lh(eye: Vec3, direction: Vec3, up: Vec3) -> Self {
246        Self::look_to_rh(eye, -direction, up)
247    }
248    /// Right-handed perspective with OpenGL depth [-1, 1].
249    pub fn perspective_rh_gl(fov_y: f32, aspect: f32, near: f32, far: f32) -> Self {
250        validate_perspective(fov_y, aspect, near, far);
251        let y = (0.5 * fov_y).tan().recip();
252        Self::from_cols(
253            vec4(y / aspect, 0.0, 0.0, 0.0),
254            vec4(0.0, y, 0.0, 0.0),
255            vec4(0.0, 0.0, (far + near) / (near - far), -1.0),
256            vec4(0.0, 0.0, 2.0 * far * near / (near - far), 0.0),
257        )
258    }
259    /// Right-handed perspective with WebGPU/Vulkan/Metal depth [0, 1].
260    pub fn perspective_rh(fov_y: f32, aspect: f32, near: f32, far: f32) -> Self {
261        validate_perspective(fov_y, aspect, near, far);
262        let y = (0.5 * fov_y).tan().recip();
263        Self::from_cols(
264            vec4(y / aspect, 0.0, 0.0, 0.0),
265            vec4(0.0, y, 0.0, 0.0),
266            vec4(0.0, 0.0, far / (near - far), -1.0),
267            vec4(0.0, 0.0, far * near / (near - far), 0.0),
268        )
269    }
270    pub fn perspective_lh(fov_y: f32, aspect: f32, near: f32, far: f32) -> Self {
271        let mut result = Self::perspective_rh(fov_y, aspect, near, far);
272        result.z_axis = -result.z_axis;
273        result
274    }
275    pub fn perspective_infinite_rh(fov_y: f32, aspect: f32, near: f32) -> Self {
276        validate_perspective(fov_y, aspect, near, near * 2.0);
277        let y = (0.5 * fov_y).tan().recip();
278        Self::from_cols(
279            vec4(y / aspect, 0.0, 0.0, 0.0),
280            vec4(0.0, y, 0.0, 0.0),
281            vec4(0.0, 0.0, -1.0, -1.0),
282            vec4(0.0, 0.0, -near, 0.0),
283        )
284    }
285    pub fn perspective_infinite_reverse_rh(fov_y: f32, aspect: f32, near: f32) -> Self {
286        validate_perspective(fov_y, aspect, near, near * 2.0);
287        let y = (0.5 * fov_y).tan().recip();
288        Self::from_cols(
289            vec4(y / aspect, 0.0, 0.0, 0.0),
290            vec4(0.0, y, 0.0, 0.0),
291            vec4(0.0, 0.0, 0.0, -1.0),
292            vec4(0.0, 0.0, near, 0.0),
293        )
294    }
295    pub fn orthographic_rh_gl(
296        left: f32,
297        right: f32,
298        bottom: f32,
299        top: f32,
300        near: f32,
301        far: f32,
302    ) -> Self {
303        validate_orthographic(left, right, bottom, top, near, far);
304        Self::from_cols(
305            vec4(2.0 / (right - left), 0.0, 0.0, 0.0),
306            vec4(0.0, 2.0 / (top - bottom), 0.0, 0.0),
307            vec4(0.0, 0.0, -2.0 / (far - near), 0.0),
308            vec4(
309                -(right + left) / (right - left),
310                -(top + bottom) / (top - bottom),
311                -(far + near) / (far - near),
312                1.0,
313            ),
314        )
315    }
316    pub fn orthographic_rh(
317        left: f32,
318        right: f32,
319        bottom: f32,
320        top: f32,
321        near: f32,
322        far: f32,
323    ) -> Self {
324        validate_orthographic(left, right, bottom, top, near, far);
325        Self::from_cols(
326            vec4(2.0 / (right - left), 0.0, 0.0, 0.0),
327            vec4(0.0, 2.0 / (top - bottom), 0.0, 0.0),
328            vec4(0.0, 0.0, -1.0 / (far - near), 0.0),
329            vec4(
330                -(right + left) / (right - left),
331                -(top + bottom) / (top - bottom),
332                -near / (far - near),
333                1.0,
334            ),
335        )
336    }
337    pub fn orthographic_lh(
338        left: f32,
339        right: f32,
340        bottom: f32,
341        top: f32,
342        near: f32,
343        far: f32,
344    ) -> Self {
345        let mut result = Self::orthographic_rh(left, right, bottom, top, near, far);
346        result.z_axis = -result.z_axis;
347        result
348    }
349    /// Extracts TRS from an affine matrix with orthogonal, nonzero scale axes.
350    /// Shear and perspective are rejected rather than approximated.
351    pub fn try_to_scale_rotation_translation(self) -> Option<(Vec3, Quat, Vec3)> {
352        if !self.is_finite() || !self.row(3).abs_diff_eq(Vec4::W, 1.0e-5) {
353            return None;
354        }
355        let mut scale = vec3(
356            self.x_axis.truncate().length(),
357            self.y_axis.truncate().length(),
358            self.z_axis.truncate().length(),
359        );
360        if scale.min_element() <= 0.0 {
361            return None;
362        }
363        if self.determinant() < 0.0 {
364            scale.x = -scale.x;
365        }
366        let x = self.x_axis.truncate() / scale.x;
367        let y = self.y_axis.truncate() / scale.y;
368        let z = self.z_axis.truncate() / scale.z;
369        if x.dot(y).abs() > 1.0e-4 || y.dot(z).abs() > 1.0e-4 || z.dot(x).abs() > 1.0e-4 {
370            return None;
371        }
372        let rotation = rotation_from_basis(x, y, z);
373        Some((scale, rotation, self.w_axis.truncate()))
374    }
375    pub fn to_scale_rotation_translation(self) -> (Vec3, Quat, Vec3) {
376        self.try_to_scale_rotation_translation()
377            .unwrap_or((Vec3::NAN, Quat::NAN, Vec3::NAN))
378    }
379}
380fn rotation_from_basis(x: Vec3, y: Vec3, z: Vec3) -> Quat {
381    let trace = x.x + y.y + z.z;
382    let q = if trace > 0.0 {
383        let s = (trace + 1.0).sqrt() * 2.0;
384        Quat::from_xyzw((y.z - z.y) / s, (z.x - x.z) / s, (x.y - y.x) / s, s * 0.25)
385    } else if x.x > y.y && x.x > z.z {
386        let s = (1.0 + x.x - y.y - z.z).sqrt() * 2.0;
387        Quat::from_xyzw(s * 0.25, (y.x + x.y) / s, (z.x + x.z) / s, (y.z - z.y) / s)
388    } else if y.y > z.z {
389        let s = (1.0 + y.y - x.x - z.z).sqrt() * 2.0;
390        Quat::from_xyzw((y.x + x.y) / s, s * 0.25, (z.y + y.z) / s, (z.x - x.z) / s)
391    } else {
392        let s = (1.0 + z.z - x.x - y.y).sqrt() * 2.0;
393        Quat::from_xyzw((z.x + x.z) / s, (z.y + y.z) / s, s * 0.25, (x.y - y.x) / s)
394    };
395    q.normalize()
396}
397fn validate_perspective(fov: f32, aspect: f32, near: f32, far: f32) {
398    assert!(
399        fov.is_finite()
400            && fov > 0.0
401            && fov < std::f32::consts::PI
402            && aspect.is_finite()
403            && aspect > 0.0
404            && near.is_finite()
405            && far.is_finite()
406            && near > 0.0
407            && far > near,
408        "invalid perspective frustum"
409    );
410}
411fn validate_orthographic(left: f32, right: f32, bottom: f32, top: f32, near: f32, far: f32) {
412    assert!(
413        [left, right, bottom, top, near, far]
414            .into_iter()
415            .all(f32::is_finite)
416            && left != right
417            && bottom != top
418            && near != far,
419        "invalid orthographic frustum"
420    );
421}
422impl Index<usize> for Mat4 {
423    type Output = Vec4;
424    fn index(&self, index: usize) -> &Vec4 {
425        match index {
426            0 => &self.x_axis,
427            1 => &self.y_axis,
428            2 => &self.z_axis,
429            3 => &self.w_axis,
430            _ => panic!("matrix column {index} is out of range"),
431        }
432    }
433}
434impl IndexMut<usize> for Mat4 {
435    fn index_mut(&mut self, index: usize) -> &mut Vec4 {
436        match index {
437            0 => &mut self.x_axis,
438            1 => &mut self.y_axis,
439            2 => &mut self.z_axis,
440            3 => &mut self.w_axis,
441            _ => panic!("matrix column {index} is out of range"),
442        }
443    }
444}
445impl Mul<Vec4> for Mat4 {
446    type Output = Vec4;
447    fn mul(self, rhs: Vec4) -> Vec4 {
448        self.mul_vec4(rhs)
449    }
450}
451impl Mul for Mat4 {
452    type Output = Self;
453    fn mul(self, rhs: Self) -> Self {
454        Self::from_cols(
455            self * rhs.x_axis,
456            self * rhs.y_axis,
457            self * rhs.z_axis,
458            self * rhs.w_axis,
459        )
460    }
461}
462impl Mul<f32> for Mat4 {
463    type Output = Self;
464    fn mul(self, rhs: f32) -> Self {
465        Self::from_cols(
466            self.x_axis * rhs,
467            self.y_axis * rhs,
468            self.z_axis * rhs,
469            self.w_axis * rhs,
470        )
471    }
472}
473impl Mul<Mat4> for f32 {
474    type Output = Mat4;
475    fn mul(self, rhs: Mat4) -> Mat4 {
476        rhs * self
477    }
478}
479impl MulAssign for Mat4 {
480    fn mul_assign(&mut self, rhs: Self) {
481        *self = *self * rhs;
482    }
483}
484impl Add for Mat4 {
485    type Output = Self;
486    fn add(self, rhs: Self) -> Self {
487        Self::from_cols(
488            self.x_axis + rhs.x_axis,
489            self.y_axis + rhs.y_axis,
490            self.z_axis + rhs.z_axis,
491            self.w_axis + rhs.w_axis,
492        )
493    }
494}
495impl Sub for Mat4 {
496    type Output = Self;
497    fn sub(self, rhs: Self) -> Self {
498        Self::from_cols(
499            self.x_axis - rhs.x_axis,
500            self.y_axis - rhs.y_axis,
501            self.z_axis - rhs.z_axis,
502            self.w_axis - rhs.w_axis,
503        )
504    }
505}
ferravex_math/matrix.rs

ferravex_math/
matrix.rs
