nova_math/matrix/
mod.rs

1/*
2 *
3 * This file is a part of NovaEngine
4 * https://gitlab.com/MindSpunk/NovaEngine
5 *
6 *
7 * MIT License
8 *
9 * Copyright (c) 2018 Nathan Voglsam
10 *
11 * Permission is hereby granted, free of charge, to any person obtaining a copy
12 * of this software and associated documentation files (the "Software"), to deal
13 * in the Software without restriction, including without limitation the rights
14 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
15 * copies of the Software, and to permit persons to whom the Software is
16 * furnished to do so, subject to the following conditions:
17 *
18 * The above copyright notice and this permission notice shall be included in all
19 * copies or substantial portions of the Software.
20 *
21 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
22 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
23 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
24 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
25 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
26 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
27 * SOFTWARE.
28 */
29
30#[cfg(any(target_arch = "x86", target_arch = "x86_64"))]
31mod simd_x86;
32
33use core::fmt::Display;
34use core::fmt::Error;
35use core::fmt::Formatter;
36use core::ops::Add;
37use core::ops::AddAssign;
38use core::ops::Div;
39use core::ops::DivAssign;
40use core::ops::Index;
41use core::ops::IndexMut;
42use core::ops::Mul;
43use core::ops::MulAssign;
44use core::ops::Sub;
45use core::ops::SubAssign;
46use core::ptr;
47
48use crate::traits::Column;
49use crate::traits::ColumnRef;
50use crate::traits::ColumnRefMut;
51use crate::traits::DotProduct;
52use crate::traits::IntoSTD140;
53use crate::traits::Inverse;
54use crate::traits::InverseAssign;
55use crate::traits::Normalize;
56use crate::traits::Real;
57use crate::traits::Row;
58use crate::traits::Transpose;
59use crate::traits::TransposeAssign;
60use crate::vector::TVec3;
61use crate::vector::TVec4;
62
63///
64/// A column major 4x4 floating point matrix
65///
66#[repr(C)]
67#[repr(align(16))]
68#[derive(Clone, Debug)]
69pub struct TMat4x4<T: Real> {
70    data: [T; 16],
71}
72
73impl<T: Real> TMat4x4<T> {
74    ///
75    /// Construct a new 4x4 matrix from a flat array of 16 elements
76    ///
77    #[inline]
78    pub fn new(input: [T; 16]) -> TMat4x4<T> {
79        TMat4x4 { data: input }
80    }
81
82    ///
83    /// Construct a new 4x4 matrix of entirely zeroes
84    ///
85    #[inline]
86    pub fn zero() -> TMat4x4<T> {
87        TMat4x4 {
88            data: [T::zero(); 16],
89        }
90    }
91
92    ///
93    /// Construct a new 4x4 identity matrix
94    ///
95    pub fn identity() -> TMat4x4<T> {
96        let data: [T; 16] = [
97            T::one(),
98            T::zero(),
99            T::zero(),
100            T::zero(),
101            T::zero(),
102            T::one(),
103            T::zero(),
104            T::zero(),
105            T::zero(),
106            T::zero(),
107            T::one(),
108            T::zero(),
109            T::zero(),
110            T::zero(),
111            T::zero(),
112            T::one(),
113        ];
114        TMat4x4 { data }
115    }
116
117    ///
118    /// Construct a new Vulkan compatible perspective transform matrix
119    ///
120    pub fn perspective(aspect: T, fov: T, near: T, far: T) -> TMat4x4<T> {
121        let mut mat = TMat4x4::<T>::zero();
122
123        let one = T::one();
124        let two = one + one;
125
126        let tan_half_fov = T::one() / (fov / two).tan();
127
128        mat[(0, 0)] = tan_half_fov / aspect;
129        mat[(1, 1)] = -tan_half_fov;
130        mat[(2, 2)] = far / (far - near);
131        mat[(2, 3)] = -(far * near) / (far - near);
132        mat[(3, 2)] = one;
133
134        mat
135    }
136
137    ///
138    /// Construct a new translation matrix with the given translation
139    ///
140    pub fn translation(translation: TVec3<T>) -> TMat4x4<T> {
141        let mut mat = TMat4x4::identity();
142        mat[(0, 3)] = translation[0];
143        mat[(1, 3)] = translation[1];
144        mat[(2, 3)] = translation[2];
145
146        mat
147    }
148
149    ///
150    /// Apply a translation to this matrix
151    ///
152    pub fn translate(&mut self, translation: TVec3<T>) {
153        let col_0 = self.get_column(0);
154        let col_1 = self.get_column(1);
155        let col_2 = self.get_column(2);
156        let col_3 = self.get_column(3);
157
158        let col_0 = col_0 * translation[0];
159        let col_1 = col_1 * translation[1];
160        let col_2 = col_2 * translation[2];
161
162        let col_ref = self.get_column_ref_mut(3);
163        *col_ref = col_0 + col_1 + col_2 + col_3;
164    }
165
166    ///
167    /// Get an angle axis rotation matrix
168    ///
169    pub fn rotation(angle: T, axis: TVec3<T>) -> TMat4x4<T> {
170        let mut out = TMat4x4::<T>::identity();
171        out.rotate(angle, axis);
172        out
173    }
174
175    ///
176    /// Apply an angle axis rotation to this matrix
177    ///
178    pub fn rotate(&mut self, angle: T, in_axis: TVec3<T>) {
179        let sin_angle = angle.sin();
180        let cos_angle = angle.cos();
181
182        let axis = in_axis.normalize();
183        let temp: TVec3<T> = axis * (T::one() - cos_angle);
184
185        let mut rot = TMat4x4::<T>::identity();
186
187        rot[(0, 0)] = cos_angle + temp[0] * axis[0];
188        rot[(0, 1)] = temp[0] * axis[1] + sin_angle * axis[2];
189        rot[(0, 2)] = temp[0] * axis[2] - sin_angle * axis[1];
190
191        rot[(1, 0)] = temp[1] * axis[0] - sin_angle * axis[2];
192        rot[(1, 1)] = cos_angle + temp[1] * axis[1];
193        rot[(1, 2)] = temp[1] * axis[2] + sin_angle * axis[0];
194
195        rot[(2, 0)] = temp[2] * axis[0] + sin_angle * axis[1];
196        rot[(2, 1)] = temp[2] * axis[1] - sin_angle * axis[0];
197        rot[(2, 2)] = cos_angle + temp[2] * axis[2];
198
199        let col_0 = self.get_column(0);
200        let col_1 = self.get_column(1);
201        let col_2 = self.get_column(2);
202
203        let col_ref = self.get_column_ref_mut(0);
204        *col_ref = col_0 * rot[(0, 0)] + col_1 * rot[(0, 1)] + col_2 * rot[(0, 2)];
205
206        let col_ref = self.get_column_ref_mut(1);
207        *col_ref = col_0 * rot[(1, 0)] + col_1 * rot[(1, 1)] + col_2 * rot[(1, 2)];
208
209        let col_ref = self.get_column_ref_mut(2);
210        *col_ref = col_0 * rot[(2, 0)] + col_1 * rot[(2, 1)] + col_2 * rot[(2, 2)];
211    }
212
213    ///
214    /// Get a scaling matrix
215    ///
216    pub fn scaling(scale: TVec3<T>) -> TMat4x4<T> {
217        let mut result = Self::identity();
218
219        let col = result.get_column_ref_mut(0);
220        *col = *col * scale[0];
221
222        let col = result.get_column_ref_mut(1);
223        *col = *col * scale[1];
224
225        let col = result.get_column_ref_mut(2);
226        *col = *col * scale[2];
227
228        result
229    }
230
231    ///
232    /// Apply scaling to the matrix
233    ///
234    pub fn scale(&mut self, scale: TVec3<T>) {
235        let col = self.get_column_ref_mut(0);
236        *col = *col * scale[0];
237
238        let col = self.get_column_ref_mut(1);
239        *col = *col * scale[1];
240
241        let col = self.get_column_ref_mut(2);
242        *col = *col * scale[2];
243    }
244
245    ///
246    /// Construct a new matrix from the four columns passed into the function
247    ///
248    pub fn from_columns(col0: [T; 4], col1: [T; 4], col2: [T; 4], col3: [T; 4]) -> Self {
249        let mut data = [T::zero(); 16];
250        data[0..4].copy_from_slice(&col0);
251        data[4..8].copy_from_slice(&col1);
252        data[8..12].copy_from_slice(&col2);
253        data[12..16].copy_from_slice(&col3);
254        Self::new(data)
255    }
256
257    #[cfg(any(target_arch = "x86", target_arch = "x86_64"))]
258    #[inline]
259    fn internal_apply(&self, vec: TVec4<T>) -> TVec4<T> {
260        if T::is_f32() {
261            simd_x86::simd_f32_apply(self, vec)
262        } else {
263            self.internal_apply_scalar(vec)
264        }
265    }
266
267    #[cfg(not(any(target_arch = "x86", target_arch = "x86_64")))]
268    #[inline]
269    fn internal_apply(&self, vec: TVec4<T>) -> TVec4<T> {
270        self.internal_apply_scalar(vec)
271    }
272
273    #[inline]
274    fn internal_apply_scalar(&self, vec: TVec4<T>) -> TVec4<T> {
275        let row0 = self.get_row(0);
276        let row1 = self.get_row(1);
277        let row2 = self.get_row(2);
278        let row3 = self.get_row(3);
279
280        let mut out = TVec4::<T>::zero();
281
282        out[0] = row0.dot(&vec);
283        out[1] = row1.dot(&vec);
284        out[2] = row2.dot(&vec);
285        out[3] = row3.dot(&vec);
286
287        out
288    }
289
290    ///
291    /// Apply this matrix to a vector, creating a new one in the process
292    ///
293    pub fn apply(&self, vec: TVec4<T>) -> TVec4<T> {
294        self.internal_apply(vec)
295    }
296
297    ///
298    /// Apply a matrix to the given vector
299    ///
300    pub fn apply_to(&self, rhs: &mut TVec4<T>) {
301        *rhs = self.apply(*rhs)
302    }
303
304    ///
305    /// Multiply the given matrix onto this matrix
306    ///
307    pub fn multiply(&mut self, rhs: &TMat4x4<T>) {
308        internal_mat4_mul_assign(self, rhs);
309    }
310
311    ///
312    /// Returns column refs as TVec4<T>s
313    ///
314    #[inline]
315    fn as_columns_ref(&self) -> (&TVec4<T>, &TVec4<T>, &TVec4<T>, &TVec4<T>) {
316        unsafe {
317            (
318                &*(&self.data[0] as *const T as *const TVec4<T>),
319                &*(&self.data[4] as *const T as *const TVec4<T>),
320                &*(&self.data[8] as *const T as *const TVec4<T>),
321                &*(&self.data[12] as *const T as *const TVec4<T>),
322            )
323        }
324    }
325
326    ///
327    /// Returns column mut refs as TVec4<T>s
328    ///
329    #[inline]
330    fn as_columns_mut(&mut self) -> (&mut TVec4<T>, &mut TVec4<T>, &mut TVec4<T>, &mut TVec4<T>) {
331        unsafe {
332            (
333                &mut *(&mut self.data[0] as *mut T as *mut TVec4<T>),
334                &mut *(&mut self.data[4] as *mut T as *mut TVec4<T>),
335                &mut *(&mut self.data[8] as *mut T as *mut TVec4<T>),
336                &mut *(&mut self.data[12] as *mut T as *mut TVec4<T>),
337            )
338        }
339    }
340}
341
342impl<T: Real> Column for TMat4x4<T> {
343    type Output = TVec4<T>;
344
345    #[inline]
346    fn get_column(&self, col: usize) -> Self::Output {
347        TVec4::new(
348            self.data[(col * 4)],
349            self.data[(col * 4) + 1],
350            self.data[(col * 4) + 2],
351            self.data[(col * 4) + 3],
352        )
353    }
354}
355
356impl<T: Real> ColumnRef for TMat4x4<T> {
357    #[inline]
358    fn get_column_ref(&self, col: usize) -> &Self::Output {
359        unsafe { &*(&self.data[(col * 4)] as *const T as *const TVec4<T>) }
360    }
361}
362
363impl<T: Real> ColumnRefMut for TMat4x4<T> {
364    #[inline]
365    fn get_column_ref_mut(&mut self, col: usize) -> &mut Self::Output {
366        unsafe { &mut *(&mut self.data[(col * 4)] as *mut T as *mut TVec4<T>) }
367    }
368}
369
370impl<T: Real> Row for TMat4x4<T> {
371    type Output = TVec4<T>;
372
373    #[inline]
374    fn get_row(&self, row: usize) -> Self::Output {
375        TVec4::new(
376            self.data[row],
377            self.data[row + 4],
378            self.data[row + 8],
379            self.data[row + 12],
380        )
381    }
382}
383
384impl<T: Real> Display for TMat4x4<T> {
385    fn fmt(&self, f: &mut Formatter) -> Result<(), Error> {
386        let precision = f.precision().unwrap_or(f.width().unwrap_or(4));
387        for row in 0..4 {
388            let row = self.get_row(row);
389            let row_x = row[0];
390            let row_y = row[1];
391            let row_z = row[2];
392            let row_w = row[3];
393            writeln!(
394                f,
395                "    │ {:^width$} {:^width$} {:^width$} {:^width$} │",
396                row_x,
397                row_y,
398                row_z,
399                row_w,
400                width = precision,
401            )?;
402        }
403
404        Ok(())
405    }
406}
407
408impl<T: Real> From<&[T; 16]> for TMat4x4<T> {
409    #[inline]
410    fn from(other: &[T; 16]) -> Self {
411        TMat4x4::new(*other)
412    }
413}
414
415impl<T: Real> Into<[T; 16]> for TMat4x4<T> {
416    #[inline]
417    fn into(self) -> [T; 16] {
418        self.data
419    }
420}
421
422impl<T: Real> Index<usize> for TMat4x4<T> {
423    type Output = T;
424
425    ///
426    /// Index the matrix linearly
427    ///
428    #[inline]
429    fn index(&self, index: usize) -> &Self::Output {
430        &self.data[index]
431    }
432}
433
434impl<T: Real> Index<(usize, usize)> for TMat4x4<T> {
435    type Output = T;
436
437    ///
438    /// Index the matrix. (row, column)
439    ///
440    #[inline]
441    fn index(&self, index: (usize, usize)) -> &Self::Output {
442        let row = index.0;
443        let col = index.1;
444        &self.data[(col * 4) + row]
445    }
446}
447
448impl<T: Real> IndexMut<usize> for TMat4x4<T> {
449    ///
450    /// Index the matrix linearly
451    ///
452    #[inline]
453    fn index_mut(&mut self, index: usize) -> &mut Self::Output {
454        &mut self.data[index]
455    }
456}
457
458impl<T: Real> IndexMut<(usize, usize)> for TMat4x4<T> {
459    ///
460    /// Index the matrix. (row, column)
461    ///
462    #[inline]
463    fn index_mut(&mut self, index: (usize, usize)) -> &mut Self::Output {
464        let row = index.0;
465        let col = index.1;
466        &mut self.data[(col * 4) + row]
467    }
468}
469
470impl<T: Real> Add<TMat4x4<T>> for TMat4x4<T> {
471    type Output = Self;
472
473    fn add(mut self, rhs: TMat4x4<T>) -> Self::Output {
474        Self::add_assign(&mut self, rhs);
475        self
476    }
477}
478
479impl<T: Real> AddAssign<TMat4x4<T>> for TMat4x4<T> {
480    fn add_assign(&mut self, rhs: TMat4x4<T>) {
481        let (sv1, sv2, sv3, sv4) = self.as_columns_mut();
482        let (rv1, rv2, rv3, rv4) = rhs.as_columns_ref();
483
484        TVec4::add_assign(sv1, *rv1);
485        TVec4::add_assign(sv2, *rv2);
486        TVec4::add_assign(sv3, *rv3);
487        TVec4::add_assign(sv4, *rv4);
488    }
489}
490
491impl<T: Real> Sub<TMat4x4<T>> for TMat4x4<T> {
492    type Output = Self;
493
494    fn sub(mut self, rhs: TMat4x4<T>) -> Self::Output {
495        Self::sub_assign(&mut self, rhs);
496        self
497    }
498}
499
500impl<T: Real> SubAssign<TMat4x4<T>> for TMat4x4<T> {
501    fn sub_assign(&mut self, rhs: TMat4x4<T>) {
502        let (sv1, sv2, sv3, sv4) = self.as_columns_mut();
503        let (rv1, rv2, rv3, rv4) = rhs.as_columns_ref();
504
505        TVec4::sub_assign(sv1, *rv1);
506        TVec4::sub_assign(sv2, *rv2);
507        TVec4::sub_assign(sv3, *rv3);
508        TVec4::sub_assign(sv4, *rv4);
509    }
510}
511
512impl<T: Real> Div<TMat4x4<T>> for TMat4x4<T> {
513    type Output = Self;
514
515    fn div(mut self, rhs: TMat4x4<T>) -> Self::Output {
516        Self::div_assign(&mut self, rhs);
517        self
518    }
519}
520
521impl<T: Real> DivAssign<TMat4x4<T>> for TMat4x4<T> {
522    fn div_assign(&mut self, rhs: TMat4x4<T>) {
523        let (sv1, sv2, sv3, sv4) = self.as_columns_mut();
524        let (rv1, rv2, rv3, rv4) = rhs.as_columns_ref();
525
526        TVec4::div_assign(sv1, *rv1);
527        TVec4::div_assign(sv2, *rv2);
528        TVec4::div_assign(sv3, *rv3);
529        TVec4::div_assign(sv4, *rv4);
530    }
531}
532
533impl<T: Real> Mul<TMat4x4<T>> for TMat4x4<T> {
534    type Output = Self;
535
536    fn mul(mut self, rhs: TMat4x4<T>) -> Self::Output {
537        Self::mul_assign(&mut self, rhs);
538        self
539    }
540}
541
542#[cfg(any(target_arch = "x86", target_arch = "x86_64"))]
543#[inline]
544fn internal_mat4_mul_assign<T: Real>(lhs: &mut TMat4x4<T>, rhs: &TMat4x4<T>) {
545    if T::is_f32() {
546        simd_x86::simd_f32_mul_assign(lhs, rhs);
547    } else {
548        internal_mat4_mul_assign_scalar(lhs, rhs);
549    }
550}
551
552#[cfg(not(any(target_arch = "x86", target_arch = "x86_64")))]
553#[inline]
554fn internal_mat4_mul_assign<T: Real>(lhs: &mut TMat4x4<T>, rhs: TMat4x4<T>) {
555    internal_mat4_mul_assign_scalar(lhs, rhs);
556}
557
558#[inline]
559fn internal_mat4_mul_assign_scalar<T: Real>(lhs: &mut TMat4x4<T>, rhs: &TMat4x4<T>) {
560    let row0 = lhs.get_row(0);
561    let row1 = lhs.get_row(1);
562    let row2 = lhs.get_row(2);
563    let row3 = lhs.get_row(3);
564    let col0 = rhs.get_column(0);
565    let col1 = rhs.get_column(1);
566    let col2 = rhs.get_column(2);
567    let col3 = rhs.get_column(3);
568
569    {
570        lhs[(0, 0)] = col0.dot(&row0);
571        lhs[(0, 1)] = col1.dot(&row0);
572        lhs[(0, 2)] = col2.dot(&row0);
573        lhs[(0, 3)] = col3.dot(&row0);
574    }
575    {
576        lhs[(1, 0)] = col0.dot(&row1);
577        lhs[(1, 1)] = col1.dot(&row1);
578        lhs[(1, 2)] = col2.dot(&row1);
579        lhs[(1, 3)] = col3.dot(&row1);
580    }
581    {
582        lhs[(2, 0)] = col0.dot(&row2);
583        lhs[(2, 1)] = col1.dot(&row2);
584        lhs[(2, 2)] = col2.dot(&row2);
585        lhs[(2, 3)] = col3.dot(&row2);
586    }
587    {
588        lhs[(3, 0)] = col0.dot(&row3);
589        lhs[(3, 1)] = col1.dot(&row3);
590        lhs[(3, 2)] = col2.dot(&row3);
591        lhs[(3, 3)] = col3.dot(&row3);
592    }
593}
594
595impl<T: Real> MulAssign<TMat4x4<T>> for TMat4x4<T> {
596    fn mul_assign(&mut self, rhs: TMat4x4<T>) {
597        internal_mat4_mul_assign(self, &rhs)
598    }
599}
600
601impl<T: Real> Mul<TVec4<T>> for TMat4x4<T> {
602    type Output = TVec4<T>;
603
604    fn mul(self, rhs: TVec4<T>) -> Self::Output {
605        self.apply(rhs)
606    }
607}
608
609impl<T: Real> MulAssign<T> for TMat4x4<T> {
610    fn mul_assign(&mut self, rhs: T) {
611        *self.get_column_ref_mut(0) *= rhs;
612        *self.get_column_ref_mut(1) *= rhs;
613        *self.get_column_ref_mut(2) *= rhs;
614        *self.get_column_ref_mut(3) *= rhs;
615    }
616}
617
618impl<T: Real> Mul<T> for TMat4x4<T> {
619    type Output = TMat4x4<T>;
620
621    fn mul(mut self, rhs: T) -> Self::Output {
622        *self.get_column_ref_mut(0) *= rhs;
623        *self.get_column_ref_mut(1) *= rhs;
624        *self.get_column_ref_mut(2) *= rhs;
625        *self.get_column_ref_mut(3) *= rhs;
626        self
627    }
628}
629
630impl<T: Real> PartialEq<TMat4x4<T>> for TMat4x4<T> {
631    fn eq(&self, other: &TMat4x4<T>) -> bool {
632        self.get_column_ref(0) == other.get_column_ref(0)
633            && self.get_column_ref(1) == other.get_column_ref(1)
634            && self.get_column_ref(2) == other.get_column_ref(2)
635            && self.get_column_ref(3) == other.get_column_ref(3)
636    }
637
638    fn ne(&self, other: &TMat4x4<T>) -> bool {
639        self.get_column_ref(0) != other.get_column_ref(0)
640            || self.get_column_ref(1) != other.get_column_ref(1)
641            || self.get_column_ref(2) != other.get_column_ref(2)
642            || self.get_column_ref(3) != other.get_column_ref(3)
643    }
644}
645
646impl<T: Real> Transpose for TMat4x4<T> {
647    #[inline]
648    fn transpose(mut self) -> Self {
649        self.transpose_assign();
650        self
651    }
652}
653
654impl<T: Real> TransposeAssign for TMat4x4<T> {
655    #[inline]
656    fn transpose_assign(&mut self) {
657        unsafe {
658            ptr::swap(&mut self[(1, 0)], &mut self[(0, 1)]);
659            ptr::swap(&mut self[(2, 1)], &mut self[(1, 2)]);
660            ptr::swap(&mut self[(3, 2)], &mut self[(2, 3)]);
661
662            ptr::swap(&mut self[(2, 0)], &mut self[(0, 2)]);
663            ptr::swap(&mut self[(3, 0)], &mut self[(0, 3)]);
664            ptr::swap(&mut self[(3, 1)], &mut self[(1, 3)]);
665        }
666    }
667}
668
669impl<T: Real> Inverse for TMat4x4<T> {
670    fn inverse(self) -> Self {
671        let mut m = self;
672
673        let m_col0 = m.get_column(0);
674
675        let coef00: T = m[(2, 2)] * m[(3, 3)] - m[(2, 3)] * m[(3, 2)];
676        let coef02: T = m[(2, 1)] * m[(3, 3)] - m[(2, 3)] * m[(3, 1)];
677        let coef03: T = m[(2, 1)] * m[(3, 2)] - m[(2, 2)] * m[(3, 1)];
678
679        let coef04: T = m[(1, 2)] * m[(3, 3)] - m[(1, 3)] * m[(3, 2)];
680        let coef06: T = m[(1, 1)] * m[(3, 3)] - m[(1, 3)] * m[(3, 1)];
681        let coef07: T = m[(1, 1)] * m[(3, 2)] - m[(1, 2)] * m[(3, 1)];
682
683        let coef08: T = m[(1, 2)] * m[(2, 3)] - m[(1, 3)] * m[(2, 2)];
684        let coef10: T = m[(1, 1)] * m[(2, 3)] - m[(1, 3)] * m[(2, 1)];
685        let coef11: T = m[(1, 1)] * m[(2, 2)] - m[(1, 2)] * m[(2, 1)];
686
687        let coef12: T = m[(0, 2)] * m[(3, 3)] - m[(0, 3)] * m[(3, 2)];
688        let coef14: T = m[(0, 1)] * m[(3, 3)] - m[(0, 3)] * m[(3, 1)];
689        let coef15: T = m[(0, 1)] * m[(3, 2)] - m[(0, 2)] * m[(3, 1)];
690
691        let coef16: T = m[(0, 2)] * m[(2, 3)] - m[(0, 3)] * m[(2, 2)];
692        let coef18: T = m[(0, 1)] * m[(2, 3)] - m[(0, 3)] * m[(2, 1)];
693        let coef19: T = m[(0, 1)] * m[(2, 2)] - m[(0, 2)] * m[(2, 1)];
694
695        let coef20: T = m[(0, 2)] * m[(1, 3)] - m[(0, 3)] * m[(1, 2)];
696        let coef22: T = m[(0, 1)] * m[(1, 3)] - m[(0, 3)] * m[(1, 1)];
697        let coef23: T = m[(0, 1)] * m[(1, 2)] - m[(0, 2)] * m[(1, 1)];
698
699        let fac0 = TVec4::<T>::new(coef00, coef00, coef02, coef03);
700        let fac1 = TVec4::<T>::new(coef04, coef04, coef06, coef07);
701        let fac2 = TVec4::<T>::new(coef08, coef08, coef10, coef11);
702        let fac3 = TVec4::<T>::new(coef12, coef12, coef14, coef15);
703        let fac4 = TVec4::<T>::new(coef16, coef16, coef18, coef19);
704        let fac5 = TVec4::<T>::new(coef20, coef20, coef22, coef23);
705
706        let vec0 = TVec4::<T>::new(m[(0, 1)], m[(0, 0)], m[(0, 0)], m[(0, 0)]);
707        let vec1 = TVec4::<T>::new(m[(1, 1)], m[(1, 0)], m[(1, 0)], m[(1, 0)]);
708        let vec2 = TVec4::<T>::new(m[(2, 1)], m[(2, 0)], m[(2, 0)], m[(2, 0)]);
709        let vec3 = TVec4::<T>::new(m[(3, 1)], m[(3, 0)], m[(3, 0)], m[(3, 0)]);
710
711        let inv0 = vec1 * fac0 - vec2 * fac1 + vec3 * fac2;
712        let inv1 = vec0 * fac0 - vec2 * fac3 + vec3 * fac4;
713        let inv2 = vec0 * fac1 - vec1 * fac3 + vec3 * fac5;
714        let inv3 = vec0 * fac2 - vec1 * fac4 + vec2 * fac5;
715
716        let sign_a = TVec4::<T>::new(T::one(), -T::one(), T::one(), -T::one());
717        let sign_b = TVec4::<T>::new(-T::one(), T::one(), -T::one(), T::one());
718        *m.get_column_ref_mut(0) = inv0 * sign_a;
719        *m.get_column_ref_mut(1) = inv1 * sign_b;
720        *m.get_column_ref_mut(2) = inv2 * sign_a;
721        *m.get_column_ref_mut(3) = inv3 * sign_b;
722        let inverse = m;
723
724        let row0 = TVec4::<T>::new(
725            inverse[(0, 0)],
726            inverse[(0, 1)],
727            inverse[(0, 2)],
728            inverse[(0, 3)],
729        );
730
731        let dot0 = m_col0 * row0;
732        let dot1: T = (dot0[0] + dot0[1]) + (dot0[2] + dot0[3]);
733
734        let one_over_determinant = T::one() / dot1;
735
736        inverse * one_over_determinant
737    }
738}
739
740impl<T: Real> InverseAssign for TMat4x4<T> {
741    fn inverse_assign(&mut self) {
742        let m = self;
743
744        let m_col0 = m.get_column(0);
745
746        let coef00: T = m[(2, 2)] * m[(3, 3)] - m[(2, 3)] * m[(3, 2)];
747        let coef02: T = m[(2, 1)] * m[(3, 3)] - m[(2, 3)] * m[(3, 1)];
748        let coef03: T = m[(2, 1)] * m[(3, 2)] - m[(2, 2)] * m[(3, 1)];
749
750        let coef04: T = m[(1, 2)] * m[(3, 3)] - m[(1, 3)] * m[(3, 2)];
751        let coef06: T = m[(1, 1)] * m[(3, 3)] - m[(1, 3)] * m[(3, 1)];
752        let coef07: T = m[(1, 1)] * m[(3, 2)] - m[(1, 2)] * m[(3, 1)];
753
754        let coef08: T = m[(1, 2)] * m[(2, 3)] - m[(1, 3)] * m[(2, 2)];
755        let coef10: T = m[(1, 1)] * m[(2, 3)] - m[(1, 3)] * m[(2, 1)];
756        let coef11: T = m[(1, 1)] * m[(2, 2)] - m[(1, 2)] * m[(2, 1)];
757
758        let coef12: T = m[(0, 2)] * m[(3, 3)] - m[(0, 3)] * m[(3, 2)];
759        let coef14: T = m[(0, 1)] * m[(3, 3)] - m[(0, 3)] * m[(3, 1)];
760        let coef15: T = m[(0, 1)] * m[(3, 2)] - m[(0, 2)] * m[(3, 1)];
761
762        let coef16: T = m[(0, 2)] * m[(2, 3)] - m[(0, 3)] * m[(2, 2)];
763        let coef18: T = m[(0, 1)] * m[(2, 3)] - m[(0, 3)] * m[(2, 1)];
764        let coef19: T = m[(0, 1)] * m[(2, 2)] - m[(0, 2)] * m[(2, 1)];
765
766        let coef20: T = m[(0, 2)] * m[(1, 3)] - m[(0, 3)] * m[(1, 2)];
767        let coef22: T = m[(0, 1)] * m[(1, 3)] - m[(0, 3)] * m[(1, 1)];
768        let coef23: T = m[(0, 1)] * m[(1, 2)] - m[(0, 2)] * m[(1, 1)];
769
770        let fac0 = TVec4::<T>::new(coef00, coef00, coef02, coef03);
771        let fac1 = TVec4::<T>::new(coef04, coef04, coef06, coef07);
772        let fac2 = TVec4::<T>::new(coef08, coef08, coef10, coef11);
773        let fac3 = TVec4::<T>::new(coef12, coef12, coef14, coef15);
774        let fac4 = TVec4::<T>::new(coef16, coef16, coef18, coef19);
775        let fac5 = TVec4::<T>::new(coef20, coef20, coef22, coef23);
776
777        let vec0 = TVec4::<T>::new(m[(0, 1)], m[(0, 0)], m[(0, 0)], m[(0, 0)]);
778        let vec1 = TVec4::<T>::new(m[(1, 1)], m[(1, 0)], m[(1, 0)], m[(1, 0)]);
779        let vec2 = TVec4::<T>::new(m[(2, 1)], m[(2, 0)], m[(2, 0)], m[(2, 0)]);
780        let vec3 = TVec4::<T>::new(m[(3, 1)], m[(3, 0)], m[(3, 0)], m[(3, 0)]);
781
782        let inv0 = vec1 * fac0 - vec2 * fac1 + vec3 * fac2;
783        let inv1 = vec0 * fac0 - vec2 * fac3 + vec3 * fac4;
784        let inv2 = vec0 * fac1 - vec1 * fac3 + vec3 * fac5;
785        let inv3 = vec0 * fac2 - vec1 * fac4 + vec2 * fac5;
786
787        let sign_a = TVec4::<T>::new(T::one(), -T::one(), T::one(), -T::one());
788        let sign_b = TVec4::<T>::new(-T::one(), T::one(), -T::one(), T::one());
789        *m.get_column_ref_mut(0) = inv0 * sign_a;
790        *m.get_column_ref_mut(1) = inv1 * sign_b;
791        *m.get_column_ref_mut(2) = inv2 * sign_a;
792        *m.get_column_ref_mut(3) = inv3 * sign_b;
793        let inverse = m;
794
795        let row0 = TVec4::<T>::new(
796            inverse[(0, 0)],
797            inverse[(0, 1)],
798            inverse[(0, 2)],
799            inverse[(0, 3)],
800        );
801
802        let dot0 = m_col0 * row0;
803        let dot1: T = (dot0[0] + dot0[1]) + (dot0[2] + dot0[3]);
804
805        let one_over_determinant = T::one() / dot1;
806
807        inverse.mul_assign(one_over_determinant);
808    }
809}
810
811pub mod std140 {
812    use crate::matrix::TMat4x4;
813    use crate::traits::{Pack, Real, Transpose};
814
815    ///
816    /// A wrapper struct that is used to implement std140 packing for the underlying vector
817    ///
818    #[repr(transparent)]
819    #[derive(Clone, Debug)]
820    pub struct TMat4x4P<T: Real>(TMat4x4<T>);
821
822    impl<T: Real> From<TMat4x4<T>> for TMat4x4P<T> {
823        fn from(other: TMat4x4<T>) -> Self {
824            Self(other)
825        }
826    }
827
828    impl<T: Real> Pack for TMat4x4P<T> {
829        type GLSLOutput = [T; 16];
830        type HLSLOutput = [T; 16];
831        type GLSLOutputArray = Self::GLSLOutput;
832        type HLSLOutputArray = Self::HLSLOutput;
833        type CPUOutput = [T; 16];
834
835        #[inline]
836        fn into_packed_glsl(self) -> Self::GLSLOutput {
837            self.0.data
838        }
839
840        #[inline]
841        fn into_packed_hlsl(self) -> Self::HLSLOutput {
842            self.0.transpose().data
843        }
844
845        #[inline]
846        fn into_packed_glsl_array(self) -> Self::GLSLOutputArray {
847            self.0.data
848        }
849
850        #[inline]
851        fn into_packed_hlsl_array(self) -> Self::HLSLOutputArray {
852            self.0.transpose().data
853        }
854
855        #[inline]
856        fn into_packed_cpu(self) -> Self::CPUOutput {
857            self.0.data
858        }
859    }
860}
861
862impl<T: Real> IntoSTD140 for TMat4x4<T> {
863    type Output = std140::TMat4x4P<T>;
864
865    fn into_std140(self) -> Self::Output {
866        std140::TMat4x4P::from(self)
867    }
868}
869
870#[cfg(test)]
871pub mod tests;
nova_math/matrix/mod.rs

nova_math/matrix/
mod.rs
