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
10mod impls;
12
13use crate::RecipOrZero;
14
15pub trait ComputeMassProperties3d {
19 fn mass(&self, density: f32) -> f32;
23
24 #[doc(alias = "unit_principal_moment_of_inertia")]
28 fn unit_principal_angular_inertia(&self) -> Vec3;
29
30 #[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 #[inline]
48 fn local_inertial_frame(&self) -> Quat {
49 Quat::IDENTITY
50 }
51
52 #[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 #[inline]
63 fn angular_inertia_tensor(&self, mass: f32) -> AngularInertiaTensor {
64 mass * self.unit_angular_inertia_tensor()
65 }
66
67 fn center_of_mass(&self) -> Vec3;
71
72 #[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#[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 pub mass: f32,
103 pub principal_angular_inertia: Vec3,
107 pub local_inertial_frame: Quat,
109 pub center_of_mass: Vec3,
113}
114
115impl Default for MassProperties3d {
116 fn default() -> Self {
118 Self::ZERO
119 }
120}
121
122impl MassProperties3d {
123 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 #[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 #[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 #[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 #[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 #[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 #[inline]
223 pub fn unit_principal_angular_inertia(&self) -> Vec3 {
224 self.mass.recip_or_zero() * self.principal_angular_inertia
225 }
226
227 #[inline]
231 pub fn unit_angular_inertia_tensor(&self) -> AngularInertiaTensor {
232 self.mass.recip_or_zero() * self.angular_inertia_tensor()
233 }
234
235 #[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 #[inline]
246 pub fn shifted_angular_inertia_tensor(&self, offset: Vec3) -> AngularInertiaTensor {
247 self.angular_inertia_tensor().shifted(self.mass, offset)
248 }
249
250 #[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 #[inline]
267 pub fn transformed_by(mut self, isometry: impl Into<Isometry3d>) -> Self {
268 self.transform_by(isometry);
269 self
270 }
271
272 #[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 #[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 #[inline]
299 pub fn set_mass(&mut self, new_mass: f32, update_angular_inertia: bool) {
300 if update_angular_inertia {
301 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 let new_center_of_mass =
325 (self.center_of_mass * mass1 + other.center_of_mass * mass2) / new_mass;
326
327 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 return Self {
360 center_of_mass: self.center_of_mass,
361 ..Self::ZERO
362 };
363 }
364
365 let new_mass = mass1 - mass2;
366
367 let new_center_of_mass =
369 (self.center_of_mass * mass1 - other.center_of_mass * mass2) * new_mass.recip_or_zero();
370
371 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 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 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}