rapier2d/utils/
angular_inertia_ops.rs

1//! SimdAngularInertia trait for angular inertia operations.
2
3#[cfg(feature = "simd-is-enabled")]
4use crate::math::SimdReal;
5#[cfg(feature = "dim3")]
6use crate::math::{Matrix, Real, Vector};
7use crate::utils::{SimdRealCopy, simd_inv};
8#[cfg(feature = "dim3")]
9use parry::utils::SdpMatrix3;
10
11/// Trait for angular inertia operations.
12pub trait AngularInertiaOps<N>: Copy + core::fmt::Debug + Default {
13    /// The angular vector type.
14    type AngVector;
15    /// The angular matrix type.
16    type AngMatrix;
17    /// Returns the inverse of this angular inertia.
18    fn inverse(&self) -> Self;
19    /// Transforms a vector by this angular inertia.
20    fn transform_vector(&self, pt: Self::AngVector) -> Self::AngVector;
21    /// Converts this angular inertia into a matrix.
22    fn into_matrix(self) -> Self::AngMatrix;
23}
24
25impl<N: SimdRealCopy + Default> AngularInertiaOps<N> for N {
26    type AngVector = N;
27    type AngMatrix = N;
28
29    fn inverse(&self) -> Self {
30        simd_inv(*self)
31    }
32
33    fn transform_vector(&self, pt: N) -> N {
34        pt * *self
35    }
36
37    fn into_matrix(self) -> Self::AngMatrix {
38        self
39    }
40}
41
42#[cfg(feature = "dim3")]
43impl AngularInertiaOps<Real> for SdpMatrix3<Real> {
44    type AngVector = Vector;
45    type AngMatrix = Matrix;
46
47    #[inline]
48    fn inverse(&self) -> Self {
49        let minor_m12_m23 = self.m22 * self.m33 - self.m23 * self.m23;
50        let minor_m11_m23 = self.m12 * self.m33 - self.m13 * self.m23;
51        let minor_m11_m22 = self.m12 * self.m23 - self.m13 * self.m22;
52
53        let determinant =
54            self.m11 * minor_m12_m23 - self.m12 * minor_m11_m23 + self.m13 * minor_m11_m22;
55
56        if determinant == 0.0 {
57            Self::zero()
58        } else {
59            SdpMatrix3 {
60                m11: minor_m12_m23 / determinant,
61                m12: -minor_m11_m23 / determinant,
62                m13: minor_m11_m22 / determinant,
63                m22: (self.m11 * self.m33 - self.m13 * self.m13) / determinant,
64                m23: (self.m13 * self.m12 - self.m23 * self.m11) / determinant,
65                m33: (self.m11 * self.m22 - self.m12 * self.m12) / determinant,
66            }
67        }
68    }
69
70    fn transform_vector(&self, v: Vector) -> Vector {
71        let x = self.m11 * v.x + self.m12 * v.y + self.m13 * v.z;
72        let y = self.m12 * v.x + self.m22 * v.y + self.m23 * v.z;
73        let z = self.m13 * v.x + self.m23 * v.y + self.m33 * v.z;
74        Vector::new(x, y, z)
75    }
76
77    #[inline]
78    #[rustfmt::skip]
79    fn into_matrix(self) -> Matrix {
80        Matrix::from_cols_array(&[
81            self.m11, self.m12, self.m13,
82            self.m12, self.m22, self.m23,
83            self.m13, self.m23, self.m33,
84        ])
85    }
86}
87
88#[cfg(feature = "simd-is-enabled")]
89impl AngularInertiaOps<SimdReal> for parry::utils::SdpMatrix3<SimdReal> {
90    type AngVector = na::Vector3<SimdReal>;
91    type AngMatrix = na::Matrix3<SimdReal>;
92
93    #[inline]
94    fn inverse(&self) -> Self {
95        self.inverse_unchecked()
96    }
97
98    #[inline]
99    fn transform_vector(&self, v: na::Vector3<SimdReal>) -> na::Vector3<SimdReal> {
100        let x = self.m11 * v.x + self.m12 * v.y + self.m13 * v.z;
101        let y = self.m12 * v.x + self.m22 * v.y + self.m23 * v.z;
102        let z = self.m13 * v.x + self.m23 * v.y + self.m33 * v.z;
103        na::Vector3::new(x, y, z)
104    }
105
106    #[inline]
107    #[rustfmt::skip]
108    fn into_matrix(self) -> na::Matrix3<SimdReal> {
109        na::Matrix3::new(
110            self.m11, self.m12, self.m13,
111            self.m12, self.m22, self.m23,
112            self.m13, self.m23, self.m33,
113        )
114    }
115}
rapier2d/utils/angular_inertia_ops.rs

rapier2d/utils/
angular_inertia_ops.rs
