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