Mesh Volume Integration

void b3MeshShape::ComputeInertia(b3Inertia* inertia, r32 density) const {
    b3Vec3 center(0.0f, 0.0f, 0.0f);
    r32 volume = 0.0f;
    b3Mat33 I;
    I.SetZero();

    // Pick reference point.
    b3Vec3 v1(0.0f, 0.0f, 0.0f);

    const r32 inv4 = 0.25f;
    const r32 inv6 = 1.0f / 6.0f;
    const r32 inv60 = 1.0f / 60.0f;
    const r32 inv120 = 1.0f / 120.0f;

    b3Vec3 diag(0.0f, 0.0f, 0.0f);
    b3Vec3 offDiag(0.0f, 0.0f, 0.0f);

    for (u32 i = 0; i < m_mesh->triangleCount; ++i) {
        const b3Triangle* f = m_mesh->triangles + i;

        b3Vec3 v2 = m_mesh->vertices[f->indices[0]];
        b3Vec3 v3 = m_mesh->vertices[f->indices[1]];
        b3Vec3 v4 = m_mesh->vertices[f->indices[2]];
        b3Vec3 tetraCenter = inv4 * (v1 + v2 + v3 + v4);

        b3Vec3 e1 = v2 - v1;
        b3Vec3 e2 = v3 - v1;
        b3Vec3 e3 = v4 - v1;
        r32 det = b3Det(e1, e2, e3);
        r32 tetraVolume = inv6 * det;

        // Volume weighted center of mass
        center += tetraVolume * tetraCenter;
        volume += tetraVolume;

        // Volume weighted inertia tensor
        // https://github.com/melax/sandbox
        for (u32 j = 0; j < 3; ++j) {
            u32 j1 = (j + 1) % 3;
            u32 j2 = (j + 2) % 3;

            diag[j] += inv60 * det *
                (e1[j] * e2[j] + e2[j] * e3[j] + e3[j] * e1[j] +
                    e1[j] * e1[j] + e2[j] * e2[j] + e3[j] * e3[j]);

            offDiag[j] += inv120 * det  *
                (e1[j1] * e2[j2] + e2[j1] * e3[j2] + e3[j1] * e1[j2] +
                    e1[j1] * e3[j2] + e2[j1] * e1[j2] + e3[j1] * e2[j2] +
                    e1[j1] * e1[j2] * 2.0f + e2[j1] * e2[j2] * 2.0f + e3[j1] * e3[j2] * 2.0f);
        }
    }

    b3Assert(volume > B3_EPSILON);

    r32 invVolume = 1.0f / volume;

    diag = invVolume * diag;
    offDiag = invVolume * offDiag;

    I.x.Set(diag.y + diag.z, -offDiag.z, -offDiag.y);
    I.y.Set(-offDiag.z, diag.x + diag.z, -offDiag.x);
    I.z.Set(-offDiag.y, -offDiag.x, diag.x + diag.y);

    inertia->center = invVolume * center;
    inertia->mass = density * volume;
    inertia->I = inertia->mass * I;
}