parry3d/utils/
sdp_matrix.rs

1use crate::math::Real;
2use core::ops::{Add, Mul};
3use na::{Matrix2, Matrix3, Matrix3x2, SimdRealField, Vector2, Vector3};
4
5#[cfg(feature = "rkyv")]
6#[cfg(feature = "rkyv")]
7use rkyv::{bytecheck, Archive, CheckBytes};
8
9/// A 2x2 symmetric-definite-positive matrix.
10#[derive(Copy, Clone, Debug, PartialEq)]
11#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
12#[cfg_attr(
13    feature = "rkyv",
14    derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes),
15    archive(as = "Self", bound(archive = "N: Archive<Archived = N>"))
16)]
17pub struct SdpMatrix2<N> {
18    /// The component at the first row and first column of this matrix.
19    pub m11: N,
20    /// The component at the first row and second column of this matrix.
21    pub m12: N,
22    /// The component at the second row and second column of this matrix.
23    pub m22: N,
24}
25
26impl<N: SimdRealField + Copy> SdpMatrix2<N> {
27    /// A new SDP 2x2 matrix with the given components.
28    ///
29    /// Because the matrix is symmetric, only the lower off-diagonal component is required.
30    pub fn new(m11: N, m12: N, m22: N) -> Self {
31        Self { m11, m12, m22 }
32    }
33
34    /// Build an `SdpMatrix2` structure from a plain matrix, assuming it is SDP.
35    ///
36    /// No check is performed to ensure `mat` is actually SDP.
37    pub fn from_sdp_matrix(mat: Matrix2<N>) -> Self {
38        Self {
39            m11: mat.m11,
40            m12: mat.m12,
41            m22: mat.m22,
42        }
43    }
44
45    /// Create a new SDP matrix filled with zeros.
46    pub fn zero() -> Self {
47        Self {
48            m11: N::zero(),
49            m12: N::zero(),
50            m22: N::zero(),
51        }
52    }
53
54    /// Create a new SDP matrix with its diagonal filled with `val`, and its off-diagonal elements set to zero.
55    pub fn diagonal(val: N) -> Self {
56        Self {
57            m11: val,
58            m12: N::zero(),
59            m22: val,
60        }
61    }
62
63    /// Adds `val` to the diagonal components of `self`.
64    pub fn add_diagonal(&mut self, elt: N) -> Self {
65        Self {
66            m11: self.m11 + elt,
67            m12: self.m12,
68            m22: self.m22 + elt,
69        }
70    }
71
72    /// Compute the inverse of this SDP matrix without performing any inversibility check.
73    pub fn inverse_unchecked(&self) -> Self {
74        self.inverse_and_get_determinant_unchecked().0
75    }
76
77    /// Compute the inverse of this SDP matrix without performing any inversibility check.
78    pub fn inverse_and_get_determinant_unchecked(&self) -> (Self, N) {
79        let determinant = self.m11 * self.m22 - self.m12 * self.m12;
80        let m11 = self.m22 / determinant;
81        let m12 = -self.m12 / determinant;
82        let m22 = self.m11 / determinant;
83
84        (Self { m11, m12, m22 }, determinant)
85    }
86
87    /// Convert this SDP matrix to a regular matrix representation.
88    pub fn into_matrix(self) -> Matrix2<N> {
89        Matrix2::new(self.m11, self.m12, self.m12, self.m22)
90    }
91}
92
93impl<N: SimdRealField + Copy> Add<SdpMatrix2<N>> for SdpMatrix2<N> {
94    type Output = Self;
95
96    fn add(self, rhs: SdpMatrix2<N>) -> Self {
97        Self::new(self.m11 + rhs.m11, self.m12 + rhs.m12, self.m22 + rhs.m22)
98    }
99}
100
101impl<N: SimdRealField + Copy> Mul<Vector2<N>> for SdpMatrix2<N> {
102    type Output = Vector2<N>;
103
104    fn mul(self, rhs: Vector2<N>) -> Self::Output {
105        Vector2::new(
106            self.m11 * rhs.x + self.m12 * rhs.y,
107            self.m12 * rhs.x + self.m22 * rhs.y,
108        )
109    }
110}
111
112impl Mul<Real> for SdpMatrix2<Real> {
113    type Output = SdpMatrix2<Real>;
114
115    fn mul(self, rhs: Real) -> Self::Output {
116        SdpMatrix2::new(self.m11 * rhs, self.m12 * rhs, self.m22 * rhs)
117    }
118}
119
120/// A 3x3 symmetric-definite-positive matrix.
121#[derive(Copy, Clone, Debug, PartialEq)]
122#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
123#[cfg_attr(
124    feature = "rkyv",
125    derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes),
126    archive(as = "Self", bound(archive = "N: Archive<Archived = N>"))
127)]
128pub struct SdpMatrix3<N> {
129    /// The component at the first row and first column of this matrix.
130    pub m11: N,
131    /// The component at the first row and second column of this matrix.
132    pub m12: N,
133    /// The component at the first row and third column of this matrix.
134    pub m13: N,
135    /// The component at the second row and second column of this matrix.
136    pub m22: N,
137    /// The component at the second row and third column of this matrix.
138    pub m23: N,
139    /// The component at the third row and third column of this matrix.
140    pub m33: N,
141}
142
143impl<N: SimdRealField + Copy> SdpMatrix3<N> {
144    /// A new SDP 3x3 matrix with the given components.
145    ///
146    /// Because the matrix is symmetric, only the lower off-diagonal components is required.
147    pub fn new(m11: N, m12: N, m13: N, m22: N, m23: N, m33: N) -> Self {
148        Self {
149            m11,
150            m12,
151            m13,
152            m22,
153            m23,
154            m33,
155        }
156    }
157
158    /// Build an `SdpMatrix3` structure from a plain matrix, assuming it is SDP.
159    ///
160    /// No check is performed to ensure `mat` is actually SDP.
161    pub fn from_sdp_matrix(mat: Matrix3<N>) -> Self {
162        Self {
163            m11: mat.m11,
164            m12: mat.m12,
165            m13: mat.m13,
166            m22: mat.m22,
167            m23: mat.m23,
168            m33: mat.m33,
169        }
170    }
171
172    /// Create a new SDP matrix filled with zeros.
173    pub fn zero() -> Self {
174        Self {
175            m11: N::zero(),
176            m12: N::zero(),
177            m13: N::zero(),
178            m22: N::zero(),
179            m23: N::zero(),
180            m33: N::zero(),
181        }
182    }
183
184    /// Create a new SDP matrix with its diagonal filled with `val`, and its off-diagonal elements set to zero.
185    pub fn diagonal(val: N) -> Self {
186        Self {
187            m11: val,
188            m12: N::zero(),
189            m13: N::zero(),
190            m22: val,
191            m23: N::zero(),
192            m33: val,
193        }
194    }
195
196    /// Are all components of this matrix equal to zero?
197    pub fn is_zero(&self) -> bool {
198        self.m11.is_zero()
199            && self.m12.is_zero()
200            && self.m13.is_zero()
201            && self.m22.is_zero()
202            && self.m23.is_zero()
203            && self.m33.is_zero()
204    }
205
206    /// Compute the inverse of this SDP matrix without performing any inversibility check.
207    pub fn inverse_unchecked(&self) -> Self {
208        let minor_m12_m23 = self.m22 * self.m33 - self.m23 * self.m23;
209        let minor_m11_m23 = self.m12 * self.m33 - self.m13 * self.m23;
210        let minor_m11_m22 = self.m12 * self.m23 - self.m13 * self.m22;
211
212        let determinant =
213            self.m11 * minor_m12_m23 - self.m12 * minor_m11_m23 + self.m13 * minor_m11_m22;
214        let inv_det = N::one() / determinant;
215
216        SdpMatrix3 {
217            m11: minor_m12_m23 * inv_det,
218            m12: -minor_m11_m23 * inv_det,
219            m13: minor_m11_m22 * inv_det,
220            m22: (self.m11 * self.m33 - self.m13 * self.m13) * inv_det,
221            m23: (self.m13 * self.m12 - self.m23 * self.m11) * inv_det,
222            m33: (self.m11 * self.m22 - self.m12 * self.m12) * inv_det,
223        }
224    }
225
226    /// Compute the quadratic form `m.transpose() * self * m`.
227    pub fn quadform3x2(&self, m: &Matrix3x2<N>) -> SdpMatrix2<N> {
228        let x0 = self.m11 * m.m11 + self.m12 * m.m21 + self.m13 * m.m31;
229        let y0 = self.m12 * m.m11 + self.m22 * m.m21 + self.m23 * m.m31;
230        let z0 = self.m13 * m.m11 + self.m23 * m.m21 + self.m33 * m.m31;
231
232        let x1 = self.m11 * m.m12 + self.m12 * m.m22 + self.m13 * m.m32;
233        let y1 = self.m12 * m.m12 + self.m22 * m.m22 + self.m23 * m.m32;
234        let z1 = self.m13 * m.m12 + self.m23 * m.m22 + self.m33 * m.m32;
235
236        let m11 = m.m11 * x0 + m.m21 * y0 + m.m31 * z0;
237        let m12 = m.m11 * x1 + m.m21 * y1 + m.m31 * z1;
238        let m22 = m.m12 * x1 + m.m22 * y1 + m.m32 * z1;
239
240        SdpMatrix2 { m11, m12, m22 }
241    }
242
243    /// Compute the quadratic form `m.transpose() * self * m`.
244    pub fn quadform(&self, m: &Matrix3<N>) -> Self {
245        let x0 = self.m11 * m.m11 + self.m12 * m.m21 + self.m13 * m.m31;
246        let y0 = self.m12 * m.m11 + self.m22 * m.m21 + self.m23 * m.m31;
247        let z0 = self.m13 * m.m11 + self.m23 * m.m21 + self.m33 * m.m31;
248
249        let x1 = self.m11 * m.m12 + self.m12 * m.m22 + self.m13 * m.m32;
250        let y1 = self.m12 * m.m12 + self.m22 * m.m22 + self.m23 * m.m32;
251        let z1 = self.m13 * m.m12 + self.m23 * m.m22 + self.m33 * m.m32;
252
253        let x2 = self.m11 * m.m13 + self.m12 * m.m23 + self.m13 * m.m33;
254        let y2 = self.m12 * m.m13 + self.m22 * m.m23 + self.m23 * m.m33;
255        let z2 = self.m13 * m.m13 + self.m23 * m.m23 + self.m33 * m.m33;
256
257        let m11 = m.m11 * x0 + m.m21 * y0 + m.m31 * z0;
258        let m12 = m.m11 * x1 + m.m21 * y1 + m.m31 * z1;
259        let m13 = m.m11 * x2 + m.m21 * y2 + m.m31 * z2;
260
261        let m22 = m.m12 * x1 + m.m22 * y1 + m.m32 * z1;
262        let m23 = m.m12 * x2 + m.m22 * y2 + m.m32 * z2;
263        let m33 = m.m13 * x2 + m.m23 * y2 + m.m33 * z2;
264
265        Self {
266            m11,
267            m12,
268            m13,
269            m22,
270            m23,
271            m33,
272        }
273    }
274
275    /// Adds `elt` to the diagonal components of `self`.
276    pub fn add_diagonal(&self, elt: N) -> Self {
277        Self {
278            m11: self.m11 + elt,
279            m12: self.m12,
280            m13: self.m13,
281            m22: self.m22 + elt,
282            m23: self.m23,
283            m33: self.m33 + elt,
284        }
285    }
286}
287
288impl<N: Add<N>> Add<SdpMatrix3<N>> for SdpMatrix3<N> {
289    type Output = SdpMatrix3<N::Output>;
290
291    fn add(self, rhs: SdpMatrix3<N>) -> Self::Output {
292        SdpMatrix3 {
293            m11: self.m11 + rhs.m11,
294            m12: self.m12 + rhs.m12,
295            m13: self.m13 + rhs.m13,
296            m22: self.m22 + rhs.m22,
297            m23: self.m23 + rhs.m23,
298            m33: self.m33 + rhs.m33,
299        }
300    }
301}
302
303impl Mul<Real> for SdpMatrix3<Real> {
304    type Output = SdpMatrix3<Real>;
305
306    fn mul(self, rhs: Real) -> Self::Output {
307        SdpMatrix3 {
308            m11: self.m11 * rhs,
309            m12: self.m12 * rhs,
310            m13: self.m13 * rhs,
311            m22: self.m22 * rhs,
312            m23: self.m23 * rhs,
313            m33: self.m33 * rhs,
314        }
315    }
316}
317
318impl<N: SimdRealField + Copy> Mul<Vector3<N>> for SdpMatrix3<N> {
319    type Output = Vector3<N>;
320
321    fn mul(self, rhs: Vector3<N>) -> Self::Output {
322        let x = self.m11 * rhs.x + self.m12 * rhs.y + self.m13 * rhs.z;
323        let y = self.m12 * rhs.x + self.m22 * rhs.y + self.m23 * rhs.z;
324        let z = self.m13 * rhs.x + self.m23 * rhs.y + self.m33 * rhs.z;
325        Vector3::new(x, y, z)
326    }
327}
328
329impl<N: SimdRealField + Copy> Mul<Matrix3<N>> for SdpMatrix3<N> {
330    type Output = Matrix3<N>;
331
332    fn mul(self, rhs: Matrix3<N>) -> Self::Output {
333        let x0 = self.m11 * rhs.m11 + self.m12 * rhs.m21 + self.m13 * rhs.m31;
334        let y0 = self.m12 * rhs.m11 + self.m22 * rhs.m21 + self.m23 * rhs.m31;
335        let z0 = self.m13 * rhs.m11 + self.m23 * rhs.m21 + self.m33 * rhs.m31;
336
337        let x1 = self.m11 * rhs.m12 + self.m12 * rhs.m22 + self.m13 * rhs.m32;
338        let y1 = self.m12 * rhs.m12 + self.m22 * rhs.m22 + self.m23 * rhs.m32;
339        let z1 = self.m13 * rhs.m12 + self.m23 * rhs.m22 + self.m33 * rhs.m32;
340
341        let x2 = self.m11 * rhs.m13 + self.m12 * rhs.m23 + self.m13 * rhs.m33;
342        let y2 = self.m12 * rhs.m13 + self.m22 * rhs.m23 + self.m23 * rhs.m33;
343        let z2 = self.m13 * rhs.m13 + self.m23 * rhs.m23 + self.m33 * rhs.m33;
344
345        Matrix3::new(x0, x1, x2, y0, y1, y2, z0, z1, z2)
346    }
347}
348
349impl<N: SimdRealField + Copy> Mul<Matrix3x2<N>> for SdpMatrix3<N> {
350    type Output = Matrix3x2<N>;
351
352    fn mul(self, rhs: Matrix3x2<N>) -> Self::Output {
353        let x0 = self.m11 * rhs.m11 + self.m12 * rhs.m21 + self.m13 * rhs.m31;
354        let y0 = self.m12 * rhs.m11 + self.m22 * rhs.m21 + self.m23 * rhs.m31;
355        let z0 = self.m13 * rhs.m11 + self.m23 * rhs.m21 + self.m33 * rhs.m31;
356
357        let x1 = self.m11 * rhs.m12 + self.m12 * rhs.m22 + self.m13 * rhs.m32;
358        let y1 = self.m12 * rhs.m12 + self.m22 * rhs.m22 + self.m23 * rhs.m32;
359        let z1 = self.m13 * rhs.m12 + self.m23 * rhs.m22 + self.m33 * rhs.m32;
360
361        Matrix3x2::new(x0, x1, y0, y1, z0, z1)
362    }
363}
364
365impl<T> From<[SdpMatrix3<Real>; 4]> for SdpMatrix3<T>
366where
367    T: From<[Real; 4]>,
368{
369    fn from(data: [SdpMatrix3<Real>; 4]) -> Self {
370        SdpMatrix3 {
371            m11: T::from([data[0].m11, data[1].m11, data[2].m11, data[3].m11]),
372            m12: T::from([data[0].m12, data[1].m12, data[2].m12, data[3].m12]),
373            m13: T::from([data[0].m13, data[1].m13, data[2].m13, data[3].m13]),
374            m22: T::from([data[0].m22, data[1].m22, data[2].m22, data[3].m22]),
375            m23: T::from([data[0].m23, data[1].m23, data[2].m23, data[3].m23]),
376            m33: T::from([data[0].m33, data[1].m33, data[2].m33, data[3].m33]),
377        }
378    }
379}
380
381#[cfg(feature = "simd-nightly")]
382impl From<[SdpMatrix3<Real>; 8]> for SdpMatrix3<simba::simd::f32x8> {
383    fn from(data: [SdpMatrix3<Real>; 8]) -> Self {
384        SdpMatrix3 {
385            m11: simba::simd::f32x8::from([
386                data[0].m11,
387                data[1].m11,
388                data[2].m11,
389                data[3].m11,
390                data[4].m11,
391                data[5].m11,
392                data[6].m11,
393                data[7].m11,
394            ]),
395            m12: simba::simd::f32x8::from([
396                data[0].m12,
397                data[1].m12,
398                data[2].m12,
399                data[3].m12,
400                data[4].m12,
401                data[5].m12,
402                data[6].m12,
403                data[7].m12,
404            ]),
405            m13: simba::simd::f32x8::from([
406                data[0].m13,
407                data[1].m13,
408                data[2].m13,
409                data[3].m13,
410                data[4].m13,
411                data[5].m13,
412                data[6].m13,
413                data[7].m13,
414            ]),
415            m22: simba::simd::f32x8::from([
416                data[0].m22,
417                data[1].m22,
418                data[2].m22,
419                data[3].m22,
420                data[4].m22,
421                data[5].m22,
422                data[6].m22,
423                data[7].m22,
424            ]),
425            m23: simba::simd::f32x8::from([
426                data[0].m23,
427                data[1].m23,
428                data[2].m23,
429                data[3].m23,
430                data[4].m23,
431                data[5].m23,
432                data[6].m23,
433                data[7].m23,
434            ]),
435            m33: simba::simd::f32x8::from([
436                data[0].m33,
437                data[1].m33,
438                data[2].m33,
439                data[3].m33,
440                data[4].m33,
441                data[5].m33,
442                data[6].m33,
443                data[7].m33,
444            ]),
445        }
446    }
447}
448
449#[cfg(feature = "simd-nightly")]
450impl From<[SdpMatrix3<Real>; 16]> for SdpMatrix3<simba::simd::f32x16> {
451    fn from(data: [SdpMatrix3<Real>; 16]) -> Self {
452        SdpMatrix3 {
453            m11: simba::simd::f32x16::from([
454                data[0].m11,
455                data[1].m11,
456                data[2].m11,
457                data[3].m11,
458                data[4].m11,
459                data[5].m11,
460                data[6].m11,
461                data[7].m11,
462                data[8].m11,
463                data[9].m11,
464                data[10].m11,
465                data[11].m11,
466                data[12].m11,
467                data[13].m11,
468                data[14].m11,
469                data[15].m11,
470            ]),
471            m12: simba::simd::f32x16::from([
472                data[0].m12,
473                data[1].m12,
474                data[2].m12,
475                data[3].m12,
476                data[4].m12,
477                data[5].m12,
478                data[6].m12,
479                data[7].m12,
480                data[8].m12,
481                data[9].m12,
482                data[10].m12,
483                data[11].m12,
484                data[12].m12,
485                data[13].m12,
486                data[14].m12,
487                data[15].m12,
488            ]),
489            m13: simba::simd::f32x16::from([
490                data[0].m13,
491                data[1].m13,
492                data[2].m13,
493                data[3].m13,
494                data[4].m13,
495                data[5].m13,
496                data[6].m13,
497                data[7].m13,
498                data[8].m13,
499                data[9].m13,
500                data[10].m13,
501                data[11].m13,
502                data[12].m13,
503                data[13].m13,
504                data[14].m13,
505                data[15].m13,
506            ]),
507            m22: simba::simd::f32x16::from([
508                data[0].m22,
509                data[1].m22,
510                data[2].m22,
511                data[3].m22,
512                data[4].m22,
513                data[5].m22,
514                data[6].m22,
515                data[7].m22,
516                data[8].m22,
517                data[9].m22,
518                data[10].m22,
519                data[11].m22,
520                data[12].m22,
521                data[13].m22,
522                data[14].m22,
523                data[15].m22,
524            ]),
525            m23: simba::simd::f32x16::from([
526                data[0].m23,
527                data[1].m23,
528                data[2].m23,
529                data[3].m23,
530                data[4].m23,
531                data[5].m23,
532                data[6].m23,
533                data[7].m23,
534                data[8].m23,
535                data[9].m23,
536                data[10].m23,
537                data[11].m23,
538                data[12].m23,
539                data[13].m23,
540                data[14].m23,
541                data[15].m23,
542            ]),
543            m33: simba::simd::f32x16::from([
544                data[0].m33,
545                data[1].m33,
546                data[2].m33,
547                data[3].m33,
548                data[4].m33,
549                data[5].m33,
550                data[6].m33,
551                data[7].m33,
552                data[8].m33,
553                data[9].m33,
554                data[10].m33,
555                data[11].m33,
556                data[12].m33,
557                data[13].m33,
558                data[14].m33,
559                data[15].m33,
560            ]),
561        }
562    }
563}
parry3d/utils/sdp_matrix.rs

parry3d/utils/
sdp_matrix.rs
