bevy_heavy/dim3/
mod.rs

1use alloc::vec::Vec;
2
3use bevy_math::{DVec3, Isometry3d, Mat3, Quat, Vec3};
4#[cfg(all(feature = "bevy_reflect", feature = "serialize"))]
5use bevy_reflect::{ReflectDeserialize, ReflectSerialize};
6
7mod angular_inertia;
8pub use angular_inertia::{AngularInertiaTensor, AngularInertiaTensorError};
9
10/// [`ComputeMassProperties3d`] implementations for 3D geometric primitives.
11mod impls;
12
13use crate::RecipOrZero;
14
15/// A trait for computing [`MassProperties3d`] for 3D objects.
16///
17/// For the 2D equivalent, see [`ComputeMassProperties2d`](crate::ComputeMassProperties2d).
18pub trait ComputeMassProperties3d {
19    /// Computes the [mass] of the object with a given `density`.
20    ///
21    /// [mass]: crate#mass
22    fn mass(&self, density: f32) -> f32;
23
24    /// Computes the principal [angular inertia] corresponding to a mass of `1.0`.
25    ///
26    /// [angular inertia]: crate#angular-inertia
27    #[doc(alias = "unit_principal_moment_of_inertia")]
28    fn unit_principal_angular_inertia(&self) -> Vec3;
29
30    /// Computes the principal [angular inertia] corresponding to the given `mass`.
31    ///
32    /// Equivalent to `mass * shape.unit_principal_angular_inertia()`.
33    ///
34    /// [angular inertia]: crate#angular-inertia
35    #[inline]
36    #[doc(alias = "principal_moment_of_inertia")]
37    fn principal_angular_inertia(&self, mass: f32) -> Vec3 {
38        mass * self.unit_principal_angular_inertia()
39    }
40
41    /// Computes the orientation of the inertial frame used by the principal axes of [inertia] in local space.
42    ///
43    /// For most primitive shapes, this returns an identity quaternion, which means that the principal axes
44    /// are aligned with the object's XYZ axes.
45    ///
46    /// [inertia]: crate#angular-inertia
47    #[inline]
48    fn local_inertial_frame(&self) -> Quat {
49        Quat::IDENTITY
50    }
51
52    /// Computes the 3x3 [`AngularInertiaTensor`] corresponding to a mass of `1.0`.
53    #[inline]
54    fn unit_angular_inertia_tensor(&self) -> AngularInertiaTensor {
55        AngularInertiaTensor::new_with_local_frame(
56            self.principal_angular_inertia(1.0),
57            self.local_inertial_frame(),
58        )
59    }
60
61    /// Computes the 3x3 [`AngularInertiaTensor`] corresponding to the given `mass`.
62    #[inline]
63    fn angular_inertia_tensor(&self, mass: f32) -> AngularInertiaTensor {
64        mass * self.unit_angular_inertia_tensor()
65    }
66
67    /// Computes the local [center of mass] relative to the object's origin.
68    ///
69    /// [center of mass]: crate#center-of-mass
70    fn center_of_mass(&self) -> Vec3;
71
72    /// Computes the [`MassProperties3d`] with a given `density`.
73    #[inline]
74    fn mass_properties(&self, density: f32) -> MassProperties3d {
75        let mass = self.mass(density);
76        MassProperties3d::new_with_local_frame(
77            mass,
78            self.principal_angular_inertia(mass),
79            self.local_inertial_frame(),
80            self.center_of_mass(),
81        )
82    }
83}
84
85/// The [mass], [angular inertia], and local [center of mass] of an object in 3D space.
86///
87/// [mass]: crate#mass
88/// [angular inertia]: crate#angular-inertia
89/// [center of mass]: crate#center-of-mass
90#[derive(Clone, Copy, Debug, PartialEq)]
91#[cfg_attr(feature = "bevy_reflect", derive(bevy_reflect::Reflect))]
92#[cfg_attr(feature = "bevy_reflect", reflect(Debug, PartialEq))]
93#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))]
94#[cfg_attr(
95    all(feature = "bevy_reflect", feature = "serialize"),
96    reflect(Serialize, Deserialize)
97)]
98pub struct MassProperties3d {
99    /// The [mass].
100    ///
101    /// [mass]: crate#mass
102    pub mass: f32,
103    /// The [angular inertia] along the principal axes defined by the local inertial frame.
104    ///
105    /// [angular inertia]: crate#angular-inertia
106    pub principal_angular_inertia: Vec3,
107    /// The orientation of the local inertial frame, defining the principal axes.
108    pub local_inertial_frame: Quat,
109    /// The local [center of mass] relative to the object's origin.
110    ///
111    /// [center of mass]: crate#center-of-mass
112    pub center_of_mass: Vec3,
113}
114
115impl Default for MassProperties3d {
116    /// Returns the default [`MassProperties3d`], with zero mass and angular inertia.
117    fn default() -> Self {
118        Self::ZERO
119    }
120}
121
122impl MassProperties3d {
123    /// Zero mass and angular inertia.
124    pub const ZERO: Self = Self {
125        mass: 0.0,
126        principal_angular_inertia: Vec3::ZERO,
127        local_inertial_frame: Quat::IDENTITY,
128        center_of_mass: Vec3::ZERO,
129    };
130
131    /// Creates a new [`MassProperties3d`] from a given mass, principal angular inertia,
132    /// and center of mass in local space.
133    #[inline]
134    pub fn new(mass: f32, principal_angular_inertia: Vec3, center_of_mass: Vec3) -> Self {
135        Self::new_with_local_frame(
136            mass,
137            principal_angular_inertia,
138            Quat::IDENTITY,
139            center_of_mass,
140        )
141    }
142
143    /// Creates a new [`MassProperties3d`] from a given mass, principal angular inertia,
144    /// local inertial frame, and center of mass in local space.
145    ///
146    /// The principal angular inertia is the angular inertia along the coordinate axes defined
147    /// by the `local_inertial_frame`, expressed in local space.
148    #[inline]
149    pub fn new_with_local_frame(
150        mass: f32,
151        principal_angular_inertia: Vec3,
152        local_inertial_frame: Quat,
153        center_of_mass: Vec3,
154    ) -> Self {
155        Self {
156            mass,
157            principal_angular_inertia,
158            local_inertial_frame,
159            center_of_mass,
160        }
161    }
162
163    /// Creates a new [`MassProperties3d`] from a given mass, angular inertia tensor,
164    /// and center of mass in local space.
165    ///
166    /// The angular inertia tensor will be diagonalized in order to extract the principal inertia
167    /// values and local principal inertia frme.
168    #[inline]
169    pub fn new_with_angular_inertia_tensor(
170        mass: f32,
171        tensor: impl Into<AngularInertiaTensor>,
172        center_of_mass: Vec3,
173    ) -> Self {
174        let (principal, local_frame) = tensor.into().principal_angular_inertia_with_local_frame();
175        Self::new_with_local_frame(mass, principal, local_frame, center_of_mass)
176    }
177
178    /// Computes approximate mass properties from the given set of points representing a shape.
179    ///
180    /// This can be used to estimate mass properties for arbitrary shapes
181    /// by providing a set of sample points from inside the shape.
182    ///
183    /// The more points there are, and the more uniformly distributed they are,
184    /// the more accurate the estimation will be.
185    #[inline]
186    pub fn from_point_cloud(points: &[Vec3], mass: f32, local_inertial_frame: Quat) -> Self {
187        let points_recip = 1.0 / points.len() as f64;
188
189        let center_of_mass =
190            (points.iter().fold(DVec3::ZERO, |acc, p| acc + p.as_dvec3()) * points_recip).as_vec3();
191        let unit_angular_inertia = points
192            .iter()
193            .fold(DVec3::ZERO, |acc, p| {
194                let p = p.as_dvec3() - center_of_mass.as_dvec3();
195                let r_x = p.reject_from_normalized(DVec3::X).length_squared();
196                let r_y = p.reject_from_normalized(DVec3::Y).length_squared();
197                let r_z = p.reject_from_normalized(DVec3::Z).length_squared();
198                acc + DVec3::new(r_x, r_y, r_z) * points_recip
199            })
200            .as_vec3();
201
202        Self::new_with_local_frame(
203            mass,
204            mass * unit_angular_inertia,
205            local_inertial_frame,
206            center_of_mass,
207        )
208    }
209
210    /// Returns the center of mass transformed into global space using the given [isometry].
211    ///
212    /// [isometry]: Isometry3d
213    #[inline]
214    pub fn global_center_of_mass(&self, isometry: impl Into<Isometry3d>) -> Vec3 {
215        let isometry: Isometry3d = isometry.into();
216        isometry.transform_point(self.center_of_mass).into()
217    }
218
219    /// Computes the principal angular inertia corresponding to a mass of `1.0`.
220    ///
221    /// If the mass is zero, a zero vector is returned.
222    #[inline]
223    pub fn unit_principal_angular_inertia(&self) -> Vec3 {
224        self.mass.recip_or_zero() * self.principal_angular_inertia
225    }
226
227    /// Computes the world-space angular inertia tensor corresponding to a mass of `1.0`.
228    ///
229    /// If the mass is zero, a zero tensor is returned.
230    #[inline]
231    pub fn unit_angular_inertia_tensor(&self) -> AngularInertiaTensor {
232        self.mass.recip_or_zero() * self.angular_inertia_tensor()
233    }
234
235    /// Computes the world-space angular inertia tensor from the principal inertia.
236    #[inline]
237    pub fn angular_inertia_tensor(&self) -> AngularInertiaTensor {
238        AngularInertiaTensor::new_with_local_frame(
239            self.principal_angular_inertia,
240            self.local_inertial_frame,
241        )
242    }
243
244    /// Computes the angular inertia tensor at a given `offset`.
245    #[inline]
246    pub fn shifted_angular_inertia_tensor(&self, offset: Vec3) -> AngularInertiaTensor {
247        self.angular_inertia_tensor().shifted(self.mass, offset)
248    }
249
250    /// Computes the world-space inverse angular inertia tensor with the square root of each element.
251    #[inline]
252    pub fn global_angular_inertia_tensor(&self, rotation: Quat) -> AngularInertiaTensor {
253        let mut lhs = Mat3::from_quat(rotation * self.local_inertial_frame);
254        let rhs = lhs.transpose();
255
256        lhs.x_axis *= self.principal_angular_inertia.x;
257        lhs.y_axis *= self.principal_angular_inertia.y;
258        lhs.z_axis *= self.principal_angular_inertia.z;
259
260        AngularInertiaTensor::from_mat3_unchecked(lhs * rhs)
261    }
262
263    /// Returns the mass properties transformed by the given [isometry].
264    ///
265    /// [isometry]: Isometry3d
266    #[inline]
267    pub fn transformed_by(mut self, isometry: impl Into<Isometry3d>) -> Self {
268        self.transform_by(isometry);
269        self
270    }
271
272    /// Transforms the mass properties by the given [isometry].
273    ///
274    /// [isometry]: Isometry3d
275    #[inline]
276    pub fn transform_by(&mut self, isometry: impl Into<Isometry3d>) {
277        let isometry: Isometry3d = isometry.into();
278        self.center_of_mass = self.global_center_of_mass(isometry);
279        self.local_inertial_frame = isometry.rotation * self.local_inertial_frame;
280    }
281
282    /// Returns the mass propeorties with the inverse of mass and principal angular inertia.
283    ///
284    /// The center of mass and local inertial frame are left unchanged.
285    #[inline]
286    pub fn inverse(&self) -> Self {
287        Self {
288            mass: self.mass.recip_or_zero(),
289            principal_angular_inertia: self.principal_angular_inertia.recip_or_zero(),
290            local_inertial_frame: self.local_inertial_frame,
291            center_of_mass: self.center_of_mass,
292        }
293    }
294
295    /// Sets the mass to the given `new_mass`.
296    ///
297    /// If `update_angular_inertia` is `true`, the principal angular inertia will be scaled accordingly.
298    #[inline]
299    pub fn set_mass(&mut self, new_mass: f32, update_angular_inertia: bool) {
300        if update_angular_inertia {
301            // Adjust angular inertia to match the new mass.
302            self.principal_angular_inertia *= new_mass * self.mass.recip_or_zero();
303        }
304        self.mass = new_mass;
305    }
306}
307
308impl core::ops::Add for MassProperties3d {
309    type Output = Self;
310
311    #[inline]
312    fn add(self, other: Self) -> Self::Output {
313        if self == Self::ZERO {
314            return other;
315        } else if other == Self::ZERO {
316            return self;
317        }
318
319        let mass1 = self.mass;
320        let mass2 = other.mass;
321        let new_mass = mass1 + mass2;
322
323        // The new center of mass is the weighted average of the centers of masses of `self` and `other`.
324        let new_center_of_mass =
325            (self.center_of_mass * mass1 + other.center_of_mass * mass2) / new_mass;
326
327        // Compute the new principal angular inertia, taking the new center of mass into account.
328        let i1 = self.shifted_angular_inertia_tensor(new_center_of_mass - self.center_of_mass);
329        let i2 = other.shifted_angular_inertia_tensor(new_center_of_mass - other.center_of_mass);
330        let new_angular_inertia = AngularInertiaTensor::from_symmetric_mat3(
331            i1.as_symmetric_mat3() + i2.as_symmetric_mat3(),
332        );
333
334        Self::new_with_angular_inertia_tensor(new_mass, new_angular_inertia, new_center_of_mass)
335    }
336}
337
338impl core::ops::AddAssign for MassProperties3d {
339    #[inline]
340    fn add_assign(&mut self, other: Self) {
341        *self = *self + other;
342    }
343}
344
345impl core::ops::Sub for MassProperties3d {
346    type Output = Self;
347
348    #[inline]
349    fn sub(self, other: Self) -> Self::Output {
350        if self == Self::ZERO || other == Self::ZERO {
351            return self;
352        }
353
354        let mass1 = self.mass;
355        let mass2 = other.mass;
356
357        if mass1 <= mass2 {
358            // The result would have non-positive mass.
359            return Self {
360                center_of_mass: self.center_of_mass,
361                ..Self::ZERO
362            };
363        }
364
365        let new_mass = mass1 - mass2;
366
367        // The new center of mass is the negated weighted average of the centers of masses of `self` and `other`.
368        let new_center_of_mass =
369            (self.center_of_mass * mass1 - other.center_of_mass * mass2) * new_mass.recip_or_zero();
370
371        // Compute the new principal angular inertia, taking the new center of mass into account.
372        let i1 = self.shifted_angular_inertia_tensor(new_center_of_mass - self.center_of_mass);
373        let i2 = other.shifted_angular_inertia_tensor(new_center_of_mass - other.center_of_mass);
374        let new_angular_inertia = AngularInertiaTensor::from_symmetric_mat3(
375            i1.as_symmetric_mat3() - i2.as_symmetric_mat3(),
376        );
377
378        Self::new_with_angular_inertia_tensor(new_mass, new_angular_inertia, new_center_of_mass)
379    }
380}
381
382impl core::ops::SubAssign for MassProperties3d {
383    #[inline]
384    fn sub_assign(&mut self, other: Self) {
385        *self = *self - other;
386    }
387}
388
389impl core::iter::Sum for MassProperties3d {
390    #[inline]
391    fn sum<I: Iterator<Item = Self>>(iter: I) -> Self {
392        let mut total_mass = 0.0;
393        let mut total_angular_inertia = AngularInertiaTensor::ZERO;
394        let mut total_center_of_mass = Vec3::ZERO;
395
396        // TODO: Avoid this allocation if possible. This is currently needed because we iterate twice.
397        let mut all_properties = Vec::with_capacity(iter.size_hint().1.unwrap_or_default());
398
399        for props in iter {
400            total_mass += props.mass;
401            total_center_of_mass += props.center_of_mass * props.mass;
402            all_properties.push(props);
403        }
404
405        if total_mass > 0.0 {
406            total_center_of_mass *= total_mass.recip_or_zero();
407        }
408
409        for props in all_properties {
410            total_angular_inertia +=
411                props.shifted_angular_inertia_tensor(total_center_of_mass - props.center_of_mass);
412        }
413
414        if !total_center_of_mass.is_finite() {
415            // The center of mass can be non-finite if the mass is non-finite.
416            total_center_of_mass = Vec3::ZERO;
417        }
418
419        Self::new_with_angular_inertia_tensor(
420            total_mass,
421            total_angular_inertia,
422            total_center_of_mass,
423        )
424    }
425}
426
427#[cfg(any(feature = "approx", test))]
428impl approx::AbsDiffEq for MassProperties3d {
429    type Epsilon = f32;
430    fn default_epsilon() -> f32 {
431        f32::EPSILON
432    }
433    fn abs_diff_eq(&self, other: &Self, epsilon: f32) -> bool {
434        self.mass.abs_diff_eq(&other.mass, epsilon)
435            && self
436                .principal_angular_inertia
437                .abs_diff_eq(other.principal_angular_inertia, epsilon)
438            && self
439                .local_inertial_frame
440                .abs_diff_eq(other.local_inertial_frame, epsilon)
441            && self
442                .center_of_mass
443                .abs_diff_eq(other.center_of_mass, epsilon)
444    }
445}
446
447#[cfg(any(feature = "approx", test))]
448impl approx::RelativeEq for MassProperties3d {
449    fn default_max_relative() -> f32 {
450        f32::EPSILON
451    }
452    fn relative_eq(&self, other: &Self, epsilon: f32, max_relative: f32) -> bool {
453        self.mass.relative_eq(&other.mass, epsilon, max_relative)
454            && self.principal_angular_inertia.relative_eq(
455                &other.principal_angular_inertia,
456                epsilon,
457                max_relative,
458            )
459            && self.local_inertial_frame.relative_eq(
460                &other.local_inertial_frame,
461                epsilon,
462                max_relative,
463            )
464            && self
465                .center_of_mass
466                .relative_eq(&other.center_of_mass, epsilon, max_relative)
467    }
468}
469
470#[cfg(any(feature = "approx", test))]
471impl approx::UlpsEq for MassProperties3d {
472    fn default_max_ulps() -> u32 {
473        4
474    }
475    fn ulps_eq(&self, other: &Self, epsilon: f32, max_ulps: u32) -> bool {
476        self.mass.ulps_eq(&other.mass, epsilon, max_ulps)
477            && self.principal_angular_inertia.ulps_eq(
478                &other.principal_angular_inertia,
479                epsilon,
480                max_ulps,
481            )
482            && self
483                .local_inertial_frame
484                .ulps_eq(&other.local_inertial_frame, epsilon, max_ulps)
485            && self
486                .center_of_mass
487                .ulps_eq(&other.center_of_mass, epsilon, max_ulps)
488    }
489}
490
491#[cfg(test)]
492mod tests {
493    use alloc::vec;
494    use bevy_math::primitives::Cuboid;
495
496    use super::*;
497
498    #[test]
499    fn sum() {
500        let mass_props = Cuboid::from_length(1.0).mass_properties(1.0);
501
502        let sum: MassProperties3d = vec![
503            mass_props,
504            mass_props.transformed_by(Vec3::Y),
505            mass_props.transformed_by(Vec3::NEG_Y),
506        ]
507        .into_iter()
508        .sum();
509
510        let expected = Cuboid::new(1.0, 3.0, 1.0).mass_properties(1.0);
511        assert_eq!(sum.mass, expected.mass);
512        assert_eq!(
513            sum.angular_inertia_tensor(),
514            expected.angular_inertia_tensor()
515        );
516        assert_eq!(sum.center_of_mass, expected.center_of_mass);
517    }
518}