bevy_heavy/dim3/
angular_inertia.rs

1use core::ops::*;
2
3use crate::MatExt;
4use bevy_math::{Mat3, Quat, Vec3};
5#[cfg(all(feature = "bevy_reflect", feature = "serialize"))]
6use bevy_reflect::{ReflectDeserialize, ReflectSerialize};
7
8use super::SymmetricEigen3;
9
10// TODO: Add errors for asymmetric and non-positive-semidefinite matrices.
11/// An error returned for an invalid [`AngularInertiaTensor`] in 3D.
12#[derive(Clone, Copy, Debug, PartialEq)]
13pub enum AngularInertiaTensorError {
14    /// The mass is negative or NaN.
15    Negative,
16    /// The mass is NaN.
17    Nan,
18}
19
20// TODO: The matrix should be symmetric and positive-semidefinite.
21//       We could add a custom `SymmetricMat3` type to enforce symmetricity and reduce memory usage.
22/// The 3x3 [angular inertia] tensor of a 3D object, representing resistance to angular acceleration.
23///
24/// The [inertia tensor] is a [symmetric], [positive-semidefinite] matrix that describes the moment of inertia
25/// for rotations about the X, Y, and Z axes. By [diagonalizing] this matrix, it is possible to extract
26/// the [principal axes of inertia] (a [`Vec3`]) and a local inertial frame (a [`Quat`]) that defines
27/// the XYZ axes. This diagonalization can be performed using the [`principal_angular_inertia_with_local_frame`] method.
28///
29/// The diagonalized representation is more compact and often easier to work with,
30/// but the full tensor can be more efficient for computations using the angular inertia.
31///
32/// [angular inertia]: crate#angular-inertia
33/// [inertia tensor]: https://en.wikipedia.org/wiki/Moment_of_inertia#Inertia_tensor
34/// [symmetric]: https://en.wikipedia.org/wiki/Symmetric_matrix
35/// [positive-semidefinite]: https://en.wikipedia.org/wiki/Definite_matrix
36/// [diagonalizing]: https://en.wikipedia.org/wiki/Diagonalizable_matrix#Diagonalization
37/// [principal axes of inertia]: https://en.wikipedia.org/wiki/Moment_of_inertia#Principal_axes
38/// [`principal_angular_inertia_with_local_frame`]: AngularInertiaTensor::principal_angular_inertia_with_local_frame
39#[derive(Clone, Copy, Debug, Default, PartialEq)]
40#[cfg_attr(feature = "bevy_reflect", derive(bevy_reflect::Reflect))]
41#[cfg_attr(feature = "bevy_reflect", reflect(Debug, PartialEq))]
42#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))]
43#[cfg_attr(
44    all(feature = "bevy_reflect", feature = "serialize"),
45    reflect(Serialize, Deserialize)
46)]
47#[doc(alias = "MomentOfInertiaTensor")]
48pub struct AngularInertiaTensor(Mat3);
49
50impl Deref for AngularInertiaTensor {
51    type Target = Mat3;
52
53    #[inline]
54    fn deref(&self) -> &Self::Target {
55        &self.0
56    }
57}
58
59impl AngularInertiaTensor {
60    /// Zero angular inertia.
61    pub const ZERO: Self = Self(Mat3::ZERO);
62
63    /// An angular inertia tensor with a principal angular inertia of `1.0` along the diagonal.
64    pub const IDENTITY: Self = Self(Mat3::IDENTITY);
65
66    /// Infinite angular inertia.
67    pub const INFINITY: Self = Self(Mat3::from_diagonal(Vec3::INFINITY));
68
69    /// Creates a new [`AngularInertiaTensor`] from the given principal angular inertia.
70    ///
71    /// The principal angular inertia represents the torque needed for a desired angular acceleration
72    /// about the local coordinate axes. To specify the orientation of the local inertial frame,
73    /// consider using [`new_with_local_frame`](AngularInertiaTensor::new_with_local_frame).
74    ///
75    /// # Panics
76    ///
77    /// Panics if any component of the principal angular inertia is negative or NaN
78    /// when `debug_assertions` are enabled.
79    #[inline]
80    #[doc(alias = "from_principal_angular_inertia")]
81    pub fn new(principal_angular_inertia: Vec3) -> Self {
82        debug_assert!(
83            principal_angular_inertia.cmpge(Vec3::ZERO).all(),
84            "principal angular inertia must be positive or zero for all axes"
85        );
86
87        Self(Mat3::from_diagonal(principal_angular_inertia))
88    }
89
90    /// Tries to create a new [`AngularInertiaTensor`] from the given principal angular inertia.
91    ///
92    /// The principal angular inertia represents the torque needed for a desired angular acceleration
93    /// about the local coordinate axes. To specify the orientation of the local inertial frame,
94    /// consider using [`try_new_with_local_frame`](AngularInertiaTensor::try_new_with_local_frame).
95    ///
96    /// # Errors
97    ///
98    /// Returns [`Err(AngularInertiaTensorError)`](AngularInertiaTensorError) if any component
99    /// of the principal angular inertia is negative or NaN.
100    #[inline]
101    pub fn try_new(principal_angular_inertia: Vec3) -> Result<Self, AngularInertiaTensorError> {
102        if !principal_angular_inertia.cmpge(Vec3::ZERO).all() {
103            Err(AngularInertiaTensorError::Negative)
104        } else if principal_angular_inertia.is_nan() {
105            Err(AngularInertiaTensorError::Nan)
106        } else {
107            Ok(Self(Mat3::from_diagonal(principal_angular_inertia)))
108        }
109    }
110
111    /// Creates a new [`AngularInertiaTensor`] from the given principal angular inertia
112    /// and the orientation of the local inertial frame.
113    ///
114    /// The principal angular inertia represents the torque needed for a desired angular acceleration
115    /// about the local coordinate axes defined by the given `orientation`.
116    ///
117    /// # Panics
118    ///
119    /// Panics if any component of the principal angular inertia is negative or NaN
120    /// when `debug_assertions` are enabled.
121    #[inline]
122    #[doc(alias = "from_principal_angular_inertia_with_local_frame")]
123    pub fn new_with_local_frame(principal_angular_inertia: Vec3, orientation: Quat) -> Self {
124        debug_assert!(
125            principal_angular_inertia.cmpge(Vec3::ZERO).all(),
126            "principal angular inertia must be positive or zero for all axes"
127        );
128
129        Self(
130            Mat3::from_quat(orientation)
131                * Mat3::from_diagonal(principal_angular_inertia)
132                * Mat3::from_quat(orientation.inverse()),
133        )
134    }
135
136    /// Tries to create a new [`AngularInertiaTensor`] from the given principal angular inertia
137    /// and the orientation of the local inertial frame.
138    ///
139    /// The principal angular inertia represents the torque needed for a desired angular acceleration
140    /// about the local coordinate axes defined by the given `orientation`.
141    ///
142    /// # Errors
143    ///
144    /// Returns [`Err(AngularInertiaTensorError)`](AngularInertiaTensorError) if any component
145    /// of the principal angular inertia is negative or NaN.
146    #[inline]
147    pub fn try_new_with_local_frame(
148        principal_angular_inertia: Vec3,
149        orientation: Quat,
150    ) -> Result<Self, AngularInertiaTensorError> {
151        if !principal_angular_inertia.cmpge(Vec3::ZERO).all() {
152            Err(AngularInertiaTensorError::Negative)
153        } else if principal_angular_inertia.is_nan() {
154            Err(AngularInertiaTensorError::Nan)
155        } else {
156            Ok(Self(
157                Mat3::from_quat(orientation)
158                    * Mat3::from_diagonal(principal_angular_inertia)
159                    * Mat3::from_quat(orientation.inverse()),
160            ))
161        }
162    }
163
164    /// Creates a new [`AngularInertiaTensor`] from the given angular inertia [tensor].
165    ///
166    /// The tensor should be [symmetric] and [positive-semidefinite], but this is *not* checked.
167    ///
168    /// [tensor]: https://en.wikipedia.org/wiki/Moment_of_inertia#Inertia_tensor
169    /// [symmetric]: https://en.wikipedia.org/wiki/Symmetric_matrix
170    /// [positive-semidefinite]: https://en.wikipedia.org/wiki/Definite_matrix
171    #[inline]
172    #[doc(alias = "from_tensor")]
173    pub fn from_mat3(mat: Mat3) -> Self {
174        Self(mat)
175    }
176
177    /// Returns the angular inertia tensor as a [`Mat3`].
178    ///
179    /// Equivalent to [`value`](AngularInertiaTensor::value).
180    #[doc(alias = "as_tensor")]
181    #[inline]
182    pub fn as_mat3(&self) -> Mat3 {
183        self.0
184    }
185
186    /// Returns a mutable reference to the [`Mat3`] stored in `self`.
187    ///
188    /// Note that this allows making changes that could make the angular inertia tensor invalid
189    /// (non-symmetric or non-positive-semidefinite).
190    ///
191    /// Equivalent to [`value_mut`](AngularInertiaTensor::value_mut).
192    #[doc(alias = "as_tensor_mut")]
193    #[inline]
194    pub fn as_mat3_mut(&mut self) -> &mut Mat3 {
195        &mut self.0
196    }
197
198    /// Returns the angular inertia tensor as a [`Mat3`].
199    ///
200    /// Equivalent to [`as_mat3`](AngularInertiaTensor::as_mat3).
201    #[inline]
202    pub fn value(self) -> Mat3 {
203        self.0
204    }
205
206    /// Returns a mutable reference to the [`Mat3`] stored in `self`.
207    ///
208    /// Note that this allows making changes that could make the angular inertia tensor invalid
209    /// (non-symmetric or non-positive-semidefinite).
210    ///
211    /// Equivalent to [`as_mat3_mut`](AngularInertiaTensor::as_mat3_mut).
212    #[inline]
213    pub fn value_mut(&mut self) -> &mut Mat3 {
214        &mut self.0
215    }
216
217    /// Returns the inverse of the angular inertia tensor.
218    #[inline]
219    pub fn inverse(self) -> Self {
220        Self(self.inverse_or_zero())
221    }
222
223    /// Sets the angular inertia tensor to the given value.
224    #[inline]
225    pub fn set(&mut self, angular_inertia: impl Into<AngularInertiaTensor>) {
226        *self = angular_inertia.into();
227    }
228
229    /// Computes the principal angular inertia and local inertial frame
230    /// by diagonalizing the 3x3 tensor matrix.
231    ///
232    /// The principal angular inertia represents the torque needed for a desired angular acceleration
233    /// about the local coordinate axes defined by the local inertial frame.
234    #[doc(alias = "diagonalize")]
235    pub fn principal_angular_inertia_with_local_frame(&self) -> (Vec3, Quat) {
236        let mut eigen = SymmetricEigen3::new(self.0).reverse();
237
238        if eigen.eigenvectors.determinant() < 0.0 {
239            core::mem::swap(
240                &mut eigen.eigenvectors.y_axis,
241                &mut eigen.eigenvectors.z_axis,
242            );
243            core::mem::swap(&mut eigen.eigenvalues.y, &mut eigen.eigenvalues.z);
244        }
245
246        let mut local_inertial_frame = Quat::from_mat3(&eigen.eigenvectors).normalize();
247
248        if !local_inertial_frame.is_finite() {
249            local_inertial_frame = Quat::IDENTITY;
250        }
251
252        // Clamp eigenvalues to be non-negative.
253        let principal_angular_inertia = eigen.eigenvalues.max(Vec3::ZERO);
254
255        (principal_angular_inertia, local_inertial_frame)
256    }
257
258    /// Computes the angular inertia tensor with the given rotation.
259    ///
260    /// This can be used to transform local angular inertia to world space.
261    #[inline]
262    pub fn rotated(self, rotation: Quat) -> Self {
263        let rot_mat3 = Mat3::from_quat(rotation);
264        Self::from_mat3((rot_mat3 * self.0) * rot_mat3.transpose())
265    }
266
267    /// Computes the angular inertia tensor shifted by the given offset, taking into account the given mass.
268    #[inline]
269    pub fn shifted(self, mass: f32, offset: Vec3) -> Self {
270        if offset != Vec3::ZERO {
271            // https://en.wikipedia.org/wiki/Parallel_axis_theorem#Tensor_generalization
272            let diagonal_element = offset.length_squared();
273            let diagonal_mat = Mat3::from_diagonal(Vec3::splat(diagonal_element));
274            let offset_outer_product =
275                Mat3::from_cols(offset * offset.x, offset * offset.y, offset * offset.z);
276            Self::from_mat3(self.0 + mass * (diagonal_mat - offset_outer_product))
277        } else {
278            self
279        }
280    }
281}
282
283impl From<Mat3> for AngularInertiaTensor {
284    #[inline]
285    fn from(angular_inertia: Mat3) -> Self {
286        Self::from_mat3(angular_inertia)
287    }
288}
289
290impl From<AngularInertiaTensor> for Mat3 {
291    #[inline]
292    fn from(angular_inertia: AngularInertiaTensor) -> Self {
293        angular_inertia.0
294    }
295}
296
297impl TryFrom<Vec3> for AngularInertiaTensor {
298    type Error = AngularInertiaTensorError;
299
300    #[inline]
301    fn try_from(principal_angular_inertia: Vec3) -> Result<Self, Self::Error> {
302        Self::try_new(principal_angular_inertia)
303    }
304}
305
306impl Add<AngularInertiaTensor> for AngularInertiaTensor {
307    type Output = Self;
308
309    #[inline]
310    fn add(self, rhs: AngularInertiaTensor) -> Self {
311        Self(self.0 + rhs.0)
312    }
313}
314
315impl AddAssign<AngularInertiaTensor> for AngularInertiaTensor {
316    #[inline]
317    fn add_assign(&mut self, rhs: AngularInertiaTensor) {
318        self.0 += rhs.0;
319    }
320}
321
322impl Mul<AngularInertiaTensor> for f32 {
323    type Output = AngularInertiaTensor;
324
325    #[inline]
326    fn mul(self, rhs: AngularInertiaTensor) -> AngularInertiaTensor {
327        AngularInertiaTensor(self * rhs.0)
328    }
329}
330
331impl MulAssign<f32> for AngularInertiaTensor {
332    #[inline]
333    fn mul_assign(&mut self, rhs: f32) {
334        self.0 *= rhs;
335    }
336}
337
338impl Div<f32> for AngularInertiaTensor {
339    type Output = Self;
340
341    #[inline]
342    fn div(self, rhs: f32) -> Self {
343        Self(self.0 / rhs)
344    }
345}
346
347impl DivAssign<f32> for AngularInertiaTensor {
348    #[inline]
349    fn div_assign(&mut self, rhs: f32) {
350        self.0 /= rhs;
351    }
352}
353
354impl Mul<AngularInertiaTensor> for Quat {
355    type Output = AngularInertiaTensor;
356
357    #[inline]
358    fn mul(self, angular_inertia: AngularInertiaTensor) -> AngularInertiaTensor {
359        angular_inertia.rotated(self)
360    }
361}
362
363impl Mul<Vec3> for AngularInertiaTensor {
364    type Output = Vec3;
365
366    #[inline]
367    fn mul(self, rhs: Vec3) -> Vec3 {
368        self.0 * rhs
369    }
370}
371
372#[cfg(any(feature = "approx", test))]
373impl approx::AbsDiffEq for AngularInertiaTensor {
374    type Epsilon = f32;
375    fn default_epsilon() -> f32 {
376        f32::EPSILON
377    }
378    fn abs_diff_eq(&self, other: &Self, epsilon: f32) -> bool {
379        self.0.abs_diff_eq(other.0, epsilon)
380    }
381}
382
383#[cfg(any(feature = "approx", test))]
384impl approx::RelativeEq for AngularInertiaTensor {
385    fn default_max_relative() -> f32 {
386        f32::EPSILON
387    }
388    fn relative_eq(&self, other: &Self, epsilon: f32, max_relative: f32) -> bool {
389        self.0.relative_eq(&other.0, epsilon, max_relative)
390    }
391}
392
393#[cfg(any(feature = "approx", test))]
394impl approx::UlpsEq for AngularInertiaTensor {
395    fn default_max_ulps() -> u32 {
396        4
397    }
398    fn ulps_eq(&self, other: &Self, epsilon: f32, max_ulps: u32) -> bool {
399        self.0.ulps_eq(&other.0, epsilon, max_ulps)
400    }
401}
bevy_heavy/dim3/angular_inertia.rs

bevy_heavy/dim3/
angular_inertia.rs
