polyhedral_mass_properties 0.2.2

#![cfg_attr(not(feature = "std"), no_std)]
#![deny(missing_docs)]

//! Calculates the mass properties (mass, center of mass and inertia matrix/tensor) of triangle meshes.
//!
//! The algorithm is based on the paper ["Computing the Moment of Inertia of a Solid Defined by a Triangle Mesh"](https://doi.org/10.1080/2151237X.2006.10129220) by Michael Kallay ([Code](https://github.com/erich666/jgt-code/blob/master/Volume_11/Number_2/Kallay2006/Moment_of_Inertia.cpp)).
//!
//! ## How it works
//! Each triangle in the mesh has its own contribution ([`TriangleContrib`]) to the mass properties of the whole mesh.
//! These contributions can be calculated independently and then summed up (`O(N)` where `N` is the number of triangles in the mesh).
//!
//! The sum can then be converted to the actual mass properties ([`MassProperties`]) with [`MassProperties::from_contrib_sum`] (`O(1)`).
//!
//! ## Examples
//! With `vertices: &[[f64; 3]]` and `indices: &[usize]`:
//!
//! ```
//! # use polyhedral_mass_properties::{MassProperties, TriangleContrib};
//! #
//! # let vertices = [
//! #     // Front
//! #     [0.0, 0.0, 1.0],
//! #     [1.0, 0.0, 1.0],
//! #     [1.0, 1.0, 1.0],
//! #     [0.0, 1.0, 1.0],
//! #     // Back
//! #     [0.0, 1.0, 0.0],
//! #     [1.0, 1.0, 0.0],
//! #     [1.0, 0.0, 0.0],
//! #     [0.0, 0.0, 0.0],
//! #     // Right
//! #     [1.0, 0.0, 0.0],
//! #     [1.0, 1.0, 0.0],
//! #     [1.0, 1.0, 1.0],
//! #     [1.0, 0.0, 1.0],
//! #     // Left
//! #     [0.0, 0.0, 1.0],
//! #     [0.0, 1.0, 1.0],
//! #     [0.0, 1.0, 0.0],
//! #     [0.0, 0.0, 0.0],
//! #     // Top
//! #     [1.0, 1.0, 0.0],
//! #     [0.0, 1.0, 0.0],
//! #     [0.0, 1.0, 1.0],
//! #     [1.0, 1.0, 1.0],
//! #     // Bottom
//! #     [1.0, 0.0, 1.0],
//! #     [0.0, 0.0, 1.0],
//! #     [0.0, 0.0, 0.0],
//! #     [1.0, 0.0, 0.0],
//! # ];
//! #
//! # let indices = [
//! #     0, 1, 2, 2, 3, 0, // front
//! #     4, 5, 6, 6, 7, 4, // back
//! #     8, 9, 10, 10, 11, 8, // right
//! #     12, 13, 14, 14, 15, 12, // left
//! #     16, 17, 18, 18, 19, 16, // top
//! #     20, 21, 22, 22, 23, 20, // bottom
//! # ];
//! #
//! let contribution_sum = indices
//!     .chunks_exact(3)
//!     .map(|indices| {
//!         TriangleContrib::new(
//!             vertices[indices[0]],
//!             vertices[indices[1]],
//!             vertices[indices[2]],
//!         )
//!     })
//!     .sum();
//! let mass_properties = MassProperties::from_contrib_sum(contribution_sum);
//! ```
//!
//! ### Iterator
//! If `indices` is an iterator over `usize` with a known length (e.g. implements [`ExactSizeIterator`]), you can do the following without needing to collect the indices first:
//!
//! ```
//! # use polyhedral_mass_properties::{MassProperties, TriangleContrib};
//! #
//! # let vertices = [
//! #     // Front
//! #     [0.0, 0.0, 1.0],
//! #     [1.0, 0.0, 1.0],
//! #     [1.0, 1.0, 1.0],
//! #     [0.0, 1.0, 1.0],
//! #     // Back
//! #     [0.0, 1.0, 0.0],
//! #     [1.0, 1.0, 0.0],
//! #     [1.0, 0.0, 0.0],
//! #     [0.0, 0.0, 0.0],
//! #     // Right
//! #     [1.0, 0.0, 0.0],
//! #     [1.0, 1.0, 0.0],
//! #     [1.0, 1.0, 1.0],
//! #     [1.0, 0.0, 1.0],
//! #     // Left
//! #     [0.0, 0.0, 1.0],
//! #     [0.0, 1.0, 1.0],
//! #     [0.0, 1.0, 0.0],
//! #     [0.0, 0.0, 0.0],
//! #     // Top
//! #     [1.0, 1.0, 0.0],
//! #     [0.0, 1.0, 0.0],
//! #     [0.0, 1.0, 1.0],
//! #     [1.0, 1.0, 1.0],
//! #     // Bottom
//! #     [1.0, 0.0, 1.0],
//! #     [0.0, 0.0, 1.0],
//! #     [0.0, 0.0, 0.0],
//! #     [1.0, 0.0, 0.0],
//! # ];
//! #
//! # let indices = [
//! #     0, 1, 2, 2, 3, 0, // front
//! #     4, 5, 6, 6, 7, 4, // back
//! #     8, 9, 10, 10, 11, 8, // right
//! #     12, 13, 14, 14, 15, 12, // left
//! #     16, 17, 18, 18, 19, 16, // top
//! #     20, 21, 22, 22, 23, 20, // bottom
//! # ];
//! # let mut indices = indices.into_iter();
//! #
//! let contribution_sum = (0..indices.len() / 3)
//!     .map(|_| {
//!         TriangleContrib::new(
//!             vertices[indices.next().unwrap()],
//!             vertices[indices.next().unwrap()],
//!             vertices[indices.next().unwrap()],
//!         )
//!     })
//!     .sum();
//! let mass_properties = MassProperties::from_contrib_sum(contribution_sum);
//! ```
//!
//! ### Multi-threading with Rayon
//! Calculating the mass properties can be easily multi-threaded by using [Rayon](https://docs.rs/rayon).
//!
//! ```
//! # use polyhedral_mass_properties::{MassProperties, TriangleContrib};
//! #
//! # let vertices = [
//! #     // Front
//! #     [0.0, 0.0, 1.0],
//! #     [1.0, 0.0, 1.0],
//! #     [1.0, 1.0, 1.0],
//! #     [0.0, 1.0, 1.0],
//! #     // Back
//! #     [0.0, 1.0, 0.0],
//! #     [1.0, 1.0, 0.0],
//! #     [1.0, 0.0, 0.0],
//! #     [0.0, 0.0, 0.0],
//! #     // Right
//! #     [1.0, 0.0, 0.0],
//! #     [1.0, 1.0, 0.0],
//! #     [1.0, 1.0, 1.0],
//! #     [1.0, 0.0, 1.0],
//! #     // Left
//! #     [0.0, 0.0, 1.0],
//! #     [0.0, 1.0, 1.0],
//! #     [0.0, 1.0, 0.0],
//! #     [0.0, 0.0, 0.0],
//! #     // Top
//! #     [1.0, 1.0, 0.0],
//! #     [0.0, 1.0, 0.0],
//! #     [0.0, 1.0, 1.0],
//! #     [1.0, 1.0, 1.0],
//! #     // Bottom
//! #     [1.0, 0.0, 1.0],
//! #     [0.0, 0.0, 1.0],
//! #     [0.0, 0.0, 0.0],
//! #     [1.0, 0.0, 0.0],
//! # ];
//! #
//! # let indices = [
//! #     0, 1, 2, 2, 3, 0, // front
//! #     4, 5, 6, 6, 7, 4, // back
//! #     8, 9, 10, 10, 11, 8, // right
//! #     12, 13, 14, 14, 15, 12, // left
//! #     16, 17, 18, 18, 19, 16, // top
//! #     20, 21, 22, 22, 23, 20, // bottom
//! # ];
//! #
//! use rayon::prelude::*;
//!
//! let contribution_sum = indices
//!     .par_chunks_exact(3) // par_chunks_exact instead of chunks_exact
//!     .map(|indices| {
//!         TriangleContrib::new(
//!             vertices[indices[0]],
//!             vertices[indices[1]],
//!             vertices[indices[2]],
//!         )
//!     })
//!     .sum();
//! let mass_properties = MassProperties::from_contrib_sum(contribution_sum);
//! ```
//!
//! <div class="warning">
//! Multi-threading has some overhead and <strong>can even be slower</strong> for small meshes, especially if you can iterate over the indices instead of collecting them first for Rayon.
//! Benchmark the multi-threaded version for your use case first before using it.
//! </div>
//!
//! ## Feature flags
//!
//! - `std`: Enable the usage of the standard library. Enabled by `fma` and `gltf`.
//! - `fma`: Use [FMA instructions](https://en.wikipedia.org/wiki/FMA_instruction_set) for better performance. Needs to be compiled with `RUSTFLAGS="-C target-cpu=native"`.
//! - `serde`: Implement the `Serialize` and `Deserialize` traits for [`Inertia`] and [`MassProperties`].
//! - `gltf`: Enable calculating the mass properties for gltf/glb files using [`MassProperties::from_gltf`].

use core::{iter::Sum, ops::Add};

const INV6: f64 = 1.0 / 6.0;
const INV120: f64 = 1.0 / 120.0;

#[cfg(feature = "gltf")]
#[inline]
fn cast_point(point: [f32; 3]) -> [f64; 3] {
    [point[0] as f64, point[1] as f64, point[2] as f64]
}

/// Error variants for [`MassProperties::from_gltf`].
#[cfg(feature = "gltf")]
#[derive(Debug)]
pub enum GltfErr {
    /// Failed to load the gltf/glb file.
    LoadErr(gltf::Error),
    /// Invalid mesh index.
    InvalidIndex,
    /// One of the primitives isn't a triangle list.
    NotTriangleList,
    /// One of the primitives doesn't have vertices.
    NoVertices,
    /// One of the primitives doesn't have indices.
    NoIndices,
    /// The calculated volume is zero.
    ZeroVolume,
}

#[cfg(feature = "gltf")]
impl std::fmt::Display for GltfErr {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        use GltfErr::*;

        match self {
            LoadErr(_) => f.write_str("Failed to load the gltf/glb file"),
            InvalidIndex => f.write_str("Invalid mesh index"),
            NotTriangleList => f.write_str("One of the primitives isn't a triangle list"),
            NoVertices => f.write_str("One of the primitives doesn't have vertices"),
            NoIndices => f.write_str("One of the primitives doesn't have indices"),
            ZeroVolume => f.write_str("The calculated volume is zero"),
        }
    }
}

#[cfg(feature = "gltf")]
impl std::error::Error for GltfErr {
    fn source(&self) -> Option<&(dyn std::error::Error + 'static)> {
        use GltfErr::*;

        match self {
            LoadErr(e) => Some(e),
            InvalidIndex | NotTriangleList | NoVertices | NoIndices | ZeroVolume => None,
        }
    }
}

/// Entries of the inertia matrix/tensor.
///
/// Since the matrix is symmetric, only 6 entries must be stored.
/// The matrix looks like the following:
/// ```text
/// ┌          ┐
/// │ xx xy xz │
/// │ xy yy yz │
/// │ xz yz zz │
/// └          ┘
/// ```
#[derive(Debug, PartialEq, Clone)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct Inertia {
    /// Entry in first row, first column.
    pub xx: f64,
    /// Entry in second row, second column.
    pub yy: f64,
    /// Entry in third row, third column.
    pub zz: f64,
    /// Entry in first row, second column (and second row, first column).
    pub xy: f64,
    /// Entry in first row, third column (and third row, first column).
    pub xz: f64,
    /// Entry in second row, third column (and third row, second column).
    pub yz: f64,
}

impl Inertia {
    /// Returns the matrix as a two-dimensional array.
    /// Column/Row major order isn't relevant since the matrix is symmetric.
    /// ```text
    /// [
    ///     [xx, xy, xz],
    ///     [xy, yy, yz],
    ///     [xz, yz, zz],
    /// ]
    /// ```
    #[inline]
    pub fn to_cols_array_2d(&self) -> [[f64; 3]; 3] {
        [
            [self.xx, self.xy, self.xz],
            [self.xy, self.yy, self.yz],
            [self.xz, self.yz, self.zz],
        ]
    }
}

/// The calculated mass properties.
///
/// See [the module documentation](crate) for examples.
#[derive(Debug, PartialEq, Clone)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct MassProperties {
    density: f64,
    /// The scalar mass as the product of the volume and density (`m = V * density`).
    pub mass: f64,
    /// The coordinates of the center of mass.
    pub center_of_mass: [f64; 3],
    /// The entries of the inertia matrix/tensor.
    pub inertia: Inertia,
}

/// The contribution of one triangle in the mesh to the mass properties of the whole mesh.
pub struct TriangleContrib {
    mass: f64,
    com_x: f64,
    com_y: f64,
    com_z: f64,
    xx: f64,
    yy: f64,
    zz: f64,
    xy: f64,
    xz: f64,
    yz: f64,
}

impl TriangleContrib {
    /// The zero element of addition for these contributions.
    ///
    /// `ZERO + x = x + ZERO = x` for any `x` with the type `TriangleContrib`.
    pub const ZERO: Self = TriangleContrib {
        mass: 0.0,
        com_x: 0.0,
        com_y: 0.0,
        com_z: 0.0,
        xx: 0.0,
        yy: 0.0,
        zz: 0.0,
        xy: 0.0,
        xz: 0.0,
        yz: 0.0,
    };

    /// Calculate the contribution of one triangle in the mesh using its three vertices.
    /// All faces must be specified in the right-handed order.
    #[inline]
    pub fn new([x1, y1, z1]: [f64; 3], [x2, y2, z2]: [f64; 3], [x3, y3, z3]: [f64; 3]) -> Self {
        #[cfg(not(feature = "fma"))]
        let v = x1 * (y2 * z3 - y3 * z2) + y1 * (z2 * x3 - x2 * z3) + z1 * (x2 * y3 - x3 * y2);
        #[cfg(feature = "fma")]
        let v = x1.mul_add(
            y2.mul_add(z3, -y3 * z2),
            y1.mul_add(z2.mul_add(x3, -x2 * z3), z1 * (x2.mul_add(y3, -x3 * y2))),
        );

        let x_sum = x1 + x2 + x3;
        let y_sum = y1 + y2 + y3;
        let z_sum = z1 + z2 + z3;

        Self {
            mass: v,
            com_x: v * x_sum,
            com_y: v * y_sum,
            com_z: v * z_sum,
            #[cfg(not(feature = "fma"))]
            xx: v * (x1 * x1 + x2 * x2 + x3 * x3 + x_sum * x_sum),
            #[cfg(feature = "fma")]
            xx: v * (x1.mul_add(x1, x2.mul_add(x2, x3.mul_add(x3, x_sum * x_sum)))),
            #[cfg(not(feature = "fma"))]
            yy: v * (y1 * y1 + y2 * y2 + y3 * y3 + y_sum * y_sum),
            #[cfg(feature = "fma")]
            yy: v * (y1.mul_add(y1, y2.mul_add(y2, y3.mul_add(y3, y_sum * y_sum)))),
            #[cfg(not(feature = "fma"))]
            zz: v * (z1 * z1 + z2 * z2 + z3 * z3 + z_sum * z_sum),
            #[cfg(feature = "fma")]
            zz: v * (z1.mul_add(z1, z2.mul_add(z2, z3.mul_add(z3, z_sum * z_sum)))),
            #[cfg(not(feature = "fma"))]
            xy: v * (y1 * x1 + y2 * x2 + y3 * x3 + y_sum * x_sum),
            #[cfg(feature = "fma")]
            xy: v * (y1.mul_add(x1, y2.mul_add(x2, y3.mul_add(x3, y_sum * x_sum)))),
            #[cfg(not(feature = "fma"))]
            xz: v * (z1 * x1 + z2 * x2 + z3 * x3 + z_sum * x_sum),
            #[cfg(feature = "fma")]
            xz: v * (z1.mul_add(x1, z2.mul_add(x2, z3.mul_add(x3, z_sum * x_sum)))),
            #[cfg(not(feature = "fma"))]
            yz: v * (z1 * y1 + z2 * y2 + z3 * y3 + z_sum * y_sum),
            #[cfg(feature = "fma")]
            yz: v * (z1.mul_add(y1, z2.mul_add(y2, z3.mul_add(y3, z_sum * y_sum)))),
        }
    }
}

impl Add for TriangleContrib {
    type Output = Self;

    #[inline]
    fn add(mut self, rhs: Self) -> Self::Output {
        self.mass += rhs.mass;
        self.com_x += rhs.com_x;
        self.com_y += rhs.com_y;
        self.com_z += rhs.com_z;
        self.xx += rhs.xx;
        self.yy += rhs.yy;
        self.zz += rhs.zz;
        self.xy += rhs.xy;
        self.xz += rhs.xz;
        self.yz += rhs.yz;
        self
    }
}

impl Sum for TriangleContrib {
    #[inline]
    fn sum<I: Iterator<Item = TriangleContrib>>(iter: I) -> Self {
        iter.fold(Self::ZERO, |acc, contrib| acc + contrib)
    }
}

impl MassProperties {
    /// Calculate the mass properties from the sum of triangle contributions with the default density `1.0`.
    /// `None` is returned if the volume is zero.
    ///
    /// Use `Self::with_density` to change the density and adapt all other values.
    ///
    /// See [the module documentation](crate) for examples.
    pub fn from_contrib_sum(sum: TriangleContrib) -> Option<Self> {
        // The volume is zero if the mass is zero.
        if sum.mass == 0.0 {
            return None;
        }

        let r = 1.0 / (4.0 * sum.mass);
        let com_x = sum.com_x * r;
        let com_y = sum.com_y * r;
        let com_z = sum.com_z * r;

        let mass = sum.mass * INV6;
        let mass_com_x = mass * com_x;
        let mass_com_y = mass * com_y;

        #[cfg(not(feature = "fma"))]
        let xy = mass_com_x * com_y - sum.xy * INV120;
        #[cfg(feature = "fma")]
        let xy = mass_com_x.mul_add(com_y, -sum.xy * INV120);
        #[cfg(not(feature = "fma"))]
        let xz = mass_com_x * com_z - sum.xz * INV120;
        #[cfg(feature = "fma")]
        let xz = mass_com_x.mul_add(com_z, -sum.xz * INV120);
        #[cfg(not(feature = "fma"))]
        let yz = mass_com_y * com_z - sum.yz * INV120;
        #[cfg(feature = "fma")]
        let yz = mass_com_y.mul_add(com_z, -sum.yz * INV120);

        #[cfg(not(feature = "fma"))]
        let xx = sum.xx * INV120 - mass_com_x * com_x;
        #[cfg(feature = "fma")]
        let xx = sum.xx.mul_add(INV120, -mass_com_x * com_x);
        #[cfg(not(feature = "fma"))]
        let yy = sum.yy * INV120 - mass_com_y * com_y;
        #[cfg(feature = "fma")]
        let yy = sum.yy.mul_add(INV120, -mass_com_y * com_y);
        #[cfg(not(feature = "fma"))]
        let zz = sum.zz * INV120 - mass * com_z * com_z;
        #[cfg(feature = "fma")]
        let zz = sum.zz.mul_add(INV120, -mass * com_z * com_z);

        Some(Self {
            density: 1.0,
            mass,
            center_of_mass: [com_x, com_y, com_z],
            inertia: Inertia {
                xx: yy + zz,
                yy: zz + xx,
                zz: xx + yy,
                xy,
                yz,
                xz,
            },
        })
    }

    /// Calculate the mass properties of the mesh with index `mesh` in the gltf/glb file `path`.
    #[cfg(feature = "gltf")]
    pub fn from_gltf<P: AsRef<std::path::Path>>(path: P, mesh: usize) -> Result<Self, GltfErr> {
        use gltf::mesh::util::ReadIndices;

        let path = path.as_ref();
        let mut gltf = gltf::Gltf::open(path).map_err(GltfErr::LoadErr)?;
        let blob = gltf.blob.take();
        let mesh = gltf.meshes().nth(mesh).ok_or(GltfErr::InvalidIndex)?;

        let buffers =
            gltf::import_buffers(&gltf.document, path.parent(), blob).map_err(GltfErr::LoadErr)?;

        let sum = mesh
            .primitives()
            .try_fold(TriangleContrib::ZERO, |acc, primitive| {
                if primitive.mode() != gltf::mesh::Mode::Triangles {
                    return Err(GltfErr::NotTriangleList);
                }

                let reader = primitive
                    .reader(|buffer| buffers.get(buffer.index()).map(|data| data.0.as_slice()));
                let vertices = reader
                    .read_positions()
                    .ok_or(GltfErr::NoVertices)?
                    .collect::<Vec<_>>();
                let indices = reader.read_indices().ok_or(GltfErr::NoIndices)?;

                macro_rules! props {
                    ($indices:ident) => {
                        (0..$indices.len() / 3)
                            .map(|_| {
                                let i0 = $indices.next().unwrap() as usize;
                                let i1 = $indices.next().unwrap() as usize;
                                let i2 = $indices.next().unwrap() as usize;

                                let p0 = vertices[i0];
                                let p1 = vertices[i1];
                                let p2 = vertices[i2];

                                TriangleContrib::new(cast_point(p0), cast_point(p1), cast_point(p2))
                            })
                            .sum()
                    };
                }
                let sum = match indices {
                    ReadIndices::U8(mut indices) => props!(indices),
                    ReadIndices::U16(mut indices) => props!(indices),
                    ReadIndices::U32(mut indices) => props!(indices),
                };

                Ok(acc + sum)
            })?;

        Self::from_contrib_sum(sum).ok_or(GltfErr::ZeroVolume)
    }

    /// Get the current density (default: `1.0`).
    ///
    /// Use `Self::with_density` to change it and adapt all other values.
    #[inline]
    pub fn density(&self) -> f64 {
        self.density
    }

    /// Update all values with the new density.
    ///
    /// # Panics
    /// In debug mode, panics if `density` is `0`. Otherwise, the values will be `inf`.
    pub fn with_density(mut self, density: f64) -> Self {
        debug_assert_ne!(density, 0.0);

        let ratio = density / self.density;
        self.density = density;
        self.mass *= ratio;
        self.inertia.xx *= ratio;
        self.inertia.yy *= ratio;
        self.inertia.zz *= ratio;
        self.inertia.xy *= ratio;
        self.inertia.xz *= ratio;
        self.inertia.yz *= ratio;
        self
    }

    /// Calculate the volume.
    #[inline]
    pub fn volume(&self) -> f64 {
        self.mass / self.density
    }
}