rapier3d/utils/
angular_inertia_ops.rs

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

rapier3d/utils/
angular_inertia_ops.rs
