glam_matrix_extras/symmetric/
symmetric_mat5.rs

1use core::iter::Sum;
2use core::ops::*;
3#[cfg(feature = "f64")]
4use glam::{DVec2, DVec3};
5use glam::{Vec2, Vec3};
6
7#[cfg(feature = "bevy_reflect")]
8use bevy_reflect::{Reflect, ReflectDeserialize, ReflectSerialize, std_traits::ReflectDefault};
9
10#[cfg(feature = "f64")]
11use crate::{DMat23, SymmetricDMat2, SymmetricDMat3};
12#[cfg(feature = "f32")]
13use crate::{Mat23, SymmetricMat2, SymmetricMat3};
14
15macro_rules! symmetric_mat5s {
16    ($($n:ident => $symmetricm2t:ident, $symmetricm3t:ident, $m23t:ident, $v2t:ident, $v3t:ident, $t:ident),+) => {
17        $(
18        /// The bottom left triangle (including the diagonal) of a symmetric 5x5 column-major matrix.
19        ///
20        /// This is useful for storing a symmetric 5x5 matrix in a more compact form and performing some
21        /// matrix operations more efficiently.
22        ///
23        /// Some defining properties of symmetric matrices include:
24        ///
25        /// - The matrix is equal to its transpose.
26        /// - The matrix has real eigenvalues.
27        /// - The eigenvectors corresponding to the eigenvalues are orthogonal.
28        /// - The matrix is always diagonalizable.
29        ///
30        /// The sum and difference of two symmetric matrices is always symmetric.
31        /// However, the product of two symmetric matrices is *only* symmetric
32        /// if the matrices are commutable, meaning that `AB = BA`.
33        ///
34        /// The 5x5 matrix is represented as:
35        ///
36        /// ```text
37        /// [ A  BT ]
38        /// [ B  D  ]
39        /// ```
40        #[derive(Clone, Copy, PartialEq)]
41        #[cfg_attr(feature = "bevy_reflect", derive(Reflect))]
42        #[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
43        #[cfg_attr(
44            all(feature = "bevy_reflect", feature = "serde"),
45            reflect(Debug, Default, PartialEq, Serialize, Deserialize)
46        )]
47        pub struct $n {
48            /// The bottom left triangle of the top left 3x3 block of the matrix,
49            /// including the diagonal.
50            pub a: $symmetricm3t,
51            /// The bottom left 2x3 block of the matrix.
52            pub b: $m23t,
53            /// The bottom left triangle of the bottom right 2x2 block of the matrix,
54            /// including the diagonal.
55            pub d: $symmetricm2t,
56        }
57
58        impl $n {
59            /// A symmetric 5x5 matrix with all elements set to `0.0`.
60            pub const ZERO: Self = Self::new(
61                $symmetricm3t::ZERO,
62                $m23t::ZERO,
63                $symmetricm2t::ZERO,
64            );
65
66            /// A symmetric 5x5 identity matrix, where all diagonal elements are `1.0`,
67            /// and all off-diagonal elements are `0.0`.
68            pub const IDENTITY: Self = Self::new(
69                $symmetricm3t::IDENTITY,
70                $m23t::ZERO,
71                $symmetricm2t::IDENTITY,
72            );
73
74            /// All NaNs.
75            pub const NAN: Self = Self::new(
76                $symmetricm3t::NAN,
77                $m23t::NAN,
78                $symmetricm2t::NAN,
79            );
80
81            /// Creates a new symmetric 5x5 matrix from its bottom left triangle, including diagonal elements.
82            ///
83            /// The matrix is represented as:
84            ///
85            /// ```text
86            /// [ A  BT ]
87            /// [ B  D  ]
88            /// ```
89            #[inline(always)]
90            #[must_use]
91            pub const fn new(
92                a: $symmetricm3t,
93                b: $m23t,
94                d: $symmetricm2t,
95            ) -> Self {
96                Self { a, b, d }
97            }
98
99            /// Creates a new symmetric 5x5 matrix from the outer product `[v1, v2] * [v1, v2]^T`.
100            #[inline]
101            #[must_use]
102            pub fn from_outer_product(v1: $v3t, v2: $v2t) -> Self {
103                Self::new(
104                    $symmetricm3t::from_outer_product(v1),
105                    $m23t::from_outer_product(v2, v1),
106                    $symmetricm2t::from_outer_product(v2),
107                )
108            }
109
110            /// Returns `true` if, and only if, all elements are finite.
111            /// If any element is either `NaN` or positive or negative infinity, this will return `false`.
112            #[inline]
113            #[must_use]
114            pub fn is_finite(&self) -> bool {
115                self.a.is_finite() && self.b.is_finite() && self.d.is_finite()
116            }
117
118            /// Returns `true` if any elements are `NaN`.
119            #[inline]
120            #[must_use]
121            pub fn is_nan(&self) -> bool {
122                self.a.is_nan() || self.b.is_nan() || self.d.is_nan()
123            }
124
125            /// Returns the inverse of `self`.
126            ///
127            /// If the matrix is not invertible the returned matrix will be invalid.
128            #[inline]
129            #[must_use]
130            pub fn inverse(&self) -> Self {
131                let inv_d = self.d.inverse();
132                let bt_inv_d = inv_d.mul(self.b);
133                let bt_inv_d_b = $symmetricm3t::complete_mat23_sandwich(&bt_inv_d, &self.b);
134
135                let res_a = self.a.sub(bt_inv_d_b).inverse();
136                let neg_res_bt = bt_inv_d.mul(res_a);
137                let res_d = $symmetricm2t::complete_mat23_sandwich(&bt_inv_d, &neg_res_bt).add(inv_d);
138
139                Self::new(res_a, -neg_res_bt, res_d)
140            }
141
142            /// Returns the inverse of `self`, or a zero matrix if the matrix is not invertible.
143            #[inline]
144            #[must_use]
145            pub fn inverse_or_zero(&self) -> Self {
146                // TODO: Optimize this.
147                let inverse = self.inverse();
148                if inverse.is_finite() {
149                    inverse
150                } else {
151                    Self::ZERO
152                }
153            }
154
155            /// Takes the absolute value of each element in `self`.
156            #[inline]
157            #[must_use]
158            pub fn abs(&self) -> Self {
159                Self::new(self.a.abs(), self.b.abs(), self.d.abs())
160            }
161
162            /// Transforms a 5x1 vector that is split into a 3x1 vector and 2x1 vector.
163            #[inline]
164            #[must_use]
165            pub fn mul_vec5(&self, rhs1: $v3t, rhs2: $v2t) -> ($v3t, $v2t) {
166                let res1 = $v3t::new(
167                    rhs1.x * self.a.m00 + rhs1.y * self.a.m01 + rhs1.z * self.a.m02 + rhs2.dot(self.b.col(0)),
168                    rhs1.x * self.a.m01 + rhs1.y * self.a.m11 + rhs1.z * self.a.m12 + rhs2.dot(self.b.col(1)),
169                    rhs1.x * self.a.m02 + rhs1.y * self.a.m12 + rhs1.z * self.a.m22 + rhs2.dot(self.b.col(2)),
170                );
171                let res2 = $v2t::new(
172                    rhs1.dot(self.b.row(0)) + rhs2.x * self.d.m00 + rhs2.y * self.d.m01,
173                    rhs1.dot(self.b.row(1)) + rhs2.x * self.d.m01 + rhs2.y * self.d.m11,
174                );
175                (res1, res2)
176            }
177
178            /// Adds two symmetric 5x5 matrices.
179            #[inline]
180            #[must_use]
181            pub fn add_symmetric_mat5(&self, rhs: &Self) -> Self {
182                self.add(rhs)
183            }
184
185            /// Subtracts two symmetric 5x5 matrices.
186            #[inline]
187            #[must_use]
188            pub fn sub_symmetric_mat5(&self, rhs: &Self) -> Self {
189                self.sub(rhs)
190            }
191
192            /// Multiplies a 5x5 matrix by a scalar.
193            #[inline]
194            #[must_use]
195            pub fn mul_scalar(&self, rhs: $t) -> Self {
196                Self::new(
197                    self.a.mul_scalar(rhs),
198                    self.b.mul_scalar(rhs),
199                    self.d.mul_scalar(rhs),
200                )
201            }
202
203            /// Divides a 5x5 matrix by a scalar.
204            #[inline]
205            #[must_use]
206            pub fn div_scalar(&self, rhs: $t) -> Self {
207                Self::new(
208                    self.a.div_scalar(rhs),
209                    self.b.div_scalar(rhs),
210                    self.d.div_scalar(rhs),
211                )
212            }
213        }
214
215        impl Default for $n {
216            #[inline(always)]
217            fn default() -> Self {
218                Self::IDENTITY
219            }
220        }
221
222        impl Add for $n {
223            type Output = Self;
224            #[inline]
225            fn add(self, rhs: Self) -> Self::Output {
226                Self::new(self.a.add(rhs.a), self.b.add(rhs.b), self.d.add(rhs.d))
227            }
228        }
229
230        impl Add<&Self> for $n {
231            type Output = Self;
232            #[inline]
233            fn add(self, rhs: &Self) -> Self::Output {
234                self.add(*rhs)
235            }
236        }
237
238        impl Add<Self> for &$n {
239            type Output = $n;
240            #[inline]
241            fn add(self, rhs: Self) -> Self::Output {
242                (*self).add(rhs)
243            }
244        }
245
246        impl Add<&Self> for &$n {
247            type Output = $n;
248            #[inline]
249            fn add(self, rhs: &Self) -> Self::Output {
250                (*self).add(*rhs)
251            }
252        }
253
254        impl AddAssign for $n {
255            #[inline]
256            fn add_assign(&mut self, rhs: Self) {
257                *self = self.add(rhs);
258            }
259        }
260
261        impl AddAssign<&Self> for $n {
262            #[inline]
263            fn add_assign(&mut self, rhs: &Self) {
264                self.add_assign(*rhs);
265            }
266        }
267
268        impl Sub for $n {
269            type Output = Self;
270            #[inline]
271            fn sub(self, rhs: Self) -> Self::Output {
272                Self::new(self.a.sub(rhs.a), self.b.sub(rhs.b), self.d.sub(rhs.d))
273            }
274        }
275
276        impl Sub<&Self> for $n {
277            type Output = Self;
278            #[inline]
279            fn sub(self, rhs: &Self) -> Self::Output {
280                self.sub(*rhs)
281            }
282        }
283
284        impl Sub<Self> for &$n {
285            type Output = $n;
286            #[inline]
287            fn sub(self, rhs: Self) -> Self::Output {
288                (*self).sub(rhs)
289            }
290        }
291
292        impl Sub<&Self> for &$n {
293            type Output = $n;
294            #[inline]
295            fn sub(self, rhs: &Self) -> Self::Output {
296                (*self).sub(*rhs)
297            }
298        }
299
300        impl SubAssign for $n {
301            #[inline]
302            fn sub_assign(&mut self, rhs: Self) {
303                *self = self.sub(rhs);
304            }
305        }
306
307        impl SubAssign<&Self> for $n {
308            #[inline]
309            fn sub_assign(&mut self, rhs: &Self) {
310                self.sub_assign(*rhs);
311            }
312        }
313
314        impl Neg for $n {
315            type Output = Self;
316            #[inline]
317            fn neg(self) -> Self::Output {
318                Self::new(-self.a, -self.b, -self.d)
319            }
320        }
321
322        impl Neg for &$n {
323            type Output = $n;
324            #[inline]
325            fn neg(self) -> Self::Output {
326                (*self).neg()
327            }
328        }
329
330        impl Mul<$n> for $t {
331            type Output = $n;
332            #[inline]
333            fn mul(self, rhs: $n) -> Self::Output {
334                rhs.mul_scalar(self)
335            }
336        }
337
338        impl Mul<&$n> for $t {
339            type Output = $n;
340            #[inline]
341            fn mul(self, rhs: &$n) -> Self::Output {
342                self.mul(*rhs)
343            }
344        }
345
346        impl Mul<$n> for &$t {
347            type Output = $n;
348            #[inline]
349            fn mul(self, rhs: $n) -> Self::Output {
350                (*self).mul(rhs)
351            }
352        }
353
354        impl Mul<&$n> for &$t {
355            type Output = $n;
356            #[inline]
357            fn mul(self, rhs: &$n) -> Self::Output {
358                (*self).mul(*rhs)
359            }
360        }
361
362        impl Mul<$t> for $n {
363            type Output = Self;
364            #[inline]
365            fn mul(self, rhs: $t) -> Self::Output {
366                self.mul_scalar(rhs)
367            }
368        }
369
370        impl Mul<&$t> for $n {
371            type Output = Self;
372            #[inline]
373            fn mul(self, rhs: &$t) -> Self::Output {
374                self.mul(*rhs)
375            }
376        }
377
378        impl Mul<$t> for &$n {
379            type Output = $n;
380            #[inline]
381            fn mul(self, rhs: $t) -> Self::Output {
382                (*self).mul(rhs)
383            }
384        }
385
386        impl Mul<&$t> for &$n {
387            type Output = $n;
388            #[inline]
389            fn mul(self, rhs: &$t) -> Self::Output {
390                (*self).mul(*rhs)
391            }
392        }
393
394        impl MulAssign<$t> for $n {
395            #[inline]
396            fn mul_assign(&mut self, rhs: $t) {
397                *self = self.mul(rhs);
398            }
399        }
400
401        impl MulAssign<&$t> for $n {
402            #[inline]
403            fn mul_assign(&mut self, rhs: &$t) {
404                self.mul_assign(*rhs);
405            }
406        }
407
408        impl Div<$n> for $t {
409            type Output = $n;
410            #[inline]
411            fn div(self, rhs: $n) -> Self::Output {
412                rhs.div_scalar(self)
413            }
414        }
415
416        impl Div<&$n> for $t {
417            type Output = $n;
418            #[inline]
419            fn div(self, rhs: &$n) -> Self::Output {
420                self.div(*rhs)
421            }
422        }
423
424        impl Div<$n> for &$t {
425            type Output = $n;
426            #[inline]
427            fn div(self, rhs: $n) -> Self::Output {
428                (*self).div(rhs)
429            }
430        }
431
432        impl Div<&$n> for &$t {
433            type Output = $n;
434            #[inline]
435            fn div(self, rhs: &$n) -> Self::Output {
436                (*self).div(*rhs)
437            }
438        }
439
440        impl Div<$t> for $n {
441            type Output = Self;
442            #[inline]
443            fn div(self, rhs: $t) -> Self::Output {
444                self.div_scalar(rhs)
445            }
446        }
447
448        impl Div<&$t> for $n {
449            type Output = Self;
450            #[inline]
451            fn div(self, rhs: &$t) -> Self::Output {
452                self.div(*rhs)
453            }
454        }
455
456        impl Div<$t> for &$n {
457            type Output = $n;
458            #[inline]
459            fn div(self, rhs: $t) -> Self::Output {
460                (*self).div(rhs)
461            }
462        }
463
464        impl Div<&$t> for &$n {
465            type Output = $n;
466            #[inline]
467            fn div(self, rhs: &$t) -> Self::Output {
468                (*self).div(*rhs)
469            }
470        }
471
472        impl DivAssign<$t> for $n {
473            #[inline]
474            fn div_assign(&mut self, rhs: $t) {
475                *self = self.div(rhs);
476            }
477        }
478
479        impl DivAssign<&$t> for $n {
480            #[inline]
481            fn div_assign(&mut self, rhs: &$t) {
482                self.div_assign(*rhs);
483            }
484        }
485
486        impl Sum<$n> for $n {
487            fn sum<I: Iterator<Item = $n>>(iter: I) -> Self {
488                iter.fold(Self::ZERO, Self::add)
489            }
490        }
491
492        impl<'a> Sum<&'a $n> for $n {
493            fn sum<I: Iterator<Item = &'a $n>>(iter: I) -> Self {
494                iter.fold(Self::ZERO, |a, &b| a.add(b))
495            }
496        }
497
498        #[cfg(feature = "approx")]
499        impl approx::AbsDiffEq for $n {
500            type Epsilon = $t;
501
502            #[inline]
503            fn default_epsilon() -> Self::Epsilon {
504                $t::default_epsilon()
505            }
506
507            #[inline]
508            fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
509                self.a.abs_diff_eq(&other.a, epsilon)
510                    && self.b.abs_diff_eq(&other.b, epsilon)
511                    && self.d.abs_diff_eq(&other.d, epsilon)
512            }
513        }
514
515        #[cfg(feature = "approx")]
516        impl approx::RelativeEq for $n {
517            #[inline]
518            fn default_max_relative() -> Self::Epsilon {
519                $t::default_max_relative()
520            }
521
522            #[inline]
523            fn relative_eq(
524                &self,
525                other: &Self,
526                epsilon: Self::Epsilon,
527                max_relative: Self::Epsilon,
528            ) -> bool {
529                self.a.relative_eq(&other.a, epsilon, max_relative)
530                    && self.b.relative_eq(&other.b, epsilon, max_relative)
531                    && self.d.relative_eq(&other.d, epsilon, max_relative)
532            }
533        }
534
535        #[cfg(feature = "approx")]
536        impl approx::UlpsEq for $n {
537            #[inline]
538            fn default_max_ulps() -> u32 {
539                $t::default_max_ulps()
540            }
541
542            #[inline]
543            fn ulps_eq(
544                &self,
545                other: &Self,
546                epsilon: Self::Epsilon,
547                max_ulps: u32,
548            ) -> bool {
549                self.a.ulps_eq(&other.a, epsilon, max_ulps)
550                    && self.b.ulps_eq(&other.b, epsilon, max_ulps)
551                    && self.d.ulps_eq(&other.d, epsilon, max_ulps)
552            }
553        }
554
555        impl core::fmt::Debug for $n {
556            fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
557                f.debug_struct(stringify!($n))
558                    .field("a", &self.a)
559                    .field("b", &self.b)
560                    .field("d", &self.d)
561                    .finish()
562            }
563        }
564
565        impl core::fmt::Display for $n {
566            fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
567                if let Some(p) = f.precision() {
568                    write!(
569                        f,
570                        r#"[
571  [{:.*}, {:.*}, {:.*}, {:.*}, {:.*}],
572  [{:.*}, {:.*}, {:.*}, {:.*}, {:.*}],
573  [{:.*}, {:.*}, {:.*}, {:.*}, {:.*}],
574  [{:.*}, {:.*}, {:.*}, {:.*}, {:.*}],
575  [{:.*}, {:.*}, {:.*}, {:.*}, {:.*}],
576]"#,
577                        p, self.a.m00, p, self.a.m01, p, self.a.m02, p, self.b.x_axis.x, p, self.b.x_axis.y,
578                        p, self.a.m01, p, self.a.m11, p, self.a.m12, p, self.b.y_axis.x, p, self.b.y_axis.y,
579                        p, self.a.m02, p, self.a.m12, p, self.a.m22, p, self.b.z_axis.x, p, self.b.z_axis.y,
580                        p, self.b.x_axis.x, p, self.b.y_axis.x, p, self.b.z_axis.x, p, self.d.m00, p, self.d.m01,
581                        p, self.b.x_axis.y, p, self.b.y_axis.y, p, self.b.z_axis.y, p, self.d.m01, p, self.d.m11,
582                    )
583                } else {
584                    write!(
585                        f,
586                        r#"[
587  [{}, {}, {}, {}, {}],
588  [{}, {}, {}, {}, {}],
589  [{}, {}, {}, {}, {}],
590  [{}, {}, {}, {}, {}],
591  [{}, {}, {}, {}, {}],
592]"#,
593                        self.a.m00, self.a.m01, self.a.m02, self.b.x_axis.x, self.b.x_axis.y,
594                        self.a.m01, self.a.m11, self.a.m12, self.b.y_axis.x, self.b.y_axis.y,
595                        self.a.m02, self.a.m12, self.a.m22, self.b.z_axis.x, self.b.z_axis.y,
596                        self.b.x_axis.x, self.b.y_axis.x, self.b.z_axis.x, self.d.m00, self.d.m01,
597                        self.b.x_axis.y, self.b.y_axis.y, self.b.z_axis.y, self.d.m01, self.d.m11,
598                    )
599                }
600            }
601        }
602        )+
603    }
604}
605
606#[cfg(feature = "f32")]
607symmetric_mat5s!(SymmetricMat5 => SymmetricMat2, SymmetricMat3, Mat23, Vec2, Vec3, f32);
608
609#[cfg(feature = "f64")]
610symmetric_mat5s!(SymmetricDMat5 => SymmetricDMat2, SymmetricDMat3, DMat23, DVec2, DVec3, f64);
611
612#[cfg(all(feature = "f32", feature = "f64"))]
613impl SymmetricMat5 {
614    /// Returns the double precision version of `self`.
615    #[inline]
616    #[must_use]
617    pub fn as_symmetric_dmat5(&self) -> SymmetricDMat5 {
618        SymmetricDMat5 {
619            a: self.a.as_symmetric_dmat3(),
620            b: self.b.as_dmat23(),
621            d: self.d.as_symmetric_dmat2(),
622        }
623    }
624}
625
626#[cfg(all(feature = "f32", feature = "f64"))]
627impl SymmetricDMat5 {
628    /// Returns the single precision version of `self`.
629    #[inline]
630    #[must_use]
631    pub fn as_symmetric_mat5(&self) -> SymmetricMat5 {
632        SymmetricMat5 {
633            a: self.a.as_symmetric_mat3(),
634            b: self.b.as_mat23(),
635            d: self.d.as_symmetric_mat2(),
636        }
637    }
638}
639
640#[cfg(test)]
641mod tests {
642    use approx::assert_relative_eq;
643    use glam::{Vec2, Vec3, vec2, vec3};
644
645    use crate::{Mat23, SymmetricMat2, SymmetricMat3, SymmetricMat5};
646
647    #[test]
648    fn mul_vec5() {
649        let mat = SymmetricMat5::from_outer_product(Vec3::new(1.0, 2.0, 3.0), Vec2::new(4.0, 5.0));
650
651        let (res1, res2) = mat.mul_vec5(Vec3::new(1.0, 2.0, 3.0), Vec2::new(4.0, 5.0));
652
653        assert_eq!(res1, vec3(55.0, 110.0, 165.0));
654        assert_eq!(res2, vec2(220.0, 275.0));
655    }
656
657    #[test]
658    fn inverse() {
659        let a = SymmetricMat3::new(1.0, 6.0, 7.0, 2.0, 10.0, 3.0);
660        let b = Mat23::from_cols(vec2(8.0, 9.0), vec2(11.0, 12.0), vec2(13.0, 14.0));
661        let d = SymmetricMat2::new(4.0, 15.0, 5.0);
662        let mat = SymmetricMat5 { a, b, d };
663
664        // Known solution x = (x1, x2)
665        let x1 = Vec3::new(1.0, 2.0, 3.0);
666        let x2 = Vec2::new(4.0, 5.0);
667
668        // Compute rhs = mat * x
669        let (rhs1, rhs2) = mat.mul_vec5(x1, x2);
670
671        // Solve
672        let (sol1, sol2) = mat.inverse().mul_vec5(rhs1, rhs2);
673
674        // Check solution
675        assert_relative_eq!(sol1, x1, epsilon = 1e-5);
676        assert_relative_eq!(sol2, x2, epsilon = 1e-5);
677    }
678}
glam_matrix_extras/symmetric/symmetric_mat5.rs

glam_matrix_extras/symmetric/
symmetric_mat5.rs
