glam_matrix_extras/symmetric/
symmetric_mat6.rs

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

glam_matrix_extras/symmetric/
symmetric_mat6.rs
