phys_inertia/

lib.rs

// Copyright (C) 2020-2025 phys-inertia authors. All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

extern crate nalgebra as na;
use math::{is_near_zero, vec3, Isometry3, Mat3, Mat3Ext, Real, UnitQuat, Vec3};
pub use phys_geom::shape;
use phys_geom::shape::{Capsule, Cuboid, Cylinder, InfinitePlane, Sphere, Tetrahedron};
use transformer::InertiaTensorTransformer;

use crate::math::UnitQuatExt;

pub mod math;
pub mod transformer;

/// The inertia properties of a shape.
#[derive(Clone)]
pub struct Inertia {
    /// The mass normalized diagonal inerita with principal axis pass through the center of mass.
    pub diag_inertia: Vec3,
    /// The rotation from principal axis space to shape space. None if no rotation is needed.
    pub inertia_rotation: Option<UnitQuat>,
    /// The center of mass in shape space. None if the center of mass is at the origin.
    pub center_of_mass: Option<Vec3>,
}

impl Default for Inertia {
    fn default() -> Self {
        Inertia {
            diag_inertia: Vec3::repeat(1.0),
            inertia_rotation: None,
            center_of_mass: None,
        }
    }
}

impl Inertia {
    /// Zero valued inertia.
    pub const ZERO: Inertia = Inertia {
        diag_inertia: Vec3::new(0.0, 0.0, 0.0),
        inertia_rotation: None,
        center_of_mass: None,
    };
}

/// Implement this trait to provide the inertia computation ability.
pub trait ComputeInertia {
    /// Compute the inertia properties of an object.
    fn compute_inertia(&self) -> Inertia;
}

impl ComputeInertia for Sphere {
    #[inline]
    fn compute_inertia(&self) -> Inertia {
        Inertia {
            diag_inertia: sphere(self.radius()),
            ..Default::default()
        }
    }
}

impl ComputeInertia for Cuboid {
    #[inline]
    fn compute_inertia(&self) -> Inertia {
        Inertia {
            diag_inertia: cube(self.length()),
            ..Default::default()
        }
    }
}

impl ComputeInertia for Cylinder {
    #[inline]
    fn compute_inertia(&self) -> Inertia {
        Inertia {
            diag_inertia: cylinder(self.radius(), self.height()),
            ..Default::default()
        }
    }
}

impl ComputeInertia for Capsule {
    #[inline]
    fn compute_inertia(&self) -> Inertia {
        Inertia {
            diag_inertia: capsule(self.radius(), self.height()),
            ..Default::default()
        }
    }
}

impl ComputeInertia for InfinitePlane {
    #[inline]
    fn compute_inertia(&self) -> Inertia {
        Inertia::ZERO
    }
}

/// Compute the diagonal inertia of a cube in principal axis space with mass = 1.
#[inline]
pub fn cube(size: impl Into<[Real; 3]>) -> Vec3 {
    let [a, b, c] = size.into();
    const M: Real = 1.0 / 12.0;
    let (a2, b2, c2) = (a * a, b * b, c * c);
    vec3(b2 + c2, a2 + c2, a2 + b2) * M
}

/// Compute the diagonal inertia of a solid sphere in principal axis space with mass = 1.
#[inline]
pub fn sphere(radius: Real) -> Vec3 {
    let a = 2.0 / 5.0 * radius * radius;
    vec3(a, a, a)
}

/// Compute the diagonal inertia of a cylinder in principal axis space with mass = 1.
///
/// The cylinder is assumed to be oriented along the y-axis.
#[inline]
pub fn cylinder(radius: Real, height: Real) -> Vec3 {
    const SA: Real = 1.0 / 12.0;
    const SB: Real = 1.0 / 2.0;

    let a = SA * (3.0 * radius * radius + height * height);
    let b = SB * radius * radius;
    vec3(a, b, a)
}

/// Compute the diagonal inertia of a capsule in principal axis space with mass = 1.
///
/// The capsule is assumed to be oriented along the y-axis.
#[inline]
pub fn capsule(radius: Real, height: Real) -> Vec3 {
    const A: Real = 13.0 / 20.0;
    const B: Real = 7.0 / 12.0;
    const C: Real = 3.0 / 8.0;
    const D: Real = 9.0 / 10.0;

    let radius_sq = radius * radius;
    let height_sq = height * height;
    let a = A * radius_sq + B * height_sq + C * radius * height;
    let b = D * radius_sq;
    vec3(a, b, a)
}

/// Compute the inertia tensor of a tetrahedron with mass = 1.
pub fn tetrahedron(points: [Vec3; 4]) -> Mat3 {
    let [p1, p2, p3, p4] = points;

    // Just for readability.
    let x1 = p1[0];
    let y1 = p1[1];
    let z1 = p1[2];
    let x2 = p2[0];
    let y2 = p2[1];
    let z2 = p2[2];
    let x3 = p3[0];
    let y3 = p3[1];
    let z3 = p3[2];
    let x4 = p4[0];
    let y4 = p4[1];
    let z4 = p4[2];

    let diag_x = x1 * x1
        + x1 * x2
        + x2 * x2
        + x1 * x3
        + x2 * x3
        + x3 * x3
        + x1 * x4
        + x2 * x4
        + x3 * x4
        + x4 * x4;
    let diag_y = y1 * y1
        + y1 * y2
        + y2 * y2
        + y1 * y3
        + y2 * y3
        + y3 * y3
        + y1 * y4
        + y2 * y4
        + y3 * y4
        + y4 * y4;
    let diag_z = z1 * z1
        + z1 * z2
        + z2 * z2
        + z1 * z3
        + z2 * z3
        + z3 * z3
        + z1 * z4
        + z2 * z4
        + z3 * z4
        + z4 * z4;

    let a0 = (diag_y + diag_z) * 0.1;
    let b0 = (diag_z + diag_x) * 0.1;
    let c0 = (diag_x + diag_y) * 0.1;

    let a1 = (y1 * z1 * 2.0
        + y2 * z1
        + y3 * z1
        + y4 * z1
        + y1 * z2
        + y2 * z2 * 2.0
        + y3 * z2
        + y4 * z2
        + y1 * z3
        + y2 * z3
        + y3 * z3 * 2.0
        + y4 * z3
        + y1 * z4
        + y2 * z4
        + y3 * z4
        + y4 * z4 * 2.0)
        * 0.05;
    let b1 = (x1 * z1 * 2.0
        + x2 * z1
        + x3 * z1
        + x4 * z1
        + x1 * z2
        + x2 * z2 * 2.0
        + x3 * z2
        + x4 * z2
        + x1 * z3
        + x2 * z3
        + x3 * z3 * 2.0
        + x4 * z3
        + x1 * z4
        + x2 * z4
        + x3 * z4
        + x4 * z4 * 2.0)
        * 0.05;
    let c1 = (x1 * y1 * 2.0
        + x2 * y1
        + x3 * y1
        + x4 * y1
        + x1 * y2
        + x2 * y2 * 2.0
        + x3 * y2
        + x4 * y2
        + x1 * y3
        + x2 * y3
        + x3 * y3 * 2.0
        + x4 * y3
        + x1 * y4
        + x2 * y4
        + x3 * y4
        + x4 * y4 * 2.0)
        * 0.05;

    Mat3::from_column_slice(&[a0, -b1, -c1, -b1, b0, -a1, -c1, -a1, c0])
}

/// Compute the inertia tensor of a mesh with mass = 1.
pub fn mesh(triangles: impl Iterator<Item = [Vec3; 3]>) -> Inertia {
    let mut vol = 0.0;
    let mut itot = Mat3::zeros();
    let mut com = Vec3::zeros();
    for triangle in triangles {
        let a = Vec3::zeros();
        let [b, c, d] = triangle;
        let tet = Tetrahedron {
            a: a.into(),
            b: b.into(),
            c: c.into(),
            d: d.into(),
        };
        let volume = tet.signed_volume();
        let ipart = tetrahedron([a, b, c, d]);

        com += Vec3::from(tet.center()) * volume;
        itot += ipart * volume;
        vol += volume;
    }

    if vol == 0.0 {
        return Inertia::ZERO;
    }
    let inertia = itot * vol.recip();
    com /= vol;

    let (diag_inertia, inertia_rotation) = inertia.diagonalize();
    Inertia {
        diag_inertia,
        inertia_rotation: if inertia_rotation.is_near_identity() {
            None
        } else {
            inertia_rotation.into()
        },
        center_of_mass: if is_near_zero(com, 1e-5) {
            None
        } else {
            com.into()
        },
    }
}

pub struct CompoundUnit<S> {
    pub object: S,
    pub pose: Isometry3,
    pub mass: Real,
}
/// Calculate the mass and mass-normalized inertia of compound shapes
///
/// # Parameters
///
/// - `iter`: an iterator of compound units, each unit contains a shape, a pose and a mass.
/// - `zero_mass_as_infinite`: a flag to determine the behavior when a shape's mass is 0.0.
///     - if true, when any shape's mass is 0.0, the mass of the compound shape will be 0.0.
///     - if false, when any shape's mass is 0.0, it will be ignored.
///
/// # Note
///
/// - We assume all shapes have the same density, so the center of mass and the inertia rotation
///   only depend on the shape's volume and their relative pose.
/// - if the given iterator is empty, the returned inertia will be same as a unit sphere with 1kg
///   mass.
pub fn compound<S>(
    iter: impl Iterator<Item = CompoundUnit<S>>,
    zero_mass_as_infinite: bool,
) -> (Inertia, Real)
where
    S: ComputeInertia,
{
    let mut compound_computer = InertiaTensorTransformer::new(Vec3::zeros(), Mat3::zeros(), 0.0);

    let mut has_shape = false;
    for CompoundUnit {
        object: entity,
        pose,
        mass,
    } in iter
    {
        has_shape = true;
        if mass == 0.0 {
            if zero_mass_as_infinite {
                return (Inertia::ZERO, 0.0);
            } else {
                continue;
            }
        }
        let inertia = entity.compute_inertia();

        // we assume all shapes' density is 1.0.
        let inertia_matrix = Mat3::from_diagonal(&(inertia.diag_inertia * mass));
        let mut shape_computer = InertiaTensorTransformer::new(Vec3::zeros(), inertia_matrix, mass);
        if let Some(rotation) = inertia.inertia_rotation {
            shape_computer.rotate(rotation);
        }

        if let Some(translation) = inertia.center_of_mass {
            shape_computer.translate(translation);
        }

        shape_computer.transform(&pose);
        compound_computer.add(&shape_computer);
    }
    if !has_shape {
        return (
            Inertia {
                diag_inertia: sphere(0.5),
                ..Default::default()
            },
            1.0,
        );
    }

    let compound_com = compound_computer.center_of_mass();
    compound_computer.center();
    let (diag, rot) = compound_computer.inertia().diagonalize();
    let mass = compound_computer.mass();
    (
        Inertia {
            diag_inertia: diag / compound_computer.mass(),
            inertia_rotation: if rot.is_near_identity() {
                None
            } else {
                Some(rot)
            },
            center_of_mass: if is_near_zero(compound_com, 1e-6) {
                None
            } else {
                Some(compound_com)
            },
        },
        mass,
    )
}

/// Given the low triangle part of a symmetric inertia matrix, which is column-major, then
/// diagonalize it.
///
/// # Returns
///
/// The diagonalized inertia matrix and the rotation.
#[inline]
pub fn diagonalize_inertia(inertia_matrix: [math::Real; 6]) -> (Vec3, UnitQuat) {
    let inertia = Mat3::from_column_slice(&[
        inertia_matrix[0],
        inertia_matrix[1],
        inertia_matrix[2],
        inertia_matrix[1],
        inertia_matrix[3],
        inertia_matrix[4],
        inertia_matrix[2],
        inertia_matrix[4],
        inertia_matrix[5],
    ]);
    inertia.diagonalize()
}

#[cfg(test)]
mod tests {
    use approx::assert_relative_eq;

    use super::*;

    #[test]
    fn test_cylinder() {
        let cylinder = Cylinder::new(1.0, 1.0);
        let inertia = cylinder.compute_inertia();

        assert_relative_eq!(inertia.diag_inertia, Vec3::new(7.0 / 12.0, 0.5, 7.0 / 12.0));
        assert_eq!(inertia.center_of_mass, None);
        assert_eq!(inertia.inertia_rotation, None);
    }

    #[test]
    fn test_capsule() {
        let capsule = Capsule::new(0.5, 0.5);
        let inertia = capsule.compute_inertia();

        assert_relative_eq!(inertia.diag_inertia, Vec3::new(0.9333333, 0.225, 0.9333333));
        assert_eq!(inertia.center_of_mass, None);
        assert_eq!(inertia.inertia_rotation, None);
    }

    #[test]
    fn test_cuboid() {
        let cuboid = Cuboid::new([1.0, 1.0, 1.0]);
        let inertia = cuboid.compute_inertia();

        assert_relative_eq!(
            inertia.diag_inertia,
            Vec3::new(1.0 / 6.0, 1.0 / 6.0, 1.0 / 6.0)
        );
        assert_eq!(inertia.center_of_mass, None);
        assert_eq!(inertia.inertia_rotation, None);
    }

    #[test]
    fn test_sphere() {
        let sphere = Sphere::new(1.0);
        let inertia = sphere.compute_inertia();

        assert_relative_eq!(
            inertia.diag_inertia,
            Vec3::new(2.0 / 5.0, 2.0 / 5.0, 2.0 / 5.0)
        );
        assert_eq!(inertia.center_of_mass, None);
        assert_eq!(inertia.inertia_rotation, None);
    }

    #[test]
    fn test_infinite_plane() {
        let plane = InfinitePlane::default();
        let inertia = plane.compute_inertia();
        assert_relative_eq!(inertia.diag_inertia, Inertia::ZERO.diag_inertia);
        assert_eq!(inertia.center_of_mass, None);
        assert_eq!(inertia.inertia_rotation, None);
    }

    #[test]
    fn test_tetrahedron() {
        let inertia = super::tetrahedron([
            vec3(0.0, 0.0, 0.0),
            vec3(1.0, 0.0, 0.0),
            vec3(0.0, 1.0, 0.0),
            vec3(0.0, 0.0, 1.0),
        ]);
        assert_relative_eq!(
            inertia,
            Mat3::from_column_slice(&[0.2, -0.05, -0.05, -0.05, 0.2, -0.05, -0.05, -0.05, 0.2])
        );

        let inertia = super::tetrahedron([
            vec3(1.0, 1.0, 1.0),
            vec3(-1.0, -1.0, 1.0),
            vec3(-1.0, 1.0, -1.0),
            vec3(1.0, -1.0, -1.0),
        ]);

        assert_relative_eq!(inertia, Mat3::from_diagonal(&vec3(0.4, 0.4, 0.4)));
    }

    #[test]
    fn test_mesh() {
        let com = vec3(0.5, 0.5, 0.5);
        // a cube with side length 1.0
        // ```text
        // 7-------6
        // |\      |\
        // | 3-----|-2
        // 4-|-----5 |
        //  \|      \|
        //   0-------1
        // ```
        let cube_corners = [
            vec3(0.0, 0.0, 0.0) - com,
            vec3(1.0, 0.0, 0.0) - com,
            vec3(1.0, 1.0, 0.0) - com,
            vec3(0.0, 1.0, 0.0) - com,
            vec3(0.0, 0.0, 1.0) - com,
            vec3(1.0, 0.0, 1.0) - com,
            vec3(1.0, 1.0, 1.0) - com,
            vec3(0.0, 1.0, 1.0) - com,
        ];
        let indices = [
            [0, 1, 2],
            [0, 2, 3],
            [4, 6, 5],
            [4, 7, 6],
            [0, 4, 1],
            [1, 4, 5],
            [2, 6, 3],
            [3, 6, 7],
            [0, 3, 7],
            [0, 7, 4],
            [5, 6, 2],
            [5, 2, 1],
        ];
        let triangles = indices
            .iter()
            .map(|[a, b, c]| [cube_corners[*a], cube_corners[*b], cube_corners[*c]]);

        let inertia = super::mesh(triangles);

        let expected = Cuboid::UNIT.compute_inertia();

        assert_relative_eq!(inertia.diag_inertia, expected.diag_inertia);
        assert_eq!(inertia.center_of_mass, None);
        assert_eq!(inertia.inertia_rotation, None);
    }

    #[test]
    fn test_zero_volume_mesh() {
        let triangles = vec![[
            vec3(0.0, 0.0, 0.0),
            vec3(1.0, 0.0, 0.0),
            vec3(0.0, 1.0, 0.0),
        ]];
        let inertia = super::mesh(triangles.into_iter());
        assert_relative_eq!(inertia.diag_inertia, Vec3::zeros());
    }

    mod compounds {
        use approx::assert_relative_eq;
        use phys_geom::shape::{Cuboid, Sphere};

        use super::Real;
        use crate::math::{vec3, Isometry3, UnitQuat, Vec3};
        use crate::{CompoundUnit, ComputeInertia, Inertia};

        struct TransformedShapeRefs<'a, S> {
            shapes: &'a [S],
            poses: &'a [Isometry3],
            masses: &'a [Real],
        }

        impl<'a, S: Clone> TransformedShapeRefs<'a, S> {
            #[inline]
            fn iter(&self) -> impl Iterator<Item = CompoundUnit<S>> + '_ {
                let len = self.shapes.len();
                (0..len).map(|index| CompoundUnit {
                    object: self.shapes[index].clone(),
                    pose: self.poses[index],
                    mass: self.masses[index],
                })
            }
        }

        pub(crate) fn compound<S: ComputeInertia + Clone>(
            shapes: &[S],
            poses: &[Isometry3],
            masses: &[Real],
            zero_mass_as_infinite: bool,
        ) -> (Inertia, Real) {
            let shapes = TransformedShapeRefs {
                shapes,
                poses,
                masses,
            };
            crate::compound(shapes.iter(), zero_mass_as_infinite)
        }

        #[test]
        fn test_compound_one_cube() {
            const EPS: Real = 1e-5;

            let shapes = &[Cuboid::new(Vec3::repeat(1.0))];
            let poses: &[Isometry3; 1] = &[Isometry3::identity()];
            let masses = &[1.0];
            let (
                Inertia {
                    diag_inertia,
                    center_of_mass,
                    inertia_rotation,
                },
                mass,
            ) = compound(shapes, poses, masses, true);

            assert_eq!(mass, 1.0);
            assert_eq!(inertia_rotation, None);
            assert_eq!(center_of_mass, None);
            assert_relative_eq!(
                diag_inertia,
                vec3(0.166_666_67, 0.166_666_67, 0.166_666_67),
                epsilon = EPS
            );
        }

        #[test]
        fn test_compound_two_cubes_symmetric() {
            //combine two half cubes to a unit cube.

            let shapes = &[
                Cuboid::new_xyz(0.5, 1.0, 1.0),
                Cuboid::new_xyz(0.5, 1.0, 1.0),
            ];
            let poses = &[
                Isometry3::from_parts(vec3(-0.25, 0., 0.).into(), UnitQuat::identity()),
                Isometry3::from_parts(vec3(0.25, 0., 0.).into(), UnitQuat::identity()),
            ];
            let masses = &[1.0, 1.0];
            let (
                Inertia {
                    diag_inertia,
                    inertia_rotation,
                    center_of_mass,
                },
                mass,
            ) = compound(shapes, poses, masses, true);

            assert_eq!(mass, 2.0);
            assert_relative_eq!(
                diag_inertia,
                crate::cube(vec3(1.0, 1.0, 1.0)),
                epsilon = 1e-5
            );
            assert_eq!(inertia_rotation, None);
            assert_eq!(center_of_mass, None);
        }

        #[test]
        fn test_compound_zero_volume_shape() {
            #[derive(Clone)]
            struct MockInertia {
                pub inertia: Inertia,
            }
            impl ComputeInertia for MockInertia {
                fn compute_inertia(&self) -> Inertia {
                    self.inertia.clone()
                }
            }

            let inertias = &[
                MockInertia {
                    inertia: Cuboid::UNIT.compute_inertia(),
                },
                MockInertia {
                    inertia: Inertia::ZERO,
                },
            ];
            let poses = &[
                Isometry3::from_parts(vec3(-1., 1., 0.).into(), UnitQuat::identity()),
                Isometry3::from_parts(vec3(2., 1., 0.).into(), UnitQuat::identity()),
            ];
            let masses = &[1.0, 0.0];
            let (
                Inertia {
                    diag_inertia,
                    center_of_mass,
                    inertia_rotation,
                },
                mass,
            ) = compound(inertias, poses, masses, true);
            assert_eq!(mass, 0.0);
            assert_eq!(diag_inertia, Vec3::zeros());
            assert_eq!(center_of_mass, None);
            assert_eq!(inertia_rotation, None);

            let (Inertia { diag_inertia, .. }, mass) = compound(inertias, poses, masses, false);
            assert_eq!(mass, 1.0);
            assert_relative_eq!(
                diag_inertia,
                inertias[0].inertia.diag_inertia,
                epsilon = 1e-5
            );
        }

        #[test]
        fn test_compound_empty_shape() {
            let shapes: &[Sphere; 0] = &[];
            let poses = &[];
            let masses = &[];
            let (
                Inertia {
                    diag_inertia,
                    inertia_rotation,
                    center_of_mass,
                },
                mass,
            ) = compound(shapes, poses, masses, false);
            assert_eq!(mass, 1.0);
            assert_eq!(diag_inertia, crate::sphere(0.5));
            assert_eq!(inertia_rotation, None);
            assert_eq!(center_of_mass, None);
        }
    }

    #[test]
    fn test_diagonalize_inertia() {
        let inertia = [1.0, 2.0, 3.0, 4.0, 5.0, 6.0];
        let (diag, rot) = diagonalize_inertia(inertia);
        let inertia_matrix = Mat3::from_column_slice(&[
            inertia[0], inertia[1], inertia[2], inertia[1], inertia[3], inertia[4], inertia[2],
            inertia[4], inertia[5],
        ]);

        let diag_matrix = Mat3::from_diagonal(&diag);
        let rot_matrix = Mat3::from(rot);
        let result = rot_matrix * diag_matrix * rot_matrix.transpose();
        assert_relative_eq!(inertia_matrix, result, epsilon = 1e-5);
    }
}