< Summary

Information
Class: Gravitas.Colliders.MeshRuntimeInertia
Assembly: Gravitas
File(s): /home/runner/work/Gravitas/Gravitas/src/Gravitas/Colliders/MassProperties/MeshRuntimeInertia.cs
Line coverage
100%
Covered lines: 77
Uncovered lines: 0
Coverable lines: 77
Total lines: 151
Line coverage: 100%
Branch coverage
100%
Covered branches: 26
Total branches: 26
Branch coverage: 100%
Method coverage

Feature is only available for sponsors

Upgrade to PRO version

Metrics

MethodBranch coverage Crap Score Cyclomatic complexity Line coverage
Build(...)100%44100%
Calculate(...)100%66100%
CalculateUncached(...)100%44100%
CalculateTranslated(...)100%66100%
TryMaterialize(...)100%66100%
CalculateExact(...)100%11100%
Parallel(...)100%11100%
First(...)100%11100%
Twice(...)100%11100%

File(s)

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Colliders/MassProperties/MeshRuntimeInertia.cs

#LineLine coverage
 1//=======================================================================
 2// MeshRuntimeInertia.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2026-present David Oravsky (mrdav30)
 5// See LICENSE file in the project root for full license information.
 6//=======================================================================
 7
 8using System.Runtime.CompilerServices;
 9using FixedMathSharp;
 10using EnclosedCoefficient = FixedMathSharp.EnclosedQ96Coefficient;
 11
 12namespace Gravitas.Colliders;
 13
 14/// <summary>Retains mesh geometry coefficients and certifies their final mass-applied rounding.</summary>
 15internal sealed class MeshRuntimeInertia
 16{
 17    private EnclosedCoefficient _xx, _yy, _zz, _xy, _xz, _yz;
 18    private Signed192 _firstX, _firstY, _firstZ;
 19    private Vector3d _reference;
 20    private bool _firstZero, _hasResult;
 21    private Fixed64 _lastMass;
 22    private Vector3d _lastReference;
 23    private Fixed3x3 _lastTensor;
 24
 25    internal bool Ready;
 26    internal Vector3d Center;
 27
 28    internal void Build(in MeshMassIntegrals integrals, Vector3d center)
 29    {
 59730        Signed320 denominator = CompoundInertiaMath3D.ScaleWeight(integrals.Weight);
 59731        _xx = new EnclosedCoefficient(WideArithmetic.DivideSigned576ToSigned192(integrals.Ixx, denominator), 9);
 59732        _yy = new EnclosedCoefficient(WideArithmetic.DivideSigned576ToSigned192(integrals.Iyy, denominator), 9);
 59733        _zz = new EnclosedCoefficient(WideArithmetic.DivideSigned576ToSigned192(integrals.Izz, denominator), 9);
 59734        _xy = new EnclosedCoefficient(WideArithmetic.DivideSigned576ToSigned192(integrals.Ixy, denominator), 9);
 59735        _xz = new EnclosedCoefficient(WideArithmetic.DivideSigned576ToSigned192(integrals.Ixz, denominator), 9);
 59736        _yz = new EnclosedCoefficient(WideArithmetic.DivideSigned576ToSigned192(integrals.Iyz, denominator), 9);
 59737        _firstX = WideArithmetic.DivideSigned576ToSigned192(
 59738            WideArithmetic.MultiplySigned576(integrals.FirstX, Signed192.One, Signed192.One), denominator);
 59739        _firstY = WideArithmetic.DivideSigned576ToSigned192(
 59740            WideArithmetic.MultiplySigned576(integrals.FirstY, Signed192.One, Signed192.One), denominator);
 59741        _firstZ = WideArithmetic.DivideSigned576ToSigned192(
 59742            WideArithmetic.MultiplySigned576(integrals.FirstZ, Signed192.One, Signed192.One), denominator);
 59743        _firstZero = integrals.FirstX.IsZero && integrals.FirstY.IsZero && integrals.FirstZ.IsZero;
 59744        _reference = integrals.Reference;
 59745        Center = center;
 59746        _hasResult = false;
 59747        Ready = true;
 59748    }
 49
 50    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 51    internal Fixed3x3 Calculate(Fixed64 mass, Vector3d reference, in MeshMassIntegrals integrals)
 52    {
 291853        if (_hasResult && mass == _lastMass && reference == _lastReference)
 115754            return _lastTensor;
 176155        Fixed3x3 tensor = CalculateUncached(mass, reference, integrals);
 176156        _lastMass = mass;
 176157        _lastReference = reference;
 176158        _lastTensor = tensor;
 176159        _hasResult = true;
 176160        return tensor;
 61    }
 62
 63    private Fixed3x3 CalculateUncached(Fixed64 mass, Vector3d reference, in MeshMassIntegrals integrals)
 64    {
 65        // Keep reference translation's wide temporaries off changing-mass calls.
 273866        if (reference != _reference) return CalculateTranslated(mass, reference, integrals);
 78467        return TryMaterialize(mass, _xx, _yy, _zz, _xy, _xz, _yz, out Fixed3x3 tensor)
 78468            ? tensor : CalculateExact(mass, reference, integrals);
 69    }
 70
 71    private Fixed3x3 CalculateTranslated(Fixed64 mass, Vector3d reference, in MeshMassIntegrals integrals)
 72    {
 586273        Signed192 xx = _xx.Value, yy = _yy.Value, zz = _zz.Value, xy = _xy.Value, xz = _xz.Value, yz = _yz.Value;
 97774        if (!Vector3d.TrySubtract(_reference, reference, out Vector3d delta))
 875            return CalculateExact(mass, reference, integrals);
 290776        Signed192 dx = Signed192.Raw(delta.X), dy = Signed192.Raw(delta.Y), dz = Signed192.Raw(delta.Z);
 290777        Signed192 x2 = Parallel(delta.X, delta.X), y2 = Parallel(delta.Y, delta.Y), z2 = Parallel(delta.Z, delta.Z);
 96978        xx = WideArithmetic.AddSigned192(xx, WideArithmetic.AddSigned192(y2, z2));
 96979        yy = WideArithmetic.AddSigned192(yy, WideArithmetic.AddSigned192(x2, z2));
 96980        zz = WideArithmetic.AddSigned192(zz, WideArithmetic.AddSigned192(x2, y2));
 96981        xy = WideArithmetic.SubtractSigned192(xy, Parallel(delta.X, delta.Y));
 96982        xz = WideArithmetic.SubtractSigned192(xz, Parallel(delta.X, delta.Z));
 96983        yz = WideArithmetic.SubtractSigned192(yz, Parallel(delta.Y, delta.Z));
 96984        if (!_firstZero)
 85        {
 10986            xx = WideArithmetic.AddSigned192(xx, Twice(First(dy, _firstY), First(dz, _firstZ)));
 10987            yy = WideArithmetic.AddSigned192(yy, Twice(First(dx, _firstX), First(dz, _firstZ)));
 10988            zz = WideArithmetic.AddSigned192(zz, Twice(First(dx, _firstX), First(dy, _firstY)));
 10989            xy = WideArithmetic.SubtractSigned192(xy, WideArithmetic.AddSigned192(First(dx, _firstY), First(dy, _firstX)
 10990            xz = WideArithmetic.SubtractSigned192(xz, WideArithmetic.AddSigned192(First(dx, _firstZ), First(dz, _firstX)
 10991            yz = WideArithmetic.SubtractSigned192(yz, WideArithmetic.AddSigned192(First(dy, _firstZ), First(dz, _firstY)
 92        }
 96993        return TryMaterialize(mass, new EnclosedCoefficient(xx, 9), new EnclosedCoefficient(yy, 9),
 96994            new EnclosedCoefficient(zz, 9), new EnclosedCoefficient(xy, 9), new EnclosedCoefficient(xz, 9),
 96995            new EnclosedCoefficient(yz, 9), out Fixed3x3 tensor)
 96996            ? tensor : CalculateExact(mass, reference, integrals);
 97    }
 98
 99    private static bool TryMaterialize(Fixed64 mass, in EnclosedCoefficient xx, in EnclosedCoefficient yy, in EnclosedCo
 100        in EnclosedCoefficient xy, in EnclosedCoefficient xz, in EnclosedCoefficient yz, out Fixed3x3 tensor)
 101    {
 102        // Intrinsic Q96 projection loses <1 unit. Each translated Q128 first
 103        // moment loses <2 Q96 units, so diagonal error is <1+2*(2+2)=9.
 104        // Parallel terms are exact. Coefficients fit 165 bits; mass fits 63.
 1753105        bool xFits = xx.TryMultiplyRaw(mass, out long x);
 106        bool yFits;
 107        long y;
 4571108        if (xx.Equals(yy)) { y = x; yFits = xFits; }
 344109        else yFits = yy.TryMultiplyRaw(mass, out y);
 110        bool zFits;
 111        long z;
 4737112        if (xx.Equals(zz)) { z = x; zFits = xFits; }
 261113        else zFits = zz.TryMultiplyRaw(mass, out z);
 1753114        bool fits = xFits & yFits & zFits
 1753115            & xy.TryMultiplyRaw(mass, out long a)
 1753116            & xz.TryMultiplyRaw(mass, out long b)
 1753117            & yz.TryMultiplyRaw(mass, out long c);
 1753118        if (fits)
 119        {
 1737120            tensor = new Fixed3x3(
 1737121                Fixed64.FromRaw(x), Fixed64.FromRaw(a), Fixed64.FromRaw(b),
 1737122                Fixed64.FromRaw(a), Fixed64.FromRaw(y), Fixed64.FromRaw(c),
 1737123                Fixed64.FromRaw(b), Fixed64.FromRaw(c), Fixed64.FromRaw(z));
 1737124            return true;
 125        }
 16126        tensor = default;
 16127        return false;
 128    }
 129
 130    [MethodImpl(MethodImplOptions.NoInlining)]
 131    private static Fixed3x3 CalculateExact(Fixed64 mass, Vector3d reference, in MeshMassIntegrals integrals)
 132    {
 133        // A half tie or unsupported certificate width still uses every retained bit.
 24134        CompoundInertiaTensor exact = default;
 24135        CompoundInertiaMath3D.AddMesh(ref exact, integrals, FixedQuaternion.Identity,
 24136            ExactMassPoint3D.FromPoint(integrals.Reference), reference);
 24137        return exact.MaterializeSaturated(mass, integrals.Weight);
 138    }
 139
 140    private static Signed192 Parallel(Fixed64 first, Fixed64 second) =>
 5814141        WideArithmetic.ShiftLeftSigned192By32(Fixed64.GetExactRawProduct(first, second));
 142
 143    private static Signed192 First(Signed192 displacement, Signed192 firstMoment) =>
 1308144        WideArithmetic.TruncateSigned320By64(WideArithmetic.MultiplySigned192(displacement, firstMoment));
 145
 146    private static Signed192 Twice(Signed192 first, Signed192 second)
 147    {
 327148        Signed192 sum = WideArithmetic.AddSigned192(first, second);
 327149        return WideArithmetic.AddSigned192(sum, sum);
 150    }
 151}