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
13mod eigen3;
14pub use eigen3::SymmetricEigen3;
15
16use crate::RecipOrZero;
17
18pub trait ComputeMassProperties3d {
22 fn mass(&self, density: f32) -> f32;
26
27 #[doc(alias = "unit_principal_moment_of_inertia")]
31 fn unit_principal_angular_inertia(&self) -> Vec3;
32
33 #[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 #[inline]
51 fn local_inertial_frame(&self) -> Quat {
52 Quat::IDENTITY
53 }
54
55 #[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 #[inline]
66 fn angular_inertia_tensor(&self, mass: f32) -> AngularInertiaTensor {
67 mass * self.unit_angular_inertia_tensor()
68 }
69
70 fn center_of_mass(&self) -> Vec3;
74
75 #[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#[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 pub mass: f32,
106 pub principal_angular_inertia: Vec3,
110 pub local_inertial_frame: Quat,
112 pub center_of_mass: Vec3,
116}
117
118impl Default for MassProperties3d {
119 fn default() -> Self {
121 Self::ZERO
122 }
123}
124
125impl MassProperties3d {
126 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 #[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 #[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 #[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 #[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 #[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 #[inline]
226 pub fn unit_principal_angular_inertia(&self) -> Vec3 {
227 self.mass.recip_or_zero() * self.principal_angular_inertia
228 }
229
230 #[inline]
234 pub fn unit_angular_inertia_tensor(&self) -> AngularInertiaTensor {
235 self.mass.recip_or_zero() * self.angular_inertia_tensor()
236 }
237
238 #[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 #[inline]
249 pub fn shifted_angular_inertia_tensor(&self, offset: Vec3) -> AngularInertiaTensor {
250 self.angular_inertia_tensor().shifted(self.mass, offset)
251 }
252
253 #[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 #[inline]
270 pub fn transformed_by(mut self, isometry: impl Into<Isometry3d>) -> Self {
271 self.transform_by(isometry);
272 self
273 }
274
275 #[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 #[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 #[inline]
302 pub fn set_mass(&mut self, new_mass: f32, update_angular_inertia: bool) {
303 if update_angular_inertia {
304 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 let new_center_of_mass =
328 (self.center_of_mass * mass1 + other.center_of_mass * mass2) / new_mass;
329
330 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 return Self {
361 center_of_mass: self.center_of_mass,
362 ..Self::ZERO
363 };
364 }
365
366 let new_mass = mass1 - mass2;
367
368 let new_center_of_mass =
370 (self.center_of_mass * mass1 - other.center_of_mass * mass2) / new_mass;
371
372 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 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}