rs_math3d/
matrix.rs

1// Copyright 2020-Present (c) Raja Lehtihet & Wael El Oraiby
2//
3// Redistribution and use in source and binary forms, with or without
4// modification, are permitted provided that the following conditions are met:
5//
6// 1. Redistributions of source code must retain the above copyright notice,
7// this list of conditions and the following disclaimer.
8//
9// 2. Redistributions in binary form must reproduce the above copyright notice,
10// this list of conditions and the following disclaimer in the documentation
11// and/or other materials provided with the distribution.
12//
13// 3. Neither the name of the copyright holder nor the names of its contributors
14// may be used to endorse or promote products derived from this software without
15// specific prior written permission.
16//
17// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27// POSSIBILITY OF SUCH DAMAGE.
28use crate::scalar::*;
29use crate::vector::*;
30use core::ops::*;
31
32#[repr(C)]
33#[derive(Clone, Copy, Debug)]
34pub struct Matrix2<T: Scalar> {
35    pub col: [Vector2<T>; 2],
36}
37
38#[repr(C)]
39#[derive(Clone, Copy, Debug)]
40pub struct Matrix3<T: Scalar> {
41    pub col: [Vector3<T>; 3],
42}
43
44#[repr(C)]
45#[derive(Clone, Copy, Debug)]
46pub struct Matrix4<T: Scalar> {
47    pub col: [Vector4<T>; 4],
48}
49
50/******************************************************************************
51 * Matrix2
52 *
53 * i j ------------------->
54 * | [m0 = c0_x | m2 = c1_x]
55 * V [m1 = c0_y | m3 = c1_y]
56 *
57 *  aij => i = row, j = col (yx form)
58 *
59 *****************************************************************************/
60impl<T: Scalar> Matrix2<T> {
61    pub fn new(m0: T, m1: T, m2: T, m3: T) -> Self {
62        Matrix2 {
63            col: [Vector2::new(m0, m1), Vector2::new(m2, m3)],
64        }
65    }
66
67    pub fn identity() -> Self {
68        Self::new(T::one(), T::zero(), T::zero(), T::one())
69    }
70
71    pub fn determinant(&self) -> T {
72        let m00 = self.col[0].x;
73        let m10 = self.col[0].y;
74
75        let m01 = self.col[1].x;
76        let m11 = self.col[1].y;
77
78        m00 * m11 - m01 * m10
79    }
80
81    pub fn transpose(&self) -> Self {
82        let m00 = self.col[0].x;
83        let m10 = self.col[0].y;
84
85        let m01 = self.col[1].x;
86        let m11 = self.col[1].y;
87
88        Self::new(m00, m01, m10, m11)
89    }
90
91    pub fn inverse(&self) -> Self {
92        let m00 = self.col[0].x;
93        let m10 = self.col[0].y;
94
95        let m01 = self.col[1].x;
96        let m11 = self.col[1].y;
97
98        let inv_det = T::one() / (m00 * m11 - m01 * m10);
99
100        let r00 = m11 * inv_det;
101        let r01 = -m01 * inv_det;
102        let r10 = -m10 * inv_det;
103        let r11 = m00 * inv_det;
104
105        Self::new(r00, r10, r01, r11)
106    }
107
108    //
109    //             [b00 b01]
110    //             [b10 b11]
111    //           *   |   |
112    // [a00 a01] - [c00 c01]
113    // [a10 a11] - [c10 c11]
114    //
115    pub fn mul_matrix_matrix(l: &Self, r: &Self) -> Self {
116        let a00 = l.col[0].x;
117        let a10 = l.col[0].y;
118        let a01 = l.col[1].x;
119        let a11 = l.col[1].y;
120
121        let b00 = r.col[0].x;
122        let b10 = r.col[0].y;
123        let b01 = r.col[1].x;
124        let b11 = r.col[1].y;
125
126        let c00 = a00 * b00 + a01 * b10;
127        let c01 = a00 * b01 + a01 * b11;
128        let c10 = a10 * b00 + a11 * b10;
129        let c11 = a10 * b01 + a11 * b11;
130
131        Self::new(c00, c10, c01, c11)
132    }
133
134    pub fn mul_matrix_vector(l: &Self, r: &Vector2<T>) -> Vector2<T> {
135        Self::mul_vector_matrix(r, &l.transpose())
136    }
137
138    pub fn mul_vector_matrix(l: &Vector2<T>, r: &Self) -> Vector2<T> {
139        Vector2::new(Vector2::dot(l, &r.col[0]), Vector2::dot(l, &r.col[1]))
140    }
141
142    pub fn add_matrix_matrix(l: &Self, r: &Self) -> Self {
143        Matrix2 {
144            col: [l.col[0] + r.col[0], l.col[1] + r.col[1]],
145        }
146    }
147
148    pub fn sub_matrix_matrix(l: &Self, r: &Self) -> Self {
149        Matrix2 {
150            col: [l.col[0] - r.col[0], l.col[1] - r.col[1]],
151        }
152    }
153}
154
155/******************************************************************************
156 * Matrix3
157 *
158 * i j -------------------------------->
159 * | [m0 = c0_x | m3 = c1_x | m6 = c2_x]
160 * | [m1 = c0_y | m4 = c1_y | m7 = c2_y]
161 * V [m2 = c0_z | m5 = c1_z | m8 = c2_z]
162 *
163 *  aij => i = row, j = col (yx form)
164 *
165 *****************************************************************************/
166impl<T: Scalar> Matrix3<T> {
167    pub fn new(m0: T, m1: T, m2: T, m3: T, m4: T, m5: T, m6: T, m7: T, m8: T) -> Self {
168        Matrix3 {
169            col: [
170                Vector3::new(m0, m1, m2),
171                Vector3::new(m3, m4, m5),
172                Vector3::new(m6, m7, m8),
173            ],
174        }
175    }
176
177    pub fn identity() -> Self {
178        Self::new(
179            T::one(),
180            T::zero(),
181            T::zero(),
182            T::zero(),
183            T::one(),
184            T::zero(),
185            T::zero(),
186            T::zero(),
187            T::one(),
188        )
189    }
190
191    pub fn determinant(&self) -> T {
192        let m00 = self.col[0].x;
193        let m10 = self.col[0].y;
194        let m20 = self.col[0].z;
195
196        let m01 = self.col[1].x;
197        let m11 = self.col[1].y;
198        let m21 = self.col[1].z;
199
200        let m02 = self.col[2].x;
201        let m12 = self.col[2].y;
202        let m22 = self.col[2].z;
203
204        m00 * m11 * m22 + m01 * m12 * m20 + m02 * m10 * m21
205            - m00 * m12 * m21
206            - m01 * m10 * m22
207            - m02 * m11 * m20
208    }
209
210    pub fn transpose(&self) -> Self {
211        let m00 = self.col[0].x;
212        let m10 = self.col[0].y;
213        let m20 = self.col[0].z;
214
215        let m01 = self.col[1].x;
216        let m11 = self.col[1].y;
217        let m21 = self.col[1].z;
218
219        let m02 = self.col[2].x;
220        let m12 = self.col[2].y;
221        let m22 = self.col[2].z;
222
223        Self::new(m00, m01, m02, m10, m11, m12, m20, m21, m22)
224    }
225
226    pub fn inverse(&self) -> Self {
227        let m00 = self.col[0].x;
228        let m10 = self.col[0].y;
229        let m20 = self.col[0].z;
230
231        let m01 = self.col[1].x;
232        let m11 = self.col[1].y;
233        let m21 = self.col[1].z;
234
235        let m02 = self.col[2].x;
236        let m12 = self.col[2].y;
237        let m22 = self.col[2].z;
238
239        let inv_det = T::one()
240            / (m00 * m11 * m22 + m01 * m12 * m20 + m02 * m10 * m21
241                - m00 * m12 * m21
242                - m01 * m10 * m22
243                - m02 * m11 * m20);
244
245        let r00 = (m11 * m22 - m12 * m21) * inv_det;
246        let r01 = (m02 * m21 - m01 * m22) * inv_det;
247        let r02 = (m01 * m12 - m02 * m11) * inv_det;
248        let r10 = (m12 * m20 - m10 * m22) * inv_det;
249        let r11 = (m00 * m22 - m02 * m20) * inv_det;
250        let r12 = (m02 * m10 - m00 * m12) * inv_det;
251        let r20 = (m10 * m21 - m11 * m20) * inv_det;
252        let r21 = (m01 * m20 - m00 * m21) * inv_det;
253        let r22 = (m00 * m11 - m01 * m10) * inv_det;
254
255        Self::new(r00, r10, r20, r01, r11, r21, r02, r12, r22)
256    }
257
258    //
259    //                 [b00 b01 b02]
260    //                 [b10 b11 b12]
261    //                 [b20 b21 b22]
262    //               *   |   |   |
263    // [a00 a01 a02] - [c00 c01 c02]
264    // [a10 a11 a12] - [c10 c11 c12]
265    // [a20 a21 a22] - [c10 c11 c22]
266    //
267    pub fn mul_matrix_matrix(l: &Self, r: &Self) -> Self {
268        let a00 = l.col[0].x;
269        let a10 = l.col[0].y;
270        let a20 = l.col[0].z;
271
272        let a01 = l.col[1].x;
273        let a11 = l.col[1].y;
274        let a21 = l.col[1].z;
275
276        let a02 = l.col[2].x;
277        let a12 = l.col[2].y;
278        let a22 = l.col[2].z;
279
280        let b00 = r.col[0].x;
281        let b10 = r.col[0].y;
282        let b20 = r.col[0].z;
283
284        let b01 = r.col[1].x;
285        let b11 = r.col[1].y;
286        let b21 = r.col[1].z;
287
288        let b02 = r.col[2].x;
289        let b12 = r.col[2].y;
290        let b22 = r.col[2].z;
291
292        let c00 = a00 * b00 + a01 * b10 + a02 * b20;
293        let c01 = a00 * b01 + a01 * b11 + a02 * b21;
294        let c02 = a00 * b02 + a01 * b12 + a02 * b22;
295
296        let c10 = a10 * b00 + a11 * b10 + a12 * b20;
297        let c11 = a10 * b01 + a11 * b11 + a12 * b21;
298        let c12 = a10 * b02 + a11 * b12 + a12 * b22;
299
300        let c20 = a20 * b00 + a21 * b10 + a22 * b20;
301        let c21 = a20 * b01 + a21 * b11 + a22 * b21;
302        let c22 = a20 * b02 + a21 * b12 + a22 * b22;
303
304        Self::new(c00, c10, c20, c01, c11, c21, c02, c12, c22)
305    }
306
307    pub fn mul_matrix_vector(l: &Self, r: &Vector3<T>) -> Vector3<T> {
308        Self::mul_vector_matrix(r, &l.transpose())
309    }
310
311    pub fn mul_vector_matrix(l: &Vector3<T>, r: &Self) -> Vector3<T> {
312        Vector3::new(
313            Vector3::dot(l, &r.col[0]),
314            Vector3::dot(l, &r.col[1]),
315            Vector3::dot(l, &r.col[2]),
316        )
317    }
318
319    pub fn add_matrix_matrix(l: &Self, r: &Self) -> Self {
320        Matrix3 {
321            col: [
322                l.col[0] + r.col[0],
323                l.col[1] + r.col[1],
324                l.col[2] + r.col[2],
325            ],
326        }
327    }
328
329    pub fn sub_matrix_matrix(l: &Self, r: &Self) -> Self {
330        Matrix3 {
331            col: [
332                l.col[0] - r.col[0],
333                l.col[1] - r.col[1],
334                l.col[2] - r.col[2],
335            ],
336        }
337    }
338}
339
340impl<T: FloatScalar> Matrix3<T> {
341    pub fn of_axis_angle(axis: &Vector3<T>, angle: T) -> Self {
342        let c = T::tcos(angle);
343        let s = T::tsin(angle);
344        let n = Vector3::normalize(axis);
345        let ux = n.x;
346        let uy = n.y;
347        let uz = n.z;
348        let uxx = ux * ux;
349        let uyy = uy * uy;
350        let uzz = uz * uz;
351
352        let oc = T::one() - c;
353
354        let m0 = c + uxx * oc;
355        let m1 = uy * ux * oc + uz * s;
356        let m2 = uz * ux * oc - uy * s;
357
358        let m3 = ux * uy * oc - uz * s;
359        let m4 = c + uyy * oc;
360        let m5 = uz * uy * oc + ux * s;
361
362        let m6 = ux * uz * oc + uy * s;
363        let m7 = uy * uz * oc - ux * s;
364        let m8 = c + uzz * oc;
365
366        Self::new(m0, m1, m2, m3, m4, m5, m6, m7, m8)
367    }
368}
369
370/******************************************************************************
371 * Matrix4
372 *
373 * i j -------------------------------------------->
374 * | [m0 = c0_x | m4 = c1_x | m8 = c2_x | m12= c3_x]
375 * | [m1 = c0_y | m5 = c1_y | m9 = c2_y | m13= c3_y]
376 * | [m2 = c0_z | m6 = c1_z | m10= c2_z | m14= c3_z]
377 * V [m3 = c0_w | m7 = c1_w | m11= c2_w | m15= c3_w]
378 *
379 *  aij => i = row, j = col (yx form)
380 *
381 *****************************************************************************/
382impl<T: Scalar> Matrix4<T> {
383    pub fn new(
384        m0: T,
385        m1: T,
386        m2: T,
387        m3: T,
388        m4: T,
389        m5: T,
390        m6: T,
391        m7: T,
392        m8: T,
393        m9: T,
394        m10: T,
395        m11: T,
396        m12: T,
397        m13: T,
398        m14: T,
399        m15: T,
400    ) -> Self {
401        Matrix4 {
402            col: [
403                Vector4::new(m0, m1, m2, m3),
404                Vector4::new(m4, m5, m6, m7),
405                Vector4::new(m8, m9, m10, m11),
406                Vector4::new(m12, m13, m14, m15),
407            ],
408        }
409    }
410
411    pub fn identity() -> Self {
412        Self::new(
413            T::one(),
414            T::zero(),
415            T::zero(),
416            T::zero(),
417            T::zero(),
418            T::one(),
419            T::zero(),
420            T::zero(),
421            T::zero(),
422            T::zero(),
423            T::one(),
424            T::zero(),
425            T::zero(),
426            T::zero(),
427            T::zero(),
428            T::one(),
429        )
430    }
431
432    pub fn determinant(&self) -> T {
433        let m00 = self.col[0].x;
434        let m10 = self.col[0].y;
435        let m20 = self.col[0].z;
436        let m30 = self.col[0].w;
437
438        let m01 = self.col[1].x;
439        let m11 = self.col[1].y;
440        let m21 = self.col[1].z;
441        let m31 = self.col[1].w;
442
443        let m02 = self.col[2].x;
444        let m12 = self.col[2].y;
445        let m22 = self.col[2].z;
446        let m32 = self.col[2].w;
447
448        let m03 = self.col[3].x;
449        let m13 = self.col[3].y;
450        let m23 = self.col[3].z;
451        let m33 = self.col[3].w;
452
453        m03 * m12 * m21 * m30 - m02 * m13 * m21 * m30 - m03 * m11 * m22 * m30
454            + m01 * m13 * m22 * m30
455            + m02 * m11 * m23 * m30
456            - m01 * m12 * m23 * m30
457            - m03 * m12 * m20 * m31
458            + m02 * m13 * m20 * m31
459            + m03 * m10 * m22 * m31
460            - m00 * m13 * m22 * m31
461            - m02 * m10 * m23 * m31
462            + m00 * m12 * m23 * m31
463            + m03 * m11 * m20 * m32
464            - m01 * m13 * m20 * m32
465            - m03 * m10 * m21 * m32
466            + m00 * m13 * m21 * m32
467            + m01 * m10 * m23 * m32
468            - m00 * m11 * m23 * m32
469            - m02 * m11 * m20 * m33
470            + m01 * m12 * m20 * m33
471            + m02 * m10 * m21 * m33
472            - m00 * m12 * m21 * m33
473            - m01 * m10 * m22 * m33
474            + m00 * m11 * m22 * m33
475    }
476
477    pub fn transpose(&self) -> Self {
478        let m00 = self.col[0].x;
479        let m10 = self.col[0].y;
480        let m20 = self.col[0].z;
481        let m30 = self.col[0].w;
482
483        let m01 = self.col[1].x;
484        let m11 = self.col[1].y;
485        let m21 = self.col[1].z;
486        let m31 = self.col[1].w;
487
488        let m02 = self.col[2].x;
489        let m12 = self.col[2].y;
490        let m22 = self.col[2].z;
491        let m32 = self.col[2].w;
492
493        let m03 = self.col[3].x;
494        let m13 = self.col[3].y;
495        let m23 = self.col[3].z;
496        let m33 = self.col[3].w;
497
498        Self::new(
499            m00, m01, m02, m03, m10, m11, m12, m13, m20, m21, m22, m23, m30, m31, m32, m33,
500        )
501    }
502
503    pub fn inverse(&self) -> Self {
504        let m00 = self.col[0].x;
505        let m10 = self.col[0].y;
506        let m20 = self.col[0].z;
507        let m30 = self.col[0].w;
508
509        let m01 = self.col[1].x;
510        let m11 = self.col[1].y;
511        let m21 = self.col[1].z;
512        let m31 = self.col[1].w;
513
514        let m02 = self.col[2].x;
515        let m12 = self.col[2].y;
516        let m22 = self.col[2].z;
517        let m32 = self.col[2].w;
518
519        let m03 = self.col[3].x;
520        let m13 = self.col[3].y;
521        let m23 = self.col[3].z;
522        let m33 = self.col[3].w;
523
524        let denom = m03 * m12 * m21 * m30 - m02 * m13 * m21 * m30 - m03 * m11 * m22 * m30
525            + m01 * m13 * m22 * m30
526            + m02 * m11 * m23 * m30
527            - m01 * m12 * m23 * m30
528            - m03 * m12 * m20 * m31
529            + m02 * m13 * m20 * m31
530            + m03 * m10 * m22 * m31
531            - m00 * m13 * m22 * m31
532            - m02 * m10 * m23 * m31
533            + m00 * m12 * m23 * m31
534            + m03 * m11 * m20 * m32
535            - m01 * m13 * m20 * m32
536            - m03 * m10 * m21 * m32
537            + m00 * m13 * m21 * m32
538            + m01 * m10 * m23 * m32
539            - m00 * m11 * m23 * m32
540            - m02 * m11 * m20 * m33
541            + m01 * m12 * m20 * m33
542            + m02 * m10 * m21 * m33
543            - m00 * m12 * m21 * m33
544            - m01 * m10 * m22 * m33
545            + m00 * m11 * m22 * m33;
546        let inv_det = T::one() / denom;
547
548        let r00 = (m12 * m23 * m31 - m13 * m22 * m31 + m13 * m21 * m32
549            - m11 * m23 * m32
550            - m12 * m21 * m33
551            + m11 * m22 * m33)
552            * inv_det;
553
554        let r01 = (m03 * m22 * m31 - m02 * m23 * m31 - m03 * m21 * m32
555            + m01 * m23 * m32
556            + m02 * m21 * m33
557            - m01 * m22 * m33)
558            * inv_det;
559
560        let r02 = (m02 * m13 * m31 - m03 * m12 * m31 + m03 * m11 * m32
561            - m01 * m13 * m32
562            - m02 * m11 * m33
563            + m01 * m12 * m33)
564            * inv_det;
565
566        let r03 = (m03 * m12 * m21 - m02 * m13 * m21 - m03 * m11 * m22
567            + m01 * m13 * m22
568            + m02 * m11 * m23
569            - m01 * m12 * m23)
570            * inv_det;
571
572        let r10 = (m13 * m22 * m30 - m12 * m23 * m30 - m13 * m20 * m32
573            + m10 * m23 * m32
574            + m12 * m20 * m33
575            - m10 * m22 * m33)
576            * inv_det;
577
578        let r11 = (m02 * m23 * m30 - m03 * m22 * m30 + m03 * m20 * m32
579            - m00 * m23 * m32
580            - m02 * m20 * m33
581            + m00 * m22 * m33)
582            * inv_det;
583
584        let r12 = (m03 * m12 * m30 - m02 * m13 * m30 - m03 * m10 * m32
585            + m00 * m13 * m32
586            + m02 * m10 * m33
587            - m00 * m12 * m33)
588            * inv_det;
589
590        let r13 = (m02 * m13 * m20 - m03 * m12 * m20 + m03 * m10 * m22
591            - m00 * m13 * m22
592            - m02 * m10 * m23
593            + m00 * m12 * m23)
594            * inv_det;
595
596        let r20 = (m11 * m23 * m30 - m13 * m21 * m30 + m13 * m20 * m31
597            - m10 * m23 * m31
598            - m11 * m20 * m33
599            + m10 * m21 * m33)
600            * inv_det;
601
602        let r21 = (m03 * m21 * m30 - m01 * m23 * m30 - m03 * m20 * m31
603            + m00 * m23 * m31
604            + m01 * m20 * m33
605            - m00 * m21 * m33)
606            * inv_det;
607
608        let r22 = (m01 * m13 * m30 - m03 * m11 * m30 + m03 * m10 * m31
609            - m00 * m13 * m31
610            - m01 * m10 * m33
611            + m00 * m11 * m33)
612            * inv_det;
613
614        let r23 = (m03 * m11 * m20 - m01 * m13 * m20 - m03 * m10 * m21
615            + m00 * m13 * m21
616            + m01 * m10 * m23
617            - m00 * m11 * m23)
618            * inv_det;
619
620        let r30 = (m12 * m21 * m30 - m11 * m22 * m30 - m12 * m20 * m31
621            + m10 * m22 * m31
622            + m11 * m20 * m32
623            - m10 * m21 * m32)
624            * inv_det;
625
626        let r31 = (m01 * m22 * m30 - m02 * m21 * m30 + m02 * m20 * m31
627            - m00 * m22 * m31
628            - m01 * m20 * m32
629            + m00 * m21 * m32)
630            * inv_det;
631
632        let r32 = (m02 * m11 * m30 - m01 * m12 * m30 - m02 * m10 * m31
633            + m00 * m12 * m31
634            + m01 * m10 * m32
635            - m00 * m11 * m32)
636            * inv_det;
637
638        let r33 = (m01 * m12 * m20 - m02 * m11 * m20 + m02 * m10 * m21
639            - m00 * m12 * m21
640            - m01 * m10 * m22
641            + m00 * m11 * m22)
642            * inv_det;
643
644        Self::new(
645            r00, r10, r20, r30, r01, r11, r21, r31, r02, r12, r22, r32, r03, r13, r23, r33,
646        )
647    }
648
649    //
650    //                     [b00 b01 b02 b03]
651    //                     [b10 b11 b12 b13]
652    //                     [b20 b21 b22 b23]
653    //                     [b20 b21 b22 b33]
654    //                   *   |   |   |   |
655    // [a00 a01 a02 a03] - [c00 c01 c02 c03]
656    // [a10 a11 a12 a13] - [c10 c11 c12 c13]
657    // [a20 a21 a22 a23] - [c10 c11 c22 c23]
658    // [a20 a21 a22 a33] - [c10 c11 c22 c33]
659    //
660    pub fn mul_matrix_matrix(l: &Self, r: &Self) -> Self {
661        let a00 = l.col[0].x;
662        let a10 = l.col[0].y;
663        let a20 = l.col[0].z;
664        let a30 = l.col[0].w;
665
666        let a01 = l.col[1].x;
667        let a11 = l.col[1].y;
668        let a21 = l.col[1].z;
669        let a31 = l.col[1].w;
670
671        let a02 = l.col[2].x;
672        let a12 = l.col[2].y;
673        let a22 = l.col[2].z;
674        let a32 = l.col[2].w;
675
676        let a03 = l.col[3].x;
677        let a13 = l.col[3].y;
678        let a23 = l.col[3].z;
679        let a33 = l.col[3].w;
680
681        let b00 = r.col[0].x;
682        let b10 = r.col[0].y;
683        let b20 = r.col[0].z;
684        let b30 = r.col[0].w;
685
686        let b01 = r.col[1].x;
687        let b11 = r.col[1].y;
688        let b21 = r.col[1].z;
689        let b31 = r.col[1].w;
690
691        let b02 = r.col[2].x;
692        let b12 = r.col[2].y;
693        let b22 = r.col[2].z;
694        let b32 = r.col[2].w;
695
696        let b03 = r.col[3].x;
697        let b13 = r.col[3].y;
698        let b23 = r.col[3].z;
699        let b33 = r.col[3].w;
700
701        let c00 = a00 * b00 + a01 * b10 + a02 * b20 + a03 * b30;
702        let c01 = a00 * b01 + a01 * b11 + a02 * b21 + a03 * b31;
703        let c02 = a00 * b02 + a01 * b12 + a02 * b22 + a03 * b32;
704        let c03 = a00 * b03 + a01 * b13 + a02 * b23 + a03 * b33;
705
706        let c10 = a10 * b00 + a11 * b10 + a12 * b20 + a13 * b30;
707        let c11 = a10 * b01 + a11 * b11 + a12 * b21 + a13 * b31;
708        let c12 = a10 * b02 + a11 * b12 + a12 * b22 + a13 * b32;
709        let c13 = a10 * b03 + a11 * b13 + a12 * b23 + a13 * b33;
710
711        let c20 = a20 * b00 + a21 * b10 + a22 * b20 + a23 * b30;
712        let c21 = a20 * b01 + a21 * b11 + a22 * b21 + a23 * b31;
713        let c22 = a20 * b02 + a21 * b12 + a22 * b22 + a23 * b32;
714        let c23 = a20 * b03 + a21 * b13 + a22 * b23 + a23 * b33;
715
716        let c30 = a30 * b00 + a31 * b10 + a32 * b20 + a33 * b30;
717        let c31 = a30 * b01 + a31 * b11 + a32 * b21 + a33 * b31;
718        let c32 = a30 * b02 + a31 * b12 + a32 * b22 + a33 * b32;
719        let c33 = a30 * b03 + a31 * b13 + a32 * b23 + a33 * b33;
720
721        Self::new(
722            c00, c10, c20, c30, c01, c11, c21, c31, c02, c12, c22, c32, c03, c13, c23, c33,
723        )
724    }
725
726    pub fn mul_matrix_vector(l: &Self, r: &Vector4<T>) -> Vector4<T> {
727        Self::mul_vector_matrix(r, &l.transpose())
728    }
729
730    //
731    //                     [m0 = c0_x | m4 = c1_x | m8 = c2_x | m12= c3_x]
732    // [v_x v_y v_z v_w] * [m1 = c0_y | m5 = c1_y | m9 = c2_y | m13= c3_y] = [dot(v, c0) dot(v, c1) dot(v, c2) dot(v, c3)]
733    //                     [m2 = c0_z | m6 = c1_z | m10= c2_z | m14= c3_z]
734    //                     [m3 = c0_w | m7 = c1_w | m11= c2_w | m15= c3_w]
735    //
736    pub fn mul_vector_matrix(l: &Vector4<T>, r: &Self) -> Vector4<T> {
737        Vector4::new(
738            Vector4::dot(l, &r.col[0]),
739            Vector4::dot(l, &r.col[1]),
740            Vector4::dot(l, &r.col[2]),
741            Vector4::dot(l, &r.col[3]),
742        )
743    }
744
745    pub fn add_matrix_matrix(l: &Self, r: &Self) -> Self {
746        Matrix4 {
747            col: [
748                l.col[0] + r.col[0],
749                l.col[1] + r.col[1],
750                l.col[2] + r.col[2],
751                l.col[3] + r.col[3],
752            ],
753        }
754    }
755
756    pub fn sub_matrix_matrix(l: &Self, r: &Self) -> Self {
757        Matrix4 {
758            col: [
759                l.col[0] - r.col[0],
760                l.col[1] - r.col[1],
761                l.col[2] - r.col[2],
762                l.col[3] - r.col[3],
763            ],
764        }
765    }
766}
767
768/******************************************************************************
769 * Operator overloading
770 *****************************************************************************/
771macro_rules! implMatrixOps {
772    ($mat:ident, $vec: ident) => {
773        impl<T: Scalar> Mul<$mat<T>> for $vec<T> {
774            type Output = $vec<T>;
775            fn mul(self, rhs: $mat<T>) -> $vec<T> {
776                $mat::mul_vector_matrix(&self, &rhs)
777            }
778        }
779
780        impl<T: Scalar> Mul<$vec<T>> for $mat<T> {
781            type Output = $vec<T>;
782            fn mul(self, rhs: $vec<T>) -> $vec<T> {
783                $mat::mul_matrix_vector(&self, &rhs)
784            }
785        }
786
787        impl<T: Scalar> Mul<$mat<T>> for $mat<T> {
788            type Output = $mat<T>;
789            fn mul(self, rhs: $mat<T>) -> $mat<T> {
790                $mat::mul_matrix_matrix(&self, &rhs)
791            }
792        }
793
794        impl<T: Scalar> Add<$mat<T>> for $mat<T> {
795            type Output = $mat<T>;
796            fn add(self, rhs: $mat<T>) -> $mat<T> {
797                $mat::add_matrix_matrix(&self, &rhs)
798            }
799        }
800
801        impl<T: Scalar> Sub<$mat<T>> for $mat<T> {
802            type Output = $mat<T>;
803            fn sub(self, rhs: $mat<T>) -> $mat<T> {
804                $mat::sub_matrix_matrix(&self, &rhs)
805            }
806        }
807    };
808}
809
810implMatrixOps!(Matrix2, Vector2);
811implMatrixOps!(Matrix3, Vector3);
812implMatrixOps!(Matrix4, Vector4);
813
814impl<T: Scalar> Mul<Matrix4<T>> for Vector3<T> {
815    type Output = Vector3<T>;
816    fn mul(self, rhs: Matrix4<T>) -> Vector3<T> {
817        Matrix4::mul_vector_matrix(&Vector4::new(self.x, self.y, self.z, T::one()), &rhs).xyz()
818    }
819}
820
821impl<T: Scalar> Mul<Vector3<T>> for Matrix4<T> {
822    type Output = Vector3<T>;
823    fn mul(self, rhs: Vector3<T>) -> Vector3<T> {
824        Matrix4::mul_matrix_vector(&self, &Vector4::new(rhs.x, rhs.y, rhs.z, T::one())).xyz()
825    }
826}
827
828pub trait Matrix4Extension<T: Scalar> {
829    fn mat3(&self) -> Matrix3<T>;
830}
831
832impl<T: Scalar> Matrix4Extension<T> for Matrix4<T> {
833    fn mat3(&self) -> Matrix3<T> {
834        Matrix3::new(
835            self.col[0].x,
836            self.col[0].y,
837            self.col[0].z,
838            self.col[1].x,
839            self.col[1].y,
840            self.col[1].z,
841            self.col[2].x,
842            self.col[2].y,
843            self.col[2].z,
844        )
845    }
846}
rs_math3d/matrix.rs

rs_math3d/
matrix.rs
