parry3d/mass_properties/
mass_properties_voxels.rs

1use crate::mass_properties::MassProperties;
2use crate::math::{Point, Real};
3use crate::shape::Voxels;
4
5impl MassProperties {
6    /// Computes the mass properties of a set of voxels.
7    pub fn from_voxels(density: Real, voxels: &Voxels) -> Self {
8        let mut com = Point::origin();
9        let mut num_not_empty = 0;
10        let mut angular_inertia = na::zero();
11        let block_ref_mprops = MassProperties::from_cuboid(density, voxels.voxel_size() / 2.0);
12
13        for vox in voxels.voxels() {
14            if !vox.state.is_empty() {
15                com += vox.center.coords;
16                num_not_empty += 1;
17            }
18        }
19
20        com.coords /= num_not_empty as Real;
21
22        for vox in voxels.voxels() {
23            if !vox.state.is_empty() {
24                angular_inertia +=
25                    block_ref_mprops.construct_shifted_inertia_matrix(vox.center - com);
26            }
27        }
28
29        let mass = block_ref_mprops.mass() * num_not_empty as Real;
30
31        #[cfg(feature = "dim2")]
32        return Self::new(com, mass, angular_inertia);
33        #[cfg(feature = "dim3")]
34        return Self::with_inertia_matrix(com, mass, angular_inertia);
35    }
36}