parry3d/mass_properties/
mass_properties.rs

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

parry3d/mass_properties/
mass_properties.rs
