MassProperties.hpp

Go to the documentation of this file.

// Vulcan Mass Properties
// Full mass properties for rigid body components with aggregation support
#pragma once
 
#include <vulcan/core/VulcanTypes.hpp>
 
#include <janus/janus.hpp>
 
#include <type_traits>
#include <vector>
 
namespace vulcan::mass {

template <typename Scalar> struct MassProperties {
    Scalar mass;     
    Vec3<Scalar> cg; 
    Mat3<Scalar>
        inertia; 
 
    // =========================================================================
    // Factory Constructors
    // =========================================================================

    static MassProperties<Scalar> point_mass(const Scalar &m,
                                             const Vec3<Scalar> &position) {
        return MassProperties<Scalar>{
            .mass = m, .cg = position, .inertia = Mat3<Scalar>::Zero()};
    }

    static MassProperties<Scalar> diagonal(const Scalar &m,
                                           const Vec3<Scalar> &cg_pos,
                                           const Scalar &Ixx, const Scalar &Iyy,
                                           const Scalar &Izz) {
        Mat3<Scalar> I = Mat3<Scalar>::Zero();
        I(0, 0) = Ixx;
        I(1, 1) = Iyy;
        I(2, 2) = Izz;
        return MassProperties<Scalar>{.mass = m, .cg = cg_pos, .inertia = I};
    }

    static MassProperties<Scalar>
    from_components(const Scalar &m, const Vec3<Scalar> &cg_pos,
                    const Scalar &Ixx, const Scalar &Iyy, const Scalar &Izz,
                    const Scalar &Ixy, const Scalar &Ixz, const Scalar &Iyz) {
        Mat3<Scalar> I;
        I(0, 0) = Ixx;
        I(0, 1) = -Ixy;
        I(0, 2) = -Ixz;
        I(1, 0) = -Ixy;
        I(1, 1) = Iyy;
        I(1, 2) = -Iyz;
        I(2, 0) = -Ixz;
        I(2, 1) = -Iyz;
        I(2, 2) = Izz;
        return MassProperties<Scalar>{.mass = m, .cg = cg_pos, .inertia = I};
    }
 
    // =========================================================================
    // Shape Factories
    // =========================================================================

    static MassProperties<Scalar>
    solid_sphere(const Scalar &m, const Scalar &radius,
                 const Vec3<Scalar> &cg_pos = Vec3<Scalar>::Zero()) {
        Scalar I_diag = Scalar(0.4) * m * radius * radius; // 2/5 = 0.4
        return diagonal(m, cg_pos, I_diag, I_diag, I_diag);
    }

    static MassProperties<Scalar>
    solid_cylinder(const Scalar &m, const Scalar &radius, const Scalar &length,
                   const Vec3<Scalar> &cg_pos = Vec3<Scalar>::Zero()) {
        Scalar r2 = radius * radius;
        Scalar L2 = length * length;
        Scalar Ixx = (Scalar(1) / Scalar(12)) * m * (Scalar(3) * r2 + L2);
        Scalar Izz = Scalar(0.5) * m * r2;
        return diagonal(m, cg_pos, Ixx, Ixx, Izz);
    }

    static MassProperties<Scalar>
    solid_box(const Scalar &m, const Scalar &dx, const Scalar &dy,
              const Scalar &dz,
              const Vec3<Scalar> &cg_pos = Vec3<Scalar>::Zero()) {
        Scalar k = Scalar(1) / Scalar(12);
        Scalar Ixx = k * m * (dy * dy + dz * dz);
        Scalar Iyy = k * m * (dx * dx + dz * dz);
        Scalar Izz = k * m * (dx * dx + dy * dy);
        return diagonal(m, cg_pos, Ixx, Iyy, Izz);
    }
 
    // =========================================================================
    // Backward Compatibility Factories
    // =========================================================================

    static MassProperties<Scalar> from_mass(const Scalar &m) {
        // Return with identity-scaled inertia for legacy compatibility
        Mat3<Scalar> I = Mat3<Scalar>::Identity();
        I(0, 0) = m;
        I(1, 1) = m;
        I(2, 2) = m;
        return MassProperties<Scalar>{
            .mass = m, .cg = Vec3<Scalar>::Zero(), .inertia = I};
    }

    static MassProperties<Scalar> diagonal(const Scalar &m, const Scalar &Ixx,
                                           const Scalar &Iyy,
                                           const Scalar &Izz) {
        return diagonal(m, Vec3<Scalar>::Zero(), Ixx, Iyy, Izz);
    }

    static MassProperties<Scalar> full(const Scalar &m, const Scalar &Ixx,
                                       const Scalar &Iyy, const Scalar &Izz,
                                       const Scalar &Ixy, const Scalar &Ixz,
                                       const Scalar &Iyz) {
        return from_components(m, Vec3<Scalar>::Zero(), Ixx, Iyy, Izz, Ixy, Ixz,
                               Iyz);
    }
 
    // =========================================================================
    // Parallel Axis Theorem
    // =========================================================================

    Mat3<Scalar> inertia_about_point(const Vec3<Scalar> &point) const {
        Vec3<Scalar> r = cg - point;
        Scalar r_dot_r = janus::dot(r, r);
 
        // J = I + m * (|r|² * I_3 - r ⊗ r)
        Mat3<Scalar> J = inertia;
        J(0, 0) += mass * (r_dot_r - r(0) * r(0));
        J(1, 1) += mass * (r_dot_r - r(1) * r(1));
        J(2, 2) += mass * (r_dot_r - r(2) * r(2));
        J(0, 1) -= mass * r(0) * r(1);
        J(1, 0) -= mass * r(0) * r(1);
        J(0, 2) -= mass * r(0) * r(2);
        J(2, 0) -= mass * r(0) * r(2);
        J(1, 2) -= mass * r(1) * r(2);
        J(2, 1) -= mass * r(1) * r(2);
 
        return J;
    }
 
    // =========================================================================
    // Aggregation Operators
    // =========================================================================

    MassProperties<Scalar>
    operator+(const MassProperties<Scalar> &other) const {
        Scalar total_mass = mass + other.mass;
 
        // Combined CG (mass-weighted average)
        Vec3<Scalar> combined_cg =
            (mass * cg + other.mass * other.cg) / total_mass;
 
        // Transform both inertias to combined CG and sum
        Mat3<Scalar> I1_at_cg = inertia_about_point(combined_cg);
        Mat3<Scalar> I2_at_cg = other.inertia_about_point(combined_cg);
        Mat3<Scalar> combined_inertia = I1_at_cg + I2_at_cg;
 
        return MassProperties<Scalar>{
            .mass = total_mass, .cg = combined_cg, .inertia = combined_inertia};
    }

    MassProperties<Scalar>
    operator-(const MassProperties<Scalar> &other) const {
        return *this + (Scalar(-1) * other);
    }

    MassProperties<Scalar> operator*(const Scalar &factor) const {
        return MassProperties<Scalar>{
            .mass = mass * factor, .cg = cg, .inertia = inertia * factor};
    }

    MassProperties<Scalar> operator/(const Scalar &factor) const {
        return *this * (Scalar(1) / factor);
    }

    MassProperties<Scalar> &operator+=(const MassProperties<Scalar> &other) {
        *this = *this + other;
        return *this;
    }
 
    // =========================================================================
    // Accessors (for backward compatibility)
    // =========================================================================

    Vec3<Scalar> cg_offset() const { return cg; }
};

template <typename Scalar>
MassProperties<Scalar> operator*(const Scalar &factor,
                                 const MassProperties<Scalar> &props) {
    return props * factor;
}
 
// =============================================================================
// Free Functions
// =============================================================================

template <typename Scalar>
MassProperties<Scalar> aggregate_mass_properties(
    const std::vector<MassProperties<Scalar>> &components) {
    if (components.empty()) {
        return MassProperties<Scalar>{.mass = Scalar(0),
                                      .cg = Vec3<Scalar>::Zero(),
                                      .inertia = Mat3<Scalar>::Zero()};
    }
 
    MassProperties<Scalar> result = components[0];
    for (size_t i = 1; i < components.size(); ++i) {
        result = result + components[i];
    }
    return result;
}

template <typename Scalar>
MassProperties<Scalar>
transform_mass_properties(const MassProperties<Scalar> &props,
                          const Mat3<Scalar> &rotation,
                          const Vec3<Scalar> &translation) {
    // Transform CG position
    Vec3<Scalar> new_cg = rotation * (props.cg - translation);
 
    // Transform inertia tensor: I' = R * I * R^T
    Mat3<Scalar> new_inertia = rotation * props.inertia * rotation.transpose();
 
    return MassProperties<Scalar>{
        .mass = props.mass, .cg = new_cg, .inertia = new_inertia};
}
 
// =============================================================================
// Numeric-Only Functions (require eigenvalue decomposition)
// =============================================================================

template <typename Scalar>
bool is_physically_valid(const MassProperties<Scalar> &props) {
    static_assert(std::is_same_v<Scalar, double>,
                  "is_physically_valid only works with double");
 
    if (props.mass < 0) {
        return false;
    }
 
    // Compute eigenvalues for principal moments
    Eigen::SelfAdjointEigenSolver<Mat3<double>> solver(props.inertia);
    Vec3<double> eigs = solver.eigenvalues();
 
    // All principal moments must be non-negative
    if (eigs(0) < 0 || eigs(1) < 0 || eigs(2) < 0) {
        return false;
    }
 
    // Triangle inequality
    if (eigs(0) + eigs(1) < eigs(2) || eigs(0) + eigs(2) < eigs(1) ||
        eigs(1) + eigs(2) < eigs(0)) {
        return false;
    }
 
    return true;
}

template <typename Scalar>
bool is_point_mass(const MassProperties<Scalar> &props) {
    static_assert(std::is_same_v<Scalar, double>,
                  "is_point_mass only works with double");
    return props.inertia.norm() < 1e-12;
}

template <typename Scalar>
Vec3<Scalar> principal_moments(const MassProperties<Scalar> &props) {
    static_assert(std::is_same_v<Scalar, double>,
                  "principal_moments only works with double");
    Eigen::SelfAdjointEigenSolver<Mat3<double>> solver(props.inertia);
    return solver.eigenvalues();
}

template <typename Scalar>
Mat3<Scalar> principal_axes(const MassProperties<Scalar> &props) {
    static_assert(std::is_same_v<Scalar, double>,
                  "principal_axes only works with double");
    Eigen::SelfAdjointEigenSolver<Mat3<double>> solver(props.inertia);
    return solver.eigenvectors();
}
 
} // namespace vulcan::mass