parry2d/mass_properties/
mass_properties.rs

1use crate::math::{AngVector, AngularInertia, Pose, Real, Rotation, Vector};
2#[cfg(feature = "dim3")]
3use crate::math::{MatExt, Matrix, VectorExt};
4use crate::utils;
5use core::ops::{Add, AddAssign, Sub, SubAssign};
6use num::Zero;
7
8#[cfg_attr(feature = "f32", expect(clippy::unnecessary_cast))]
9const EPSILON: Real = f32::EPSILON as Real;
10
11#[derive(Copy, Clone, Debug, Default, PartialEq)]
12#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
13#[cfg_attr(
14    feature = "rkyv",
15    derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize)
16)]
17/// The mass properties of a rigid body.
18///
19/// Mass properties define how an object responds to forces and torques in physics
20/// simulation. They include the mass, center of mass, and angular inertia (resistance
21/// to rotation).
22///
23/// # Fields
24///
25/// - **local_com**: Center of mass in the shape's local coordinate system
26/// - **inv_mass**: Inverse mass (1/mass). Zero = infinite mass (immovable)
27/// - **inv_principal_inertia**: Inverse angular inertia along principal axes
28/// - **principal_inertia_local_frame** (3D only): Rotation to principal inertia axes
29///
30/// # Why Inverse Values?
31///
32/// Physics engines store inverse mass and inertia because:
33/// - Infinite mass/inertia (immovable objects) = zero inverse
34/// - Avoids division in the simulation loop (multiply by inverse instead)
35/// - More numerically stable for very heavy objects
36///
37/// # Angular Inertia
38///
39/// Angular inertia (moment of inertia) describes resistance to rotation:
40/// - **Higher values**: Harder to spin (like a heavy wheel)
41/// - **Lower values**: Easier to spin (like a light rod)
42/// - **Different per axis**: Objects resist rotation differently around different axes
43///
44/// # Principal Inertia
45///
46/// The inertia tensor is diagonalized to principal axes:
47/// - Rotation around principal axes is independent
48/// - Simplifies physics calculations
49/// - In 2D: Only one axis (perpendicular to the plane)
50/// - In 3D: Three orthogonal axes
51///
52/// # Example
53///
54/// ```rust
55/// # #[cfg(all(feature = "dim3", feature = "f32"))] {
56/// use parry3d::mass_properties::MassProperties;
57/// use parry3d::shape::{Ball, Shape};
58/// use parry3d::math::Vector;
59///
60/// // Compute mass properties for a unit ball with density 1.0
61/// let ball = Ball::new(1.0);
62/// let props = ball.mass_properties(1.0);
63///
64/// // Mass of a unit sphere with density 1.0
65/// let mass = props.mass();
66/// println!("Mass: {}", mass);
67///
68/// // Center of mass (at origin for a ball)
69/// assert_eq!(props.local_com, Vector::ZERO);
70///
71/// // For simulation, use inverse values
72/// if props.inv_mass > 0.0 {
73///     // Object has finite mass - can be moved
74///     println!("Can apply forces");
75/// } else {
76///     // Object has infinite mass - immovable (like terrain)
77///     println!("Static/kinematic object");
78/// }
79/// # }
80/// ```
81///
82/// # Combining Mass Properties
83///
84/// Mass properties can be added to create compound objects:
85///
86/// ```rust
87/// # #[cfg(all(feature = "dim3", feature = "f32"))] {
88/// use parry3d::mass_properties::MassProperties;
89/// use parry3d::shape::{Ball, Cuboid, Shape};
90/// use parry3d::math::Vector;
91///
92/// let ball = Ball::new(1.0);
93/// let cuboid = Cuboid::new(Vector::new(1.0, 1.0, 1.0));
94///
95/// let ball_props = ball.mass_properties(1.0);
96/// let cuboid_props = cuboid.mass_properties(1.0);
97///
98/// // Combined properties (ball + cuboid)
99/// let combined = ball_props + cuboid_props;
100///
101/// // Total mass is sum of individual masses
102/// let total_mass = combined.mass();
103/// println!("Combined mass: {}", total_mass);
104/// # }
105/// ```
106pub struct MassProperties {
107    /// The center of mass in local (shape-relative) coordinates.
108    ///
109    /// This is the balance point of the object. For symmetric shapes, it's typically
110    /// at the geometric center. All angular inertia calculations are relative to this point.
111    pub local_com: Vector,
112
113    /// The inverse of the mass (1 / mass).
114    ///
115    /// - **Positive value**: Normal object with finite mass
116    /// - **Zero**: Infinite mass (immovable/static object)
117    ///
118    /// To get the actual mass, use `mass()` method or compute `1.0 / inv_mass`.
119    pub inv_mass: Real,
120
121    /// The inverse of the principal angular inertia values.
122    ///
123    /// These are the angular inertia values along the principal inertia axes:
124    /// - **2D**: Single scalar value (rotation around perpendicular axis)
125    /// - **3D**: Vector of three values (rotation around X, Y, Z principal axes)
126    ///
127    /// Angular inertia relative to the center of mass (`local_com`).
128    /// Zero components indicate infinite inertia (no rotation) along that axis.
129    pub inv_principal_inertia: AngVector,
130
131    #[cfg(feature = "dim3")]
132    /// The rotation from local coordinates to principal inertia axes (3D only).
133    ///
134    /// This rotation aligns the object's coordinate system with its principal
135    /// axes of inertia, where the inertia tensor is diagonal.
136    pub principal_inertia_local_frame: Rotation,
137}
138
139impl MassProperties {
140    /// Creates mass properties from center of mass, mass, and angular inertia (2D).
141    ///
142    /// # Arguments
143    ///
144    /// * `local_com` - Center of mass in local coordinates
145    /// * `mass` - The mass (positive value, use 0.0 for infinite mass)
146    /// * `principal_inertia` - Angular inertia around the perpendicular axis
147    ///
148    /// # Example (2D)
149    ///
150    /// ```
151    /// # #[cfg(all(feature = "dim2", feature = "f32"))] {
152    /// use parry2d::mass_properties::MassProperties;
153    /// use parry2d::math::Vector;
154    ///
155    /// // Create mass properties for a 10kg object
156    /// let props = MassProperties::new(
157    ///     Vector::ZERO,     // Centered at origin
158    ///     10.0,              // 10kg mass
159    ///     5.0                // Angular inertia
160    /// );
161    ///
162    /// assert_eq!(props.mass(), 10.0);
163    /// assert_eq!(props.inv_mass, 0.1);  // 1/10
164    /// # }
165    /// ```
166    #[cfg(feature = "dim2")]
167    pub fn new(local_com: Vector, mass: Real, principal_inertia: Real) -> Self {
168        let inv_mass = utils::inv(mass);
169        let inv_principal_inertia = utils::inv(principal_inertia);
170        Self {
171            local_com,
172            inv_mass,
173            inv_principal_inertia,
174        }
175    }
176
177    /// Initializes the mass properties from the given center-of-mass, mass, and principal angular inertia.
178    ///
179    /// The center-of-mass is specified in the local-space of the rigid-body.
180    /// The principal angular inertia are the angular inertia along the coordinate axes in the local-space
181    /// of the rigid-body.
182    #[cfg(feature = "dim3")]
183    pub fn new(local_com: Vector, mass: Real, principal_inertia: AngVector) -> Self {
184        Self::with_principal_inertia_frame(local_com, mass, principal_inertia, Rotation::IDENTITY)
185    }
186
187    /// Initializes the mass properties from the given center-of-mass, mass, and principal angular inertia.
188    ///
189    /// The center-of-mass is specified in the local-space of the rigid-body.
190    /// The principal angular inertia are the angular inertia along the coordinate axes defined by
191    /// the `principal_inertia_local_frame` expressed in the local-space of the rigid-body.
192    #[cfg(feature = "dim3")]
193    pub fn with_principal_inertia_frame(
194        local_com: Vector,
195        mass: Real,
196        principal_inertia: AngVector,
197        principal_inertia_local_frame: Rotation,
198    ) -> Self {
199        let inv_mass = utils::inv(mass);
200        let inv_principal_inertia = principal_inertia.map(utils::inv);
201        Self {
202            local_com,
203            inv_mass,
204            inv_principal_inertia,
205            principal_inertia_local_frame,
206        }
207    }
208
209    /// Initialize a new `MassProperties` from a given center-of-mass, mass, and angular inertia matrix.
210    ///
211    /// The angular inertia matrix will be diagonalized in order to extract the principal inertia
212    /// values and principal inertia frame.
213    #[cfg(feature = "dim3")]
214    pub fn with_inertia_matrix(local_com: Vector, mass: Real, inertia: Matrix) -> Self {
215        let mut eigen = inertia.symmetric_eigen();
216        if eigen.eigenvectors.determinant() < 0.0 {
217            eigen.eigenvectors.swap_cols(1, 2);
218            eigen.eigenvalues.as_mut().swap(1, 2);
219        }
220        let eigenvectors = eigen.eigenvectors;
221        let principal_inertia_local_frame = Rotation::from_mat3(&eigenvectors).normalize();
222
223        // Drop negative eigenvalues.
224        let principal_inertia = eigen.eigenvalues.map(|e| e.max(0.0));
225
226        Self::with_principal_inertia_frame(
227            local_com,
228            mass,
229            principal_inertia,
230            principal_inertia_local_frame,
231        )
232    }
233
234    /// The mass.
235    pub fn mass(&self) -> Real {
236        utils::inv(self.inv_mass)
237    }
238
239    /// The angular inertia along the principal inertia axes and center of mass of the rigid-body.
240    pub fn principal_inertia(&self) -> AngVector {
241        #[cfg(feature = "dim2")]
242        return utils::inv(self.inv_principal_inertia);
243        #[cfg(feature = "dim3")]
244        return self.inv_principal_inertia.map(utils::inv);
245    }
246
247    /// The world-space center of mass of the rigid-body.
248    pub fn world_com(&self, pos: &Pose) -> Vector {
249        pos * self.local_com
250    }
251
252    #[cfg(feature = "dim2")]
253    /// The world-space inverse angular inertia tensor of the rigid-body.
254    pub fn world_inv_inertia(&self, _rot: &Rotation) -> AngularInertia {
255        self.inv_principal_inertia
256    }
257
258    #[cfg(feature = "dim3")]
259    /// The world-space inverse angular inertia tensor of the rigid-body.
260    pub fn world_inv_inertia(&self, rot: &Rotation) -> AngularInertia {
261        if self.inv_principal_inertia != Vector::ZERO {
262            let mut lhs = Matrix::from_quat(rot * self.principal_inertia_local_frame);
263            let rhs = lhs.transpose();
264            lhs.x_axis *= self.inv_principal_inertia.x;
265            lhs.y_axis *= self.inv_principal_inertia.y;
266            lhs.z_axis *= self.inv_principal_inertia.z;
267            let inertia = lhs * rhs;
268            AngularInertia::from_sdp_matrix(inertia)
269        } else {
270            AngularInertia::zero()
271        }
272    }
273
274    #[cfg(feature = "dim3")]
275    /// Reconstructs the inverse angular inertia tensor of the rigid body from its principal inertia values and axes.
276    pub fn reconstruct_inverse_inertia_matrix(&self) -> Matrix {
277        let inv_principal_inertia = self.inv_principal_inertia;
278        let rot_mat = Matrix::from_quat(self.principal_inertia_local_frame);
279        let inv_rot_mat = Matrix::from_quat(self.principal_inertia_local_frame.inverse());
280
281        rot_mat * Matrix::from_diagonal(inv_principal_inertia) * inv_rot_mat
282    }
283
284    #[cfg(feature = "dim3")]
285    /// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axes.
286    pub fn reconstruct_inertia_matrix(&self) -> Matrix {
287        let principal_inertia = self.inv_principal_inertia.map(utils::inv);
288        let rot_mat = Matrix::from_quat(self.principal_inertia_local_frame);
289        let inv_rot_mat = Matrix::from_quat(self.principal_inertia_local_frame.inverse());
290        rot_mat * Matrix::from_diagonal(principal_inertia) * inv_rot_mat
291    }
292
293    #[cfg(feature = "dim2")]
294    pub(crate) fn construct_shifted_inertia_matrix(&self, shift: Vector) -> Real {
295        let i = utils::inv(self.inv_principal_inertia);
296
297        if self.inv_mass != 0.0 {
298            let mass = 1.0 / self.inv_mass;
299            i + shift.length_squared() * mass
300        } else {
301            i
302        }
303    }
304
305    #[cfg(feature = "dim3")]
306    pub(crate) fn construct_shifted_inertia_matrix(&self, shift: Vector) -> Matrix {
307        let matrix = self.reconstruct_inertia_matrix();
308
309        if self.inv_mass != 0.0 {
310            let mass = 1.0 / self.inv_mass;
311            let diag = shift.length_squared();
312            let diagm = Matrix::from_diagonal(Vector::splat(diag));
313            matrix + (diagm - shift.kronecker(shift)) * mass
314        } else {
315            matrix
316        }
317    }
318
319    /// Transform each element of the mass properties.
320    pub fn transform_by(&self, m: &Pose) -> Self {
321        // NOTE: we don't apply the parallel axis theorem here
322        // because the center of mass is also transformed.
323        Self {
324            local_com: m * self.local_com,
325            inv_mass: self.inv_mass,
326            inv_principal_inertia: self.inv_principal_inertia,
327            #[cfg(feature = "dim3")]
328            principal_inertia_local_frame: m.rotation * self.principal_inertia_local_frame,
329        }
330    }
331
332    /// Changes the mass on these mass-properties.
333    ///
334    /// The `adjust_angular_inertia` argument should always be `true`, unless
335    /// there are some specific reasons not to do so. Setting this to `true`
336    /// will automatically adjust the angular inertia of `self` to account
337    /// for the mass change (i.e. it will multiply the angular inertia by
338    /// `new_mass / prev_mass`). Setting it to `false` will not change the
339    /// current angular inertia.
340    pub fn set_mass(&mut self, new_mass: Real, adjust_angular_inertia: bool) {
341        let new_inv_mass = utils::inv(new_mass);
342
343        if adjust_angular_inertia {
344            let curr_mass = utils::inv(self.inv_mass);
345            self.inv_principal_inertia *= new_inv_mass * curr_mass;
346        }
347
348        self.inv_mass = new_inv_mass;
349    }
350}
351
352impl Zero for MassProperties {
353    fn zero() -> Self {
354        Self {
355            inv_mass: 0.0,
356            #[cfg(feature = "dim2")]
357            inv_principal_inertia: 0.0,
358            #[cfg(feature = "dim3")]
359            inv_principal_inertia: Vector::ZERO,
360            #[cfg(feature = "dim3")]
361            principal_inertia_local_frame: Rotation::IDENTITY,
362            local_com: Vector::ZERO,
363        }
364    }
365
366    fn is_zero(&self) -> bool {
367        *self == Self::zero()
368    }
369}
370
371impl Sub<MassProperties> for MassProperties {
372    type Output = Self;
373
374    #[cfg(feature = "dim2")]
375    fn sub(self, other: MassProperties) -> Self {
376        if self.is_zero() || other.is_zero() {
377            return self;
378        }
379
380        let m1 = utils::inv(self.inv_mass);
381        let m2 = utils::inv(other.inv_mass);
382
383        let mut new_mass = m1 - m2;
384
385        if new_mass < EPSILON {
386            // Account for small numerical errors.
387            new_mass = 0.0;
388        }
389
390        let inv_mass = utils::inv(new_mass);
391
392        let local_com = (self.local_com * m1 - other.local_com * m2) * inv_mass;
393        let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com);
394        let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com);
395        let mut inertia = i1 - i2;
396
397        if inertia < EPSILON {
398            // Account for small numerical errors.
399            inertia = 0.0;
400        }
401
402        // NOTE: we drop the negative eigenvalues that may result from subtraction rounding errors.
403        let inv_principal_inertia = utils::inv(inertia);
404
405        Self {
406            local_com,
407            inv_mass,
408            inv_principal_inertia,
409        }
410    }
411
412    #[cfg(feature = "dim3")]
413    fn sub(self, other: MassProperties) -> Self {
414        if self.is_zero() || other.is_zero() {
415            return self;
416        }
417
418        let m1 = utils::inv(self.inv_mass);
419        let m2 = utils::inv(other.inv_mass);
420        let mut new_mass = m1 - m2;
421
422        if new_mass < EPSILON {
423            new_mass = 0.0;
424        }
425
426        let inv_mass = utils::inv(new_mass);
427        let local_com = (self.local_com * m1 - other.local_com * m2) * inv_mass;
428        let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com);
429        let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com);
430        let inertia = i1 - i2;
431        Self::with_inertia_matrix(local_com, new_mass, inertia)
432    }
433}
434
435impl SubAssign<MassProperties> for MassProperties {
436    fn sub_assign(&mut self, rhs: MassProperties) {
437        *self = *self - rhs
438    }
439}
440
441impl Add<MassProperties> for MassProperties {
442    type Output = Self;
443
444    #[cfg(feature = "dim2")]
445    fn add(self, other: MassProperties) -> Self {
446        if self.is_zero() {
447            return other;
448        } else if other.is_zero() {
449            return self;
450        }
451
452        let m1 = utils::inv(self.inv_mass);
453        let m2 = utils::inv(other.inv_mass);
454        let inv_mass = utils::inv(m1 + m2);
455        let local_com = (self.local_com * m1 + other.local_com * m2) * inv_mass;
456        let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com);
457        let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com);
458        let inertia = i1 + i2;
459        let inv_principal_inertia = utils::inv(inertia);
460
461        Self {
462            local_com,
463            inv_mass,
464            inv_principal_inertia,
465        }
466    }
467
468    #[cfg(feature = "dim3")]
469    fn add(self, other: MassProperties) -> Self {
470        if self.is_zero() {
471            return other;
472        } else if other.is_zero() {
473            return self;
474        }
475
476        let m1 = utils::inv(self.inv_mass);
477        let m2 = utils::inv(other.inv_mass);
478        let inv_mass = utils::inv(m1 + m2);
479        let local_com = (self.local_com * m1 + other.local_com * m2) * inv_mass;
480        let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com);
481        let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com);
482        let inertia = i1 + i2;
483
484        Self::with_inertia_matrix(local_com, m1 + m2, inertia)
485    }
486}
487
488impl AddAssign<MassProperties> for MassProperties {
489    fn add_assign(&mut self, rhs: MassProperties) {
490        *self = *self + rhs
491    }
492}
493
494#[cfg(feature = "alloc")]
495impl core::iter::Sum<MassProperties> for MassProperties {
496    #[cfg(feature = "dim2")]
497    fn sum<I>(iter: I) -> Self
498    where
499        I: Iterator<Item = Self>,
500    {
501        use alloc::vec::Vec;
502
503        let mut total_mass = 0.0;
504        let mut total_com = Vector::ZERO;
505        let mut total_inertia = 0.0;
506        // TODO: avoid this allocation.
507        // This is needed because we iterate twice.
508        let mut all_props = Vec::new();
509
510        for props in iter {
511            let mass = utils::inv(props.inv_mass);
512            total_mass += mass;
513            total_com += props.local_com * mass;
514            all_props.push(props);
515        }
516
517        if total_mass > 0.0 {
518            total_com /= total_mass;
519        }
520
521        for props in all_props {
522            total_inertia += props.construct_shifted_inertia_matrix(total_com - props.local_com);
523        }
524
525        Self {
526            local_com: total_com,
527            inv_mass: utils::inv(total_mass),
528            inv_principal_inertia: utils::inv(total_inertia),
529        }
530    }
531
532    #[cfg(feature = "dim3")]
533    fn sum<I>(iter: I) -> Self
534    where
535        I: Iterator<Item = Self>,
536    {
537        use alloc::vec::Vec;
538
539        let mut total_mass = 0.0;
540        let mut total_com = Vector::ZERO;
541        let mut total_inertia = Matrix::ZERO;
542        // TODO: avoid this allocation.
543        // This is needed because we iterate twice.
544        let mut all_props = Vec::new();
545
546        for props in iter {
547            let mass = utils::inv(props.inv_mass);
548            total_mass += mass;
549            total_com += props.local_com * mass;
550            all_props.push(props);
551        }
552
553        if total_mass > 0.0 {
554            total_com /= total_mass;
555        }
556
557        let total_com = total_com;
558        for props in all_props {
559            total_inertia += props.construct_shifted_inertia_matrix(total_com - props.local_com);
560        }
561
562        Self::with_inertia_matrix(total_com, total_mass, total_inertia)
563    }
564}
565
566impl approx::AbsDiffEq for MassProperties {
567    type Epsilon = Real;
568    fn default_epsilon() -> Self::Epsilon {
569        Real::default_epsilon()
570    }
571
572    fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
573        #[cfg(feature = "dim2")]
574        let inertia_is_ok = self
575            .inv_principal_inertia
576            .abs_diff_eq(&other.inv_principal_inertia, epsilon);
577
578        #[cfg(feature = "dim3")]
579        let inertia_is_ok = self
580            .reconstruct_inverse_inertia_matrix()
581            .abs_diff_eq(other.reconstruct_inverse_inertia_matrix(), epsilon);
582
583        inertia_is_ok
584            && self.local_com.abs_diff_eq(other.local_com, epsilon)
585            && self.inv_mass.abs_diff_eq(&other.inv_mass, epsilon)
586    }
587}
588
589impl approx::RelativeEq for MassProperties {
590    fn default_max_relative() -> Self::Epsilon {
591        Real::default_max_relative()
592    }
593
594    fn relative_eq(
595        &self,
596        other: &Self,
597        epsilon: Self::Epsilon,
598        max_relative: Self::Epsilon,
599    ) -> bool {
600        #[cfg(feature = "dim2")]
601        let inertia_is_ok = self.inv_principal_inertia.relative_eq(
602            &other.inv_principal_inertia,
603            epsilon,
604            max_relative,
605        );
606
607        // Compare either the inertia matrix or its inverse, whichever is most precise.
608        #[cfg(feature = "dim3")]
609        let inertia_is_ok = self.reconstruct_inverse_inertia_matrix().relative_eq(
610            &other.reconstruct_inverse_inertia_matrix(),
611            epsilon,
612            max_relative,
613        ) || self.reconstruct_inertia_matrix().relative_eq(
614            &other.reconstruct_inertia_matrix(),
615            epsilon,
616            max_relative,
617        );
618
619        inertia_is_ok
620            && self
621                .local_com
622                .relative_eq(&other.local_com, epsilon, max_relative)
623            && self
624                .inv_mass
625                .relative_eq(&other.inv_mass, epsilon, max_relative)
626    }
627}
628
629#[cfg(test)]
630mod test {
631    use super::MassProperties;
632    #[cfg(feature = "dim3")]
633    use crate::math::Rotation;
634    use crate::math::{AngVector, Vector};
635    use crate::shape::{Ball, Capsule, Shape};
636    use approx::assert_relative_eq;
637    use num::Zero;
638
639    #[test]
640    fn mass_properties_add_partial_zero() {
641        let m1 = MassProperties {
642            local_com: Vector::ZERO,
643            inv_mass: 2.0,
644            #[cfg(feature = "dim2")]
645            inv_principal_inertia: 0.0,
646            #[cfg(feature = "dim3")]
647            inv_principal_inertia: Vector::ZERO,
648            #[cfg(feature = "dim3")]
649            principal_inertia_local_frame: Rotation::IDENTITY,
650        };
651        let m2 = MassProperties {
652            local_com: Vector::ZERO,
653            inv_mass: 0.0,
654            #[cfg(feature = "dim2")]
655            inv_principal_inertia: 1.0,
656            #[cfg(feature = "dim3")]
657            inv_principal_inertia: Vector::new(3.0, 2.0, 1.0),
658            #[cfg(feature = "dim3")]
659            principal_inertia_local_frame: Rotation::IDENTITY,
660        };
661        let result = MassProperties {
662            local_com: Vector::ZERO,
663            inv_mass: 2.0,
664            #[cfg(feature = "dim2")]
665            inv_principal_inertia: 1.0,
666            #[cfg(feature = "dim3")]
667            inv_principal_inertia: Vector::new(3.0, 2.0, 1.0),
668            #[cfg(feature = "dim3")]
669            // TODO: ideally this should be IDENTITY, but glam’s conversion from matrix
670            //       to quaternion returns this instead. This is OK for the cube test
671            //       due to its symmetry though this needs a closer look.
672            principal_inertia_local_frame: Rotation::from_xyzw(1.0, 0.0, 0.0, 0.0),
673        };
674
675        assert_eq!(m1 + m2, result);
676        assert_eq!(m2 + m1, result);
677    }
678
679    #[test]
680    fn mass_properties_add_sub() {
681        // Check that addition and subtraction of mass properties behave as expected.
682        let c1 = Capsule::new_x(1.0, 2.0);
683        let c2 = Capsule::new_y(3.0, 4.0);
684        let c3 = Ball::new(2.0);
685
686        let m1 = c1.mass_properties(1.0);
687        let m2 = c2.mass_properties(1.0);
688        let m3 = c3.mass_properties(1.0);
689        let m1m2m3 = m1 + m2 + m3;
690
691        assert_relative_eq!(m1 + m2, m2 + m1, epsilon = 1.0e-6);
692        assert_relative_eq!(m1m2m3 - m1, m2 + m3, epsilon = 1.0e-6);
693        assert_relative_eq!(m1m2m3 - m2, m1 + m3, epsilon = 1.0e-6);
694        assert_relative_eq!(m1m2m3 - m3, m1 + m2, epsilon = 1.0e-6);
695        assert_relative_eq!(m1m2m3 - (m1 + m2), m3, epsilon = 1.0e-5);
696        assert_relative_eq!(m1m2m3 - (m1 + m3), m2, epsilon = 1.0e-6);
697        assert_relative_eq!(m1m2m3 - (m2 + m3), m1, epsilon = 1.0e-6);
698        assert_relative_eq!(m1m2m3 - m1 - m2, m3, epsilon = 1.0e-5);
699        assert_relative_eq!(m1m2m3 - m1 - m3, m2, epsilon = 1.0e-6);
700        assert_relative_eq!(m1m2m3 - m2 - m3, m1, epsilon = 1.0e-6);
701        assert_relative_eq!(m1m2m3 - m2 - m3, m1, epsilon = 1.0e-6);
702
703        // NOTE: converting the inverse inertia matrices don't work well here because
704        //       tiny inertia value originating from the subtraction can result in a non-zero
705        //       (but large) inverse.
706        assert_relative_eq!(
707            (((m1m2m3 - m1) - m2) - m3).principal_inertia(),
708            AngVector::default(),
709            epsilon = 1.0e-2
710        );
711        assert_relative_eq!((((m1m2m3 - m1) - m2) - m3).mass(), 0.0, epsilon = 1.0e-6);
712    }
713
714    #[test]
715    #[cfg(feature = "alloc")]
716    fn mass_properties_compound() {
717        use crate::{
718            math::{Pose, Vector},
719            shape::{Compound, Cuboid, SharedShape},
720        };
721
722        // Compute the mass properties of a compound shape made of three 1x1x1 cuboids.
723        let shape = Cuboid::new(Vector::splat(0.5));
724        let mp = shape.mass_properties(1.0);
725        let iso2 = Pose::from_parts(Vector::Y.into(), Default::default());
726        let iso3 = Pose::from_parts((-Vector::Y).into(), Default::default());
727
728        // Test sum shifted result through `MassProperties::add`
729        let sum = [mp, mp.transform_by(&iso2), mp.transform_by(&iso3)]
730            .into_iter()
731            .sum::<MassProperties>();
732
733        // Test compound through `MassProperties::from_compound`
734        let compound_shape = Compound::new(vec![
735            (
736                Pose::from_parts(Vector::default().into(), Default::default()),
737                SharedShape::new(shape),
738            ),
739            (iso2, SharedShape::new(shape)),
740            (iso3, SharedShape::new(shape)),
741        ]);
742        let mp_compound = compound_shape.mass_properties(1.0);
743
744        // Check that the mass properties of the compound shape match the mass properties
745        // of a single 1x3x1 cuboid.
746        #[cfg(feature = "dim2")]
747        let expected = Cuboid::new(Vector::new(1.5, 0.5)).mass_properties(1.0);
748        #[cfg(feature = "dim3")]
749        let expected = Cuboid::new(Vector::new(1.5, 0.5, 0.5)).mass_properties(1.0);
750
751        // Sum shifted
752        assert_relative_eq!(sum.local_com, expected.local_com, epsilon = 1.0e-6);
753        assert_relative_eq!(sum.inv_mass, expected.inv_mass, epsilon = 1.0e-6);
754        #[cfg(feature = "dim3")]
755        assert!(sum
756            .inv_principal_inertia
757            .abs_diff_eq(expected.inv_principal_inertia, 1.0e-6));
758        #[cfg(feature = "dim2")]
759        assert_relative_eq!(
760            sum.inv_principal_inertia,
761            expected.inv_principal_inertia,
762            epsilon = 1.0e-6
763        );
764
765        // Compound
766        assert_relative_eq!(mp_compound.local_com, expected.local_com, epsilon = 1.0e-6);
767        assert_relative_eq!(mp_compound.inv_mass, expected.inv_mass, epsilon = 1.0e-6);
768        #[cfg(feature = "dim3")]
769        assert!(mp_compound
770            .inv_principal_inertia
771            .abs_diff_eq(expected.inv_principal_inertia, 1.0e-6));
772        #[cfg(feature = "dim2")]
773        assert_relative_eq!(
774            mp_compound.inv_principal_inertia,
775            expected.inv_principal_inertia,
776            epsilon = 1.0e-6
777        );
778    }
779
780    #[test]
781    #[cfg(feature = "alloc")]
782    fn mass_properties_sum_no_nan() {
783        let mp: MassProperties = [MassProperties::zero()].iter().copied().sum();
784        assert!(!mp.local_com.x.is_nan() && !mp.local_com.y.is_nan());
785        #[cfg(feature = "dim3")]
786        assert!(!mp.local_com.z.is_nan());
787    }
788}
parry2d/mass_properties/mass_properties.rs

parry2d/mass_properties/
mass_properties.rs
