static_math/
matrix4x4.rs

1//-------------------------------------------------------------------------
2// @file matrix4x4.rs
3//
4// @date 06/02/20 19:54:42
5// @author Martin Noblia
6// @email mnoblia@disroot.org
7//
8// @brief
9//
10// @detail
11//
12// Licence MIT:
13// Copyright <2020> <Martin Noblia>
14//
15// Permission is hereby granted, free of charge, to any person obtaining a copy
16// of this software and associated documentation files (the "Software"), to deal
17// in the Software without restriction, including without limitation the rights
18// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
19// copies of the Software, and to permit persons to whom the Software is
20// furnished to do so, subject to the following conditions:
21//
22// The above copyright notice and this permission notice shall be included in
23// all copies or substantial portions of the Software.  THE SOFTWARE IS PROVIDED
24// "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT
25// LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
26// PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
27// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
28// ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
29// WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
30//-------------------------------------------------------------------------
31use core::fmt;
32use core::ops::{Add, Mul, Div, Sub, AddAssign, SubAssign, Neg};
33use core::ops::{Deref, DerefMut, Index, IndexMut};
34
35use num::{Float, One, Zero, Num, Signed};
36use crate::matrix3x3::*;
37use crate::slices_methods::*;
38use crate::traits::LinearAlgebra;
39use crate::vector4::V4;
40use crate::utils::nearly_zero;
41
42//-------------------------------------------------------------------------
43//                        code
44//-------------------------------------------------------------------------
45/// A static Matrix of 4x4 shape
46#[derive(Copy, Clone, Debug, PartialEq)]
47pub struct M44<T>([[T; 4]; 4]);
48
49impl<T> M44<T> {
50    pub const fn new(data_input: [[T; 4]; 4]) -> M44<T> {
51        M44(data_input)
52    }
53
54    pub const fn rows(&self) -> usize {
55        self.0.len()
56    }
57
58    pub const fn cols(&self) -> usize {
59        self.rows()
60    }
61}
62
63impl<T: Float + core::iter::Sum> LinearAlgebra<T> for M44<T> {
64    fn rows(&self) -> usize {
65        self.0.len()
66    }
67
68    fn cols(&self) -> usize {
69        self.rows()
70    }
71
72    #[inline(always)]
73    fn transpose(&self) -> M44<T> {
74        M44::new([
75            [self[(0, 0)], self[(1, 0)], self[(2, 0)], self[(3, 0)]],
76            [self[(0, 1)], self[(1, 1)], self[(2, 1)], self[(3, 1)]],
77            [self[(0, 2)], self[(1, 2)], self[(2, 2)], self[(3, 2)]],
78            [self[(0, 3)], self[(1, 3)], self[(2, 3)], self[(3, 3)]],
79        ])
80    }
81
82    #[inline(always)]
83    fn trace(&self) -> T {
84        self[(0, 0)] + self[(1, 1)] + self[(2, 2)] + self[(3, 3)]
85    }
86
87    fn norm2(&self) -> T {
88        let a1 = self[(0, 0)];
89        let a2 = self[(0, 1)];
90        let a3 = self[(0, 2)];
91        let a4 = self[(0, 3)];
92        let a5 = self[(1, 0)];
93        let a6 = self[(1, 1)];
94        let a7 = self[(1, 2)];
95        let a8 = self[(1, 3)];
96        let a9 = self[(2, 0)];
97        let a10 = self[(2, 1)];
98        let a11 = self[(2, 2)];
99        let a12 = self[(2, 3)];
100        let a13 = self[(3, 0)];
101        let a14 = self[(3, 1)];
102        let a15 = self[(3, 2)];
103        let a16 = self[(3, 3)];
104
105        T::sqrt(a1 * a1
106                + a2 * a2
107                + a3 * a3
108                + a4 * a4
109                + a5 * a5
110                + a6 * a6
111                + a7 * a7
112                + a8 * a8
113                + a9 * a9
114                + a10 * a10
115                + a11 * a11
116                + a12 * a12
117                + a13 * a13
118                + a14 * a14
119                + a15 * a15
120                + a16 * a16,
121        )
122    }
123
124    #[inline(always)]
125    fn det(&self) -> T {
126        let a1 = self[(0, 0)];
127        let a2 = self[(0, 1)];
128        let a3 = self[(0, 2)];
129        let a4 = self[(0, 3)];
130        let a5 = self[(1, 0)];
131        let a6 = self[(1, 1)];
132        let a7 = self[(1, 2)];
133        let a8 = self[(1, 3)];
134        let a9 = self[(2, 0)];
135        let a10 = self[(2, 1)];
136        let a11 = self[(2, 2)];
137        let a12 = self[(2, 3)];
138        let a13 = self[(3, 0)];
139        let a14 = self[(3, 1)];
140        let a15 = self[(3, 2)];
141        let a16 = self[(3, 3)];
142
143        a1 * a10 * a15 * a8
144        - a1 * a10 * a16 * a7
145        - a1 * a11 * a14 * a8
146        + a1 * a11 * a16 * a6
147        + a1 * a12 * a14 * a7
148        - a1 * a12 * a15 * a6
149        - a10 * a13 * a3 * a8
150        + a10 * a13 * a4 * a7
151        - a10 * a15 * a4 * a5
152        + a10 * a16 * a3 * a5
153        + a11 * a13 * a2 * a8
154        - a11 * a13 * a4 * a6
155        + a11 * a14 * a4 * a5
156        - a11 * a16 * a2 * a5
157        - a12 * a13 * a2 * a7
158        + a12 * a13 * a3 * a6
159        - a12 * a14 * a3 * a5
160        + a12 * a15 * a2 * a5
161        + a14 * a3 * a8 * a9
162        - a14 * a4 * a7 * a9
163        - a15 * a2 * a8 * a9
164        + a15 * a4 * a6 * a9
165        + a16 * a2 * a7 * a9
166        - a16 * a3 * a6 * a9
167    }
168
169    /// Calculate the inverse
170    fn inverse(&self) -> Option<Self> {
171        let det = self.det();
172        if !nearly_zero(det) {
173            let det_recip = det.recip();
174            let a1 = self.get_submatrix((0, 0)).det() * det_recip;
175            let a2 = -self.get_submatrix((1, 0)).det() * det_recip;
176            let a3 = self.get_submatrix((2, 0)).det() * det_recip;
177            let a4 = -self.get_submatrix((3, 0)).det() * det_recip;
178
179            let a5 = -self.get_submatrix((0, 1)).det() * det_recip;
180            let a6 = self.get_submatrix((1, 1)).det() * det_recip;
181            let a7 = -self.get_submatrix((2, 1)).det() * det_recip;
182            let a8 = self.get_submatrix((3, 1)).det() * det_recip;
183
184            let a9 = self.get_submatrix((0, 2)).det() * det_recip;
185            let a10 = -self.get_submatrix((1, 2)).det() * det_recip;
186            let a11 = self.get_submatrix((2, 2)).det() * det_recip;
187            let a12 = -self.get_submatrix((3, 2)).det() * det_recip;
188
189            let a13 = -self.get_submatrix((0, 3)).det() * det_recip;
190            let a14 = self.get_submatrix((1, 3)).det() * det_recip;
191            let a15 = -self.get_submatrix((2, 3)).det() * det_recip;
192            let a16 = self.get_submatrix((3, 3)).det() * det_recip;
193
194            Some(M44::new([
195                [a1, a2 , a3 , a4],
196                [a5, a6 , a7 , a8],
197                [a9, a10, a11, a12],
198                [a13, a14, a15, a16],
199            ]))
200        } else {
201            None
202        }
203    }
204
205    /// Calculate de QR factorization of the M44 via gram-schmidt
206    /// orthogonalization process
207    fn qr(&self) -> Option<(Self, Self)> {
208        if !nearly_zero(self.det()) {
209            let cols = self.get_cols();
210            let mut q: [V4<T>; 4] = *M44::zeros().get_cols();
211            for i in 0..q.len() {
212                let mut q_tilde = cols[i];
213                for elem in q.iter().take(i) {
214                    q_tilde -= *elem * project_x_over_y(&*cols[i], &**elem);
215                }
216                normalize(&mut *q_tilde);
217                q[i] = q_tilde;
218            }
219            let basis = V4::new_from(q[0], q[1], q[2], q[3]);
220            let q     = M44::new_from_vecs(basis);
221            let r     = q.transpose() * (*self);
222            Some((q, r))
223        } else {
224            None
225        }
226    }
227
228}
229
230
231impl<T: Num + Copy> M44<T> {
232    /// contruct identity matrix
233    pub fn identity() -> M44<T> {
234        <M44<T> as One>::one()
235    }
236
237    /// construct the matrix with all zeros
238    pub fn zeros() -> M44<T> {
239        <M44<T> as Zero>::zero()
240    }
241
242    /// transform the matrix to a flatten vector
243    pub fn as_vec(&self) -> [T; 16] {
244        let mut result = [T::zero(); 16];
245        for (index, element) in self.iter().flatten().enumerate() {
246            result[index] = *element;
247        }
248        result
249    }
250
251    pub fn new_from_vecs(cols: V4<V4<T>>) -> Self {
252        let mut result = Self::zeros();
253
254        for i in 0..result.cols() {
255            result[(i, 0)] = cols[0][i];
256            result[(i, 1)] = cols[1][i];
257            result[(i, 2)] = cols[2][i];
258            result[(i, 3)] = cols[3][i];
259        }
260        result
261    }
262
263    /// get the diagonal of the matrix
264    pub fn get_diagonal(&self) -> V4<T> {
265        let mut result = V4::zeros();
266        let mut index: usize = 0;
267        for i in 0..self.rows() {
268            for j in 0..self.cols() {
269                if i == j {
270                    result[index] = self[(i, j)];
271                    index += 1;
272                }
273            }
274        }
275        result
276    }
277
278    // NOTE(elsuizo:2021-04-30): with this inline the performance its worse
279    // #[inline(always)]
280    pub fn get_submatrix(&self, selected: (usize, usize)) -> M33<T> {
281        let mut values: [T; 9] = [T::zero(); 9];
282        let mut result: M33<T> = M33::zeros();
283        let mut index: usize = 0;
284        for i in 0..4 {
285            for j in 0..4 {
286                if !(i == selected.0 || j == selected.1) {
287                    // get the values from the M33
288                    values[index] = self[(i, j)];
289                    index += 1;
290                }
291            }
292        }
293        index = 0;
294        for r in 0..3 {
295            for c in 0..3 {
296                result[(r, c)] = values[index];
297                index += 1;
298            }
299        }
300        result
301    }
302
303    pub fn get_rows(self) -> V4<V4<T>> {
304        let mut r0 = V4::zeros();
305        let mut r1 = V4::zeros();
306        let mut r2 = V4::zeros();
307        let mut r3 = V4::zeros();
308
309        for j in 0..self.rows() {
310            r0[j] = self[(0, j)];
311            r1[j] = self[(1, j)];
312            r2[j] = self[(2, j)];
313            r3[j] = self[(3, j)];
314        }
315        V4::new([r0, r1, r2, r3])
316    }
317
318    pub fn get_cols(self) -> V4<V4<T>> {
319        let mut c0 = V4::zeros();
320        let mut c1 = V4::zeros();
321        let mut c2 = V4::zeros();
322        let mut c3 = V4::zeros();
323
324        for i in 0..self.cols() {
325            c0[i] = self[(i, 0)];
326            c1[i] = self[(i, 1)];
327            c2[i] = self[(i, 2)];
328            c3[i] = self[(i, 3)];
329        }
330        V4::new([c0, c1, c2, c3])
331    }
332
333    pub fn get_upper_triagular(&self) -> [T; 6] {
334        let zero = T::zero();
335        let mut result: [T; 6] = [zero; 6];
336        let mut index = 0;
337        for i in 0..self.rows() {
338            for j in 0..self.cols() {
339                if i < j {
340                    result[index] = self[(i, j)];
341                    index += 1;
342                }
343            }
344        }
345        result
346    }
347
348    pub fn get_lower_triangular(&self) -> [T; 6] {
349        let zero = T::zero();
350        let mut result: [T; 6] = [zero; 6];
351        let mut index = 0;
352        for i in 0..self.rows() {
353            for j in 0..self.cols() {
354                if i > j {
355                    result[index] = self[(i, j)];
356                    index += 1;
357                }
358            }
359        }
360        result
361    }
362
363    /// Applies `f` of each element in the M44
364    pub fn for_each(&self, f: impl Fn(T) -> T) -> Self {
365        let mut result = Self::zeros();
366        for i in 0..self.rows() {
367            for j in 0..self.cols() {
368                result[(i, j)] = f(self[(i, j)]);
369            }
370        }
371        result
372    }
373
374}
375
376impl<T: Num + Copy> Add for M44<T> {
377    type Output = Self;
378
379    fn add(self, rhs: Self) -> Self {
380        let a1 = self[(0, 0)];
381        let a2 = self[(0, 1)];
382        let a3 = self[(0, 2)];
383        let a4 = self[(0, 3)];
384        let a5 = self[(1, 0)];
385        let a6 = self[(1, 1)];
386        let a7 = self[(1, 2)];
387        let a8 = self[(1, 3)];
388        let a9 = self[(2, 0)];
389        let a10 = self[(2, 1)];
390        let a11 = self[(2, 2)];
391        let a12 = self[(2, 3)];
392        let a13 = self[(3, 0)];
393        let a14 = self[(3, 1)];
394        let a15 = self[(3, 2)];
395        let a16 = self[(3, 3)];
396        let b1 = rhs[(0, 0)];
397        let b2 = rhs[(0, 1)];
398        let b3 = rhs[(0, 2)];
399        let b4 = rhs[(0, 3)];
400        let b5 = rhs[(1, 0)];
401        let b6 = rhs[(1, 1)];
402        let b7 = rhs[(1, 2)];
403        let b8 = rhs[(1, 3)];
404        let b9 = rhs[(2, 0)];
405        let b10 = rhs[(2, 1)];
406        let b11 = rhs[(2, 2)];
407        let b12 = rhs[(2, 3)];
408        let b13 = rhs[(3, 0)];
409        let b14 = rhs[(3, 1)];
410        let b15 = rhs[(3, 2)];
411        let b16 = rhs[(3, 3)];
412        M44::new([
413            [a1 + b1, a2 + b2, a3 + b3, a4 + b4],
414            [a5 + b5, a6 + b6, a7 + b7, a8 + b8],
415            [a9 + b9, a10 + b10, a11 + b11, a12 + b12],
416            [a13 + b13, a14 + b14, a15 + b15, a16 + b16],
417        ])
418    }
419}
420
421// M44 += M44
422impl<T: Num + Copy> AddAssign for M44<T> {
423    fn add_assign(&mut self, other: Self) {
424        *self = *self + other
425    }
426}
427
428// M44 - M44
429impl<T: Num + Copy> Sub for M44<T> {
430    type Output = Self;
431
432    fn sub(self, rhs: Self) -> Self {
433        let a1  = self[(0, 0)];
434        let a2  = self[(0, 1)];
435        let a3  = self[(0, 2)];
436        let a4  = self[(0, 3)];
437        let a5  = self[(1, 0)];
438        let a6  = self[(1, 1)];
439        let a7  = self[(1, 2)];
440        let a8  = self[(1, 3)];
441        let a9  = self[(2, 0)];
442        let a10 = self[(2, 1)];
443        let a11 = self[(2, 2)];
444        let a12 = self[(2, 3)];
445        let a13 = self[(3, 0)];
446        let a14 = self[(3, 1)];
447        let a15 = self[(3, 2)];
448        let a16 = self[(3, 3)];
449
450        let b1 = rhs[(0, 0)];
451        let b2 = rhs[(0, 1)];
452        let b3 = rhs[(0, 2)];
453        let b4 = rhs[(0, 3)];
454        let b5 = rhs[(1, 0)];
455        let b6 = rhs[(1, 1)];
456        let b7 = rhs[(1, 2)];
457        let b8 = rhs[(1, 3)];
458        let b9 = rhs[(2, 0)];
459        let b10 = rhs[(2, 1)];
460        let b11 = rhs[(2, 2)];
461        let b12 = rhs[(2, 3)];
462        let b13 = rhs[(3, 0)];
463        let b14 = rhs[(3, 1)];
464        let b15 = rhs[(3, 2)];
465        let b16 = rhs[(3, 3)];
466        M44::new([
467            [a1 - b1, a2 - b2, a3 - b3, a4 - b4],
468            [a5 - b5, a6 - b6, a7 - b7, a8 - b8],
469            [a9 - b9, a10 - b10, a11 - b11, a12 - b12],
470            [a13 - b13, a14 - b14, a15 - b15, a16 - b16],
471        ])
472    }
473}
474
475// M44 -= M44
476impl<T: Num + Copy> SubAssign for M44<T> {
477    fn sub_assign(&mut self, other: Self) {
478        *self = *self - other
479    }
480}
481
482// M44 * constant
483impl<T: Num + Copy> Mul<T> for M44<T> {
484    type Output = M44<T>;
485
486    fn mul(self, rhs: T) -> M44<T> {
487        let a1 = self[(0, 0)] * rhs;
488        let a2 = self[(0, 1)] * rhs;
489        let a3 = self[(0, 2)] * rhs;
490        let a4 = self[(0, 3)] * rhs;
491        let a5 = self[(1, 0)] * rhs;
492        let a6 = self[(1, 1)] * rhs;
493        let a7 = self[(1, 2)] * rhs;
494        let a8 = self[(1, 3)] * rhs;
495        let a9 = self[(2, 0)] * rhs;
496        let a10 = self[(2, 1)] * rhs;
497        let a11 = self[(2, 2)] * rhs;
498        let a12 = self[(2, 3)] * rhs;
499        let a13 = self[(3, 0)] * rhs;
500        let a14 = self[(3, 1)] * rhs;
501        let a15 = self[(3, 2)] * rhs;
502        let a16 = self[(3, 3)] * rhs;
503
504        M44::new([
505            [a1, a2, a3, a4],
506            [a5, a6, a7, a8],
507            [a9, a10, a11, a12],
508            [a13, a14, a15, a16],
509        ])
510    }
511}
512
513// M44 / constant
514impl<T: Num + Copy> Div<T> for M44<T> {
515    type Output = Self;
516
517    fn div(self, rhs: T) -> Self::Output {
518        let a1 = self[(0, 0)] / rhs;
519        let a2 = self[(0, 1)] / rhs;
520        let a3 = self[(0, 2)] / rhs;
521        let a4 = self[(0, 3)] / rhs;
522        let a5 = self[(1, 0)] / rhs;
523        let a6 = self[(1, 1)] / rhs;
524        let a7 = self[(1, 2)] / rhs;
525        let a8 = self[(1, 3)] / rhs;
526        let a9 = self[(2, 0)] / rhs;
527        let a10 = self[(2, 1)] / rhs;
528        let a11 = self[(2, 2)] / rhs;
529        let a12 = self[(2, 3)] / rhs;
530        let a13 = self[(3, 0)] / rhs;
531        let a14 = self[(3, 1)] / rhs;
532        let a15 = self[(3, 2)] / rhs;
533        let a16 = self[(3, 3)] / rhs;
534
535        M44::new([
536            [a1, a2, a3, a4],
537            [a5, a6, a7, a8],
538            [a9, a10, a11, a12],
539            [a13, a14, a15, a16],
540        ])
541    }
542}
543
544// M44 * M44
545impl<T: Num + Copy> Mul for M44<T> {
546    type Output = Self;
547
548    fn mul(self, rhs: Self) -> Self {
549        let a1 = self[(0, 0)];
550        let a2 = self[(0, 1)];
551        let a3 = self[(0, 2)];
552        let a4 = self[(0, 3)];
553        let a5 = self[(1, 0)];
554        let a6 = self[(1, 1)];
555        let a7 = self[(1, 2)];
556        let a8 = self[(1, 3)];
557        let a9 = self[(2, 0)];
558        let a10 = self[(2, 1)];
559        let a11 = self[(2, 2)];
560        let a12 = self[(2, 3)];
561        let a13 = self[(3, 0)];
562        let a14 = self[(3, 1)];
563        let a15 = self[(3, 2)];
564        let a16 = self[(3, 3)];
565        let b1 = rhs[(0, 0)];
566        let b2 = rhs[(0, 1)];
567        let b3 = rhs[(0, 2)];
568        let b4 = rhs[(0, 3)];
569        let b5 = rhs[(1, 0)];
570        let b6 = rhs[(1, 1)];
571        let b7 = rhs[(1, 2)];
572        let b8 = rhs[(1, 3)];
573        let b9 = rhs[(2, 0)];
574        let b10 = rhs[(2, 1)];
575        let b11 = rhs[(2, 2)];
576        let b12 = rhs[(2, 3)];
577        let b13 = rhs[(3, 0)];
578        let b14 = rhs[(3, 1)];
579        let b15 = rhs[(3, 2)];
580        let b16 = rhs[(3, 3)];
581        M44::new([
582            [
583                a1 * b1 + a2 * b5 + a3 * b9 + a4 * b13,
584                a1 * b2 + a2 * b6 + a3 * b10 + a4 * b14,
585                a1 * b3 + a2 * b7 + a3 * b11 + a4 * b15,
586                a1 * b4 + a2 * b8 + a3 * b12 + a4 * b16,
587            ],
588            [
589                a5 * b1 + a6 * b5 + a7 * b9 + a8 * b13,
590                a5 * b2 + a6 * b6 + a7 * b10 + a8 * b14,
591                a5 * b3 + a6 * b7 + a7 * b11 + a8 * b15,
592                a5 * b4 + a6 * b8 + a7 * b12 + a8 * b16,
593            ],
594            [
595                a10 * b5 + a11 * b9 + a12 * b13 + a9 * b1,
596                a10 * b6 + a11 * b10 + a12 * b14 + a9 * b2,
597                a10 * b7 + a11 * b11 + a12 * b15 + a9 * b3,
598                a10 * b8 + a11 * b12 + a12 * b16 + a9 * b4,
599            ],
600            [
601                a13 * b1 + a14 * b5 + a15 * b9 + a16 * b13,
602                a13 * b2 + a14 * b6 + a15 * b10 + a16 * b14,
603                a13 * b3 + a14 * b7 + a15 * b11 + a16 * b15,
604                a13 * b4 + a14 * b8 + a15 * b12 + a16 * b16,
605            ],
606        ])
607    }
608}
609
610// -M44
611impl<T: Num + Copy + Signed> Neg for M44<T> {
612    type Output = Self;
613
614    fn neg(self) -> Self {
615        let a1  = -self[(0, 0)];
616        let a2  = -self[(0, 1)];
617        let a3  = -self[(0, 2)];
618        let a4  = -self[(0, 3)];
619        let a5  = -self[(1, 0)];
620        let a6  = -self[(1, 1)];
621        let a7  = -self[(1, 2)];
622        let a8  = -self[(1, 3)];
623        let a9  = -self[(2, 0)];
624        let a10 = -self[(2, 1)];
625        let a11 = -self[(2, 2)];
626        let a12 = -self[(2, 3)];
627        let a13 = -self[(3, 0)];
628        let a14 = -self[(3, 1)];
629        let a15 = -self[(3, 2)];
630        let a16 = -self[(3, 3)];
631
632        M44::new([
633            [a1, a2, a3, a4],
634            [a5, a6, a7, a8],
635            [a9, a10, a11, a12],
636            [a13, a14, a15, a16],
637        ])
638    }
639}
640// M44 * V4
641impl<T: Num + Copy> Mul<V4<T>> for M44<T> {
642    type Output = V4<T>;
643
644    #[inline]
645    fn mul(self, rhs: V4<T>) -> V4<T> {
646        let a_00 = self[(0, 0)];
647        let a_01 = self[(0, 1)];
648        let a_02 = self[(0, 2)];
649        let a_03 = self[(0, 3)];
650        let a_10 = self[(1, 0)];
651        let a_11 = self[(1, 1)];
652        let a_12 = self[(1, 2)];
653        let a_13 = self[(1, 3)];
654        let a_20 = self[(2, 0)];
655        let a_21 = self[(2, 1)];
656        let a_22 = self[(2, 2)];
657        let a_23 = self[(2, 3)];
658        let a_30 = self[(3, 0)];
659        let a_31 = self[(3, 1)];
660        let a_32 = self[(3, 2)];
661        let a_33 = self[(3, 3)];
662
663        let a = rhs[0];
664        let b = rhs[1];
665        let c = rhs[2];
666        let d = rhs[3];
667
668        V4::new_from(a_00 * a + a_01 * b + a_02 * c + a_03 * d,
669                     a_10 * a + a_11 * b + a_12 * c + a_13 * d,
670                     a_20 * a + a_21 * b + a_22 * c + a_23 * d,
671                     a_30 * a + a_31 * b + a_32 * c + a_33 * d)
672    }
673
674}
675
676impl<T: Num + Copy> Zero for M44<T> {
677    fn zero() -> M44<T> {
678        M44::new([[T::zero(); 4]; 4])
679    }
680
681    fn is_zero(&self) -> bool {
682        *self == M44::zero()
683    }
684}
685
686impl<T: Num + Copy> One for M44<T> {
687    /// Create an identity matrix
688    fn one() -> M44<T> {
689        let one = T::one();
690        let zero = T::zero();
691        M44::new([
692            [one, zero, zero, zero],
693            [zero, one, zero, zero],
694            [zero, zero, one, zero],
695            [zero, zero, zero, one],
696        ])
697    }
698}
699//
700impl<T> Deref for M44<T> {
701    type Target = [[T; 4]; 4];
702    #[inline]
703    fn deref(&self) -> &Self::Target {
704        &self.0
705    }
706}
707
708impl<T> DerefMut for M44<T> {
709    #[inline]
710    fn deref_mut(&mut self) -> &mut Self::Target {
711        &mut self.0
712    }
713}
714
715impl<T> From<[[T; 4]; 4]> for M44<T> {
716    fn from(data: [[T; 4]; 4]) -> M44<T> {
717        M44(data)
718    }
719}
720
721impl<T> Index<(usize, usize)> for M44<T> {
722    type Output = T;
723    #[inline(always)]
724    fn index(&self, index: (usize, usize)) -> &T {
725        &self.0[index.0][index.1]
726    }
727}
728
729impl<T> IndexMut<(usize, usize)> for M44<T> {
730    #[inline(always)]
731    fn index_mut(&mut self, index: (usize, usize)) -> &mut T {
732        &mut self.0[index.0][index.1]
733    }
734}
735
736// TODO(elsuizo:2020-03-26): hay que hacerlo mas "inteligente" para que cuando
737// ponemos un numero de mas de 1 cifra no se rompa
738//-------------------------------------------------------------------------
739//                        Display
740//-------------------------------------------------------------------------
741impl<T: Num + fmt::Display> fmt::Display for M44<T> {
742    fn fmt(&self, dest: &mut fmt::Formatter) -> fmt::Result {
743        println!();
744        writeln!(
745            dest,
746            "|{0:<7.2} {1:^7.2} {2:^7.2} {3:>7.2}|",
747            self[(0, 0)],
748            self[(0, 1)],
749            self[(0, 2)],
750            self[(0, 3)]
751        )?;
752        writeln!(
753            dest,
754            "|{0:<7.2} {1:^7.2} {2:^7.2} {3:>7.2}|",
755            self[(1, 0)],
756            self[(1, 1)],
757            self[(1, 2)],
758            self[(1, 3)]
759        )?;
760        writeln!(
761            dest,
762            "|{0:<7.2} {1:^7.2} {2:^7.2} {3:>7.2}|",
763            self[(2, 0)],
764            self[(2, 1)],
765            self[(2, 2)],
766            self[(2, 3)]
767        )?;
768        writeln!(
769            dest,
770            "|{0:<7.2} {1:^7.2} {2:^7.2} {3:>7.2}|",
771            self[(3, 0)],
772            self[(3, 1)],
773            self[(3, 2)],
774            self[(3, 3)]
775        )
776    }
777}
778
779//-------------------------------------------------------------------------
780//                        macros
781//-------------------------------------------------------------------------
782#[macro_export]
783macro_rules! m44_new {
784    ($($first_row:expr),*;
785     $($second_row:expr),*;
786     $($third_row:expr),*;
787     $($fourth_row:expr),*
788     ) => {
789        M44::new([[$($first_row),*],
790                 [$($second_row),*],
791                 [$($third_row),*],
792                 [$($fourth_row),*]])
793    }
794}
795
796//-------------------------------------------------------------------------
797//                        tests
798//-------------------------------------------------------------------------
799
800
801#[cfg(test)]
802mod test_matrix4x4 {
803    use crate::traits::LinearAlgebra;
804    use crate::matrix3x3::M33;
805    use crate::matrix4x4::M44;
806    use crate::vector4::V4;
807    use crate::utils::nearly_equal;
808    use crate::utils::compare_vecs;
809
810    const EPS: f32 = 1e-8;
811
812    #[test]
813    fn matrix4x4_create_matrix4x4_test() {
814        let m = M44::new([
815            [1.0, 2.0, 3.0, 4.0],
816            [5.0, 6.0, 7.0, 8.0],
817            [9.0, 10.0, 11.0, 12.0],
818            [13.0, 14.0, 15.0, 16.0],
819        ]);
820
821        assert_eq!(m[(1, 1)], 6.0);
822    }
823
824    #[test]
825    fn matrix4x4_identity_creation_test() {
826
827        use super::test_matrix4x4::EPS;
828
829        let expected = M44::new([
830            [1.0, 0.0, 0.0, 0.0],
831            [0.0, 1.0, 0.0, 0.0],
832            [0.0, 0.0, 1.0, 0.0],
833            [0.0, 0.0, 0.0, 1.0],
834        ]);
835        let result: M44<f32> = M44::identity();
836        assert!(compare_vecs(&result.as_vec(), &expected.as_vec(), EPS));
837    }
838
839    #[test]
840    fn matrix4x4_add_test() {
841
842        use super::test_matrix4x4::EPS;
843
844        let m1 = M44::new([
845            [1.0, 2.0, 3.0, 4.0],
846            [5.0, 6.0, 7.0, 8.0],
847            [9.0, 10.0, 11.0, 12.0],
848            [13.0, 14.0, 15.0, 16.0],
849        ]);
850
851        let m2 = M44::new([
852            [1.0, 2.0, 3.0, 4.0],
853            [5.0, 6.0, 7.0, 8.0],
854            [9.0, 10.0, 11.0, 12.0],
855            [13.0, 14.0, 15.0, 16.0],
856        ]);
857
858        let expected = M44::new([
859            [2.0, 4.0, 6.0, 8.0],
860            [10.0, 12.0, 14.0, 16.0],
861            [18.0, 20.0, 22.0, 24.0],
862            [26.0, 28.0, 30.0, 32.0],
863        ]);
864        let result = m1 + m2;
865        assert!(compare_vecs(&result.as_vec(), &expected.as_vec(), EPS));
866    }
867
868    #[test]
869    fn mul_vector_rhs() {
870        let m = m44_new!(1.0,  2.0,  3.0,  4.0;
871                         5.0,  6.0,  7.0,  8.0;
872                         9.0, 10.0, 11.0, 12.0;
873                        13.0, 14.0, 15.0, 16.0);
874
875        let v = V4::new([0.0, 1.0, 2.0, 3.0]);
876
877        let result = m * v;
878        let expected = V4::new([20.0, 44.0, 68.0, 92.0]);
879
880        assert_eq!(
881            &result[..],
882            &expected[..],
883            "\nExpected\n{:?}\nfound\n{:?}",
884            &result[..],
885            &expected[..]
886        );
887    }
888
889    #[test]
890    fn sub_test() {
891        use super::test_matrix4x4::EPS;
892
893        let m1 = m44_new!( 1.0,  2.0,  3.0,  4.0;
894                           5.0,  6.0,  7.0,  8.0;
895                           9.0, 10.0, 11.0, 12.0;
896                          13.0, 14.0, 15.0, 16.0);
897
898        let m2 = m44_new!( 1.0,  2.0,  3.0,  4.0;
899                           5.0,  6.0,  7.0,  8.0;
900                           9.0, 10.0, 11.0, 12.0;
901                          13.0, 14.0, 15.0, 16.0);
902        let result = m1 - m2;
903        let expected = m44_new!( 0.0,  0.0,  0.0,  0.0;
904                                 0.0,  0.0,  0.0,  0.0;
905                                 0.0,  0.0,  0.0,  0.0;
906                                 0.0,  0.0,  0.0,  0.0);
907        assert!(compare_vecs(&result.as_vec(), &expected.as_vec(), EPS));
908    }
909
910    #[test]
911    fn matrix4x4_product_test() {
912
913        use super::test_matrix4x4::EPS;
914
915        let m1 = M44::new([
916            [1.0, 2.0, 3.0, 4.0],
917            [5.0, 6.0, 7.0, 8.0],
918            [9.0, 10.0, 11.0, 12.0],
919            [13.0, 14.0, 15.0, 16.0],
920        ]);
921
922        let m2 = M44::new([
923            [1.0, 2.0, 3.0, 4.0],
924            [5.0, 6.0, 7.0, 8.0],
925            [9.0, 10.0, 11.0, 12.0],
926            [13.0, 14.0, 15.0, 16.0],
927        ]);
928
929        let expected = M44::new([
930            [90.0, 100.0, 110.0, 120.0],
931            [202.0, 228.0, 254.0, 280.0],
932            [314.0, 356.0, 398.0, 440.0],
933            [426.0, 484.0, 542.0, 600.0],
934        ]);
935        let result = m1 * m2;
936        assert!(compare_vecs(&result.as_vec(), &expected.as_vec(), EPS));
937    }
938
939    #[test]
940    fn matrix4x4_det_test() {
941        let m1 = M44::new([
942            [1.0, 2.0, 3.0, 1.0],
943            [5.0, 6.0, 7.0, 8.0],
944            [9.0, 0.0, 11.0, 12.0],
945            [13.0, 1.0, 15.0, 16.0],
946        ]);
947
948        let expected = 168.0;
949        let result = m1.det();
950        assert_eq!(result, expected);
951    }
952
953    #[test]
954    fn matrix4x4_norm_test() {
955
956        use super::test_matrix4x4::EPS;
957
958        let m1 = M44::new([
959            [1.0, 2.0, 3.0, 4.0],
960            [5.0, 6.0, 7.0, 8.0],
961            [9.0, 10.0, 11.0, 12.0],
962            [13.0, 14.0, 15.0, 16.0],
963        ]);
964        // NOTE(elsuizo:2019-08-08): el resultado lo calculo con Julia
965        let expected = 38.67815921162743;
966        let result = m1.norm2();
967        assert!(nearly_equal(result, expected, EPS));
968    }
969
970    #[test]
971    fn matrix4x4_transpose_test() {
972
973        use super::test_matrix4x4::EPS;
974
975        let m1 = M44::new([
976            [1.0, 2.0, 3.0, 4.0],
977            [5.0, 6.0, 7.0, 8.0],
978            [9.0, 10.0, 11.0, 12.0],
979            [13.0, 14.0, 15.0, 16.0],
980        ]);
981
982        let expected = M44::new([
983            [1.0, 5.0, 9.0, 13.0],
984            [2.0, 6.0, 10.0, 14.0],
985            [3.0, 7.0, 11.0, 15.0],
986            [4.0, 8.0, 12.0, 16.0],
987        ]);
988        let result = m1.transpose();
989        assert!(compare_vecs(&result.as_vec(), &expected.as_vec(), EPS));
990    }
991
992    #[test]
993    fn matrix4x4_zeros_test() {
994
995        use super::test_matrix4x4::EPS;
996
997        let expected = M44::new([
998            [0.0, 0.0, 0.0, 0.0],
999            [0.0, 0.0, 0.0, 0.0],
1000            [0.0, 0.0, 0.0, 0.0],
1001            [0.0, 0.0, 0.0, 0.0],
1002        ]);
1003        let result: M44<f32> = M44::zeros();
1004        assert!(compare_vecs(&result.as_vec(), &expected.as_vec(), EPS));
1005    }
1006
1007    #[test]
1008    fn matrix4x4_get_submatrix_test() {
1009
1010        use super::test_matrix4x4::EPS;
1011
1012        let m = M44::new([
1013            [1.0, 2.0, 3.0, 4.0],
1014            [5.0, 6.0, 7.0, 8.0],
1015            [9.0, 10.0, 11.0, 12.0],
1016            [13.0, 14.0, 15.0, 16.0],
1017        ]);
1018
1019        let result1 = m.get_submatrix((0, 0));
1020
1021        let expected1 = M33::new([[6.0, 7.0, 8.0], [10.0, 11.0, 12.0], [14.0, 15.0, 16.0]]);
1022
1023        assert!(compare_vecs(&result1.as_vec(), &expected1.as_vec(), EPS));
1024
1025        let result2 = m.get_submatrix((0, 1));
1026
1027        let expected2 = M33::new([[5.0, 7.0, 8.0], [9.0, 11.0, 12.0], [13.0, 15.0, 16.0]]);
1028
1029        assert!(compare_vecs(&result2.as_vec(), &expected2.as_vec(), EPS));
1030
1031        let result3 = m.get_submatrix((0, 2));
1032
1033        let expected3 = M33::new([[5.0, 6.0, 8.0], [9.0, 10.0, 12.0], [13.0, 14.0, 16.0]]);
1034
1035        assert!(compare_vecs(&result3.as_vec(), &expected3.as_vec(), EPS));
1036    }
1037
1038    #[test]
1039    fn matrix4x4_inverse_test() {
1040
1041        use super::test_matrix4x4::EPS;
1042
1043        let m = M44::new([
1044            [1.0, 1.0, 1.0, -1.0],
1045            [1.0, 1.0, -1.0, 1.0],
1046            [1.0, -1.0, 1.0, 1.0],
1047            [-1.0, 1.0, 1.0, 1.0],
1048        ]);
1049
1050        let expected = M44::new([
1051            [1.0 / 4.0, 1.0 / 4.0, 1.0 / 4.0, -1.0 / 4.0],
1052            [1.0 / 4.0, 1.0 / 4.0, -1.0 / 4.0, 1.0 / 4.0],
1053            [1.0 / 4.0, -1.0 / 4.0, 1.0 / 4.0, 1.0 / 4.0],
1054            [-1.0 / 4.0, 1.0 / 4.0, 1.0 / 4.0, 1.0 / 4.0],
1055        ]);
1056
1057        if let Some(result) = m.inverse() {
1058            assert!(compare_vecs(&result.as_vec(), &expected.as_vec(), EPS));
1059        }
1060    }
1061
1062    #[test]
1063    fn inverse_fail() {
1064        let m = M44::new([
1065            [1.0, 1.0, 1.0, -1.0],
1066            [1.0, 1.0, 1.0, -1.0],
1067            [1.0, -1.0, 1.0, 1.0],
1068            [-1.0, 1.0, 1.0, 1.0],
1069        ]);
1070
1071        let result = m.inverse();
1072        let expected = None;
1073        assert_eq!(result, expected);
1074    }
1075
1076    #[test]
1077    fn get_cols_test() {
1078        let m = m44_new!( 0.0,  1.0,  2.0,  3.0;
1079                          4.0,  5.0,  6.0,  7.0;
1080                          8.0,  9.0, 10.0, 11.0;
1081                         12.0, 13.0, 14.0, 15.0);
1082        let result = m.get_cols();
1083
1084        let expected0 = V4::new([0.0, 4.0, 8.0, 12.0]);
1085        let expected1 = V4::new([1.0, 5.0, 9.0, 13.0]);
1086        let expected2 = V4::new([2.0, 6.0, 10.0, 14.0]);
1087        let expected3 = V4::new([3.0, 7.0, 11.0, 15.0]);
1088
1089        let expected = V4::new([expected0, expected1, expected2, expected3]);
1090
1091        assert_eq!(
1092            &result[..],
1093            &expected[..],
1094            "\nExpected\n{:?}\nfound\n{:?}",
1095            &result[..],
1096            &expected[..]
1097        );
1098
1099    }
1100
1101    #[test]
1102    fn get_rows_test() {
1103        let m = m44_new!( 0.0,  1.0,  2.0,  3.0;
1104                          4.0,  5.0,  6.0,  7.0;
1105                          8.0,  9.0, 10.0, 11.0;
1106                         12.0, 13.0, 14.0, 15.0);
1107        let result = m.get_rows();
1108
1109        let expected0 = V4::new([0.0, 1.0, 2.0, 3.0]);
1110        let expected1 = V4::new([4.0, 5.0, 6.0, 7.0]);
1111        let expected2 = V4::new([8.0, 9.0, 10.0, 11.0]);
1112        let expected3 = V4::new([12.0, 13.0, 14.0, 15.0]);
1113
1114        let expected = V4::new([expected0, expected1, expected2, expected3]);
1115
1116        assert_eq!(
1117            &result[..],
1118            &expected[..],
1119            "\nExpected\n{:?}\nfound\n{:?}",
1120            &result[..],
1121            &expected[..]
1122        );
1123
1124    }
1125
1126    #[test]
1127    fn new_from_vecs_test() {
1128        let expected = m44_new!( 0.0,  1.0,  2.0,  3.0;
1129                                 4.0,  5.0,  6.0,  7.0;
1130                                 8.0,  9.0, 10.0, 11.0;
1131                                12.0, 13.0, 14.0, 15.0);
1132
1133        let cols = expected.get_cols();
1134
1135        let result = M44::new_from_vecs(cols);
1136
1137        assert!(compare_vecs(&result.as_vec(), &expected.as_vec(), EPS));
1138    }
1139
1140    #[test]
1141    fn qr_test() {
1142        let expected = m44_new!( 0.0,  1.0,  2.0,  3.0;
1143                                 4.0,  5.0,  6.0,  7.0;
1144                                 8.0,  9.0, 10.0, 11.0;
1145                                12.0, 13.0, 14.0, 15.0);
1146        if let Some((q, r)) = expected.qr() {
1147            let result = q * r;
1148            assert!(compare_vecs(&result.as_vec(), &expected.as_vec(), EPS));
1149            assert!(nearly_equal(q.det().abs(), 1.0, EPS));
1150        }
1151    }
1152
1153    #[test]
1154    fn get_diagonal() {
1155        let m = m44_new!( 0.0,  1.0,  2.0,  3.0;
1156                          4.0,  5.0,  6.0,  7.0;
1157                          8.0,  9.0, 10.0, 11.0;
1158                         12.0, 13.0, 14.0, 15.0);
1159        let result = m.get_diagonal();
1160        let expected = V4::new([0.0, 5.0, 10.0, 15.0]);
1161        assert_eq!(
1162            &result[..],
1163            &expected[..],
1164            "\nExpected\n{:?}\nfound\n{:?}",
1165            &result[..],
1166            &expected[..]
1167        );
1168    }
1169
1170    #[test]
1171    fn get_upper_triangular_test() {
1172        let m = m44_new!( 0.0,  1.0,  2.0,  3.0;
1173                          4.0,  5.0,  6.0,  7.0;
1174                          8.0,  9.0, 10.0, 11.0;
1175                         12.0, 13.0, 14.0, 15.0);
1176        let result = m.get_upper_triagular();
1177        let expected = [1.0, 2.0, 3.0, 6.0, 7.0, 11.0];
1178        assert_eq!(
1179            &result[..],
1180            &expected[..],
1181            "\nExpected\n{:?}\nfound\n{:?}",
1182            &result[..],
1183            &expected[..]
1184        );
1185    }
1186
1187    #[test]
1188    fn get_lower_triangular_test() {
1189        let m = m44_new!( 0.0,  1.0,  2.0,  3.0;
1190                          4.0,  5.0,  6.0,  7.0;
1191                          8.0,  9.0, 10.0, 11.0;
1192                         12.0, 13.0, 14.0, 15.0);
1193
1194        let result = m.get_lower_triangular();
1195        let expected = [4.0, 8.0, 9.0, 12.0, 13.0, 14.0];
1196        assert_eq!(
1197            &result[..],
1198            &expected[..],
1199            "\nExpected\n{:?}\nfound\n{:?}",
1200            &result[..],
1201            &expected[..]
1202        );
1203    }
1204
1205    #[test]
1206    fn for_each_test() {
1207        let m = m44_new!( 0.0,  1.0,  2.0,  3.0;
1208                          4.0,  5.0,  6.0,  7.0;
1209                          8.0,  9.0, 10.0, 11.0;
1210                         12.0, 13.0, 14.0, 15.0);
1211        let result = m.for_each(|element| element + 1.0);
1212        let expected = m44_new!( 1.0,  2.0,  3.0,  4.0;
1213                                 5.0,  6.0,  7.0,  8.0;
1214                                 9.0,  10.0, 11.0, 12.0;
1215                                13.0, 14.0, 15.0, 16.0);
1216        assert!(compare_vecs(&result.as_vec(), &expected.as_vec(), EPS));
1217    }
1218}
static_math/matrix4x4.rs

static_math/
matrix4x4.rs
