< Summary

Information
Class: Gravitas.Colliders.CompoundInertiaMath3D
Assembly: Gravitas
File(s): /home/runner/work/Gravitas/Gravitas/src/Gravitas/Colliders/MassProperties/CompoundInertiaMath3D.cs
Line coverage
100%
Covered lines: 138
Uncovered lines: 0
Coverable lines: 138
Total lines: 231
Line coverage: 100%
Branch coverage
100%
Covered branches: 20
Total branches: 20
Branch coverage: 100%
Method coverage

Feature is only available for sponsors

Upgrade to PRO version

Metrics

MethodBranch coverage Crap Score Cyclomatic complexity Line coverage
.cctor()100%11100%
ScaleWeight(...)100%11100%
Add(...)100%22100%
AddMesh(...)100%11100%
Difference(...)100%11100%
Weighted(...)100%11100%
GetIntrinsic(...)100%88100%
GetCapsuleIntrinsic(...)100%22100%
AddIntrinsic(...)100%22100%
AddPadded(...)100%11100%
AddRotated(...)100%11100%
RotationTerm(...)100%11100%
RotationProduct(...)100%11100%
AddParallelAxis(...)100%11100%
AddFirstMoments(...)100%66100%
RotateFirst(...)100%11100%
FirstProduct(...)100%11100%
AddFirstDiagonal(...)100%11100%
SubtractFirstProduct(...)100%11100%

File(s)

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

#LineLine coverage
 1//=======================================================================
 2// CompoundInertiaMath3D.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;
 9using FixedMathSharp;
 10
 11namespace Gravitas.Colliders;
 12
 13
 14
 15/// <summary>Accumulates homogeneous weighted moments without child mass or tensor rounding.</summary>
 16internal static class CompoundInertiaMath3D
 17{
 118    internal static readonly Signed320 Q192 = Signed320.NarrowValue(
 119        WideArithmetic.MultiplySigned576(Signed576.ExtendValue(
 120            WideArithmetic.MultiplySigned192(Signed192.One, Signed192.One)),
 121            Signed192.One, Signed192.One, Signed192.One, Signed192.One));
 22
 23    internal static Signed320 ScaleWeight(ExactMassWeight weight) =>
 340524        Signed320.NarrowValue(WideArithmetic.MultiplySigned320(weight.Numerator, Signed192.Signed(60)));
 25
 26    internal static void Add(ref CompoundInertiaTensor tensor, LSCollider part,
 27        ExactMassWeight weight, Vector3d reference)
 28    {
 28529        if (part is LSMeshCollider meshCollider)
 30        {
 3231            MeshMassIntegrals mesh = meshCollider.Mesh.GetCompoundMassIntegrals(usePrepared: false, meshCollider.Inertia
 3232            AddMesh(ref tensor, mesh, part.CompoundLocalRotation,
 3233                part.TransformRelativeMassPropertyPointExact(mesh.Reference), reference);
 3234            return;
 35        }
 25336        Span<Signed576> intrinsic = stackalloc Signed576[6];
 25337        intrinsic.Clear();
 25338        GetIntrinsic(part, weight, intrinsic);
 25339        ExactMassPoint3D point = part.CalculateLocalMassPoint();
 40
 41        // Q224 intrinsic moments acquire Q64 rotation products and Q32 padding.
 42        // Q96 semantic displacements square directly into the same Q320 frame.
 25343        AddIntrinsic(ref tensor, intrinsic, part.CompoundLocalRotation);
 25344        Signed320 dx = Difference(point.XNumerator, reference.X);
 25345        Signed320 dy = Difference(point.YNumerator, reference.Y);
 25346        Signed320 dz = Difference(point.ZNumerator, reference.Z);
 25347        AddParallelAxis(ref tensor, ScaleWeight(weight), dx, dy, dz);
 25348    }
 49
 50    internal static void AddMesh(ref CompoundInertiaTensor tensor, in MeshMassIntegrals mesh,
 51        FixedQuaternion rotation, ExactMassPoint3D point, Vector3d reference)
 52    {
 8053        Span<Signed576> intrinsic = stackalloc Signed576[6];
 24054        intrinsic[0] = mesh.Ixx; intrinsic[1] = mesh.Iyy; intrinsic[2] = mesh.Izz;
 24055        intrinsic[3] = mesh.Ixy; intrinsic[4] = mesh.Ixz; intrinsic[5] = mesh.Iyz;
 8056        AddIntrinsic(ref tensor, intrinsic, rotation);
 8057        Signed320 dx = Difference(point.XNumerator, reference.X);
 8058        Signed320 dy = Difference(point.YNumerator, reference.Y);
 8059        Signed320 dz = Difference(point.ZNumerator, reference.Z);
 8060        AddParallelAxis(ref tensor, ScaleWeight(mesh.Weight), dx, dy, dz);
 8061        AddFirstMoments(ref tensor, mesh, rotation, dx, dy, dz);
 8062    }
 63
 64    private static Signed320 Difference(Signed320 point, Fixed64 reference) =>
 99965        WideArithmetic.SubtractSigned320(point, Signed320.NarrowValue(
 99966            WideArithmetic.MultiplySigned320(
 99967                WideArithmetic.MultiplySigned192(Signed192.Raw(reference), Signed192.One), Signed192.One)));
 68
 69    private static Signed576 Weighted(ExactMassWeight weight, Signed192 geometry, int factor = 60) =>
 47670        WideArithmetic.MultiplySigned576(
 47671            WideArithmetic.MultiplySigned320(weight.Numerator, geometry), Signed192.Signed(factor));
 72
 73    private static void GetIntrinsic(LSCollider part, ExactMassWeight weight, Span<Signed576> values)
 74    {
 75        switch (part)
 76        {
 77            case LSSphereCollider sphere:
 13478                Fixed64 radius = sphere.GetMassPropertyRadius();
 13479                Signed576 spherical = Weighted(weight,
 13480                    ExactMassProperties.InertiaGeometryTerm(Fixed64.FromFraction(2, 5), radius, radius));
 13481                values[0] = values[1] = values[2] = spherical;
 13482                break;
 83            case LSCuboidCollider box:
 6584                Vector3d extents = box.GetMassPropertyHalfExtents();
 6585                Signed576 x = Weighted(weight, ExactMassProperties.InertiaGeometryTerm(Fixed64.One, extents.X, extents.X
 6586                Signed576 y = Weighted(weight, ExactMassProperties.InertiaGeometryTerm(Fixed64.One, extents.Y, extents.Y
 6587                Signed576 z = Weighted(weight, ExactMassProperties.InertiaGeometryTerm(Fixed64.One, extents.Z, extents.Z
 6588                values[0] = WideArithmetic.AddSigned576(y, z);
 6589                values[1] = WideArithmetic.AddSigned576(x, z);
 6590                values[2] = WideArithmetic.AddSigned576(x, y);
 6591                break;
 92            case LSCylinderCollider cylinder:
 1593                cylinder.GetInertiaGeometry(out Signed192 cylinderTransverse, out Signed192 cylinderAxial);
 1594                values[0] = values[2] = Weighted(weight, cylinderTransverse);
 1595                values[1] = Weighted(weight, cylinderAxial);
 1596                break;
 97            case LSConeCollider cone:
 1898                cone.GetInertiaGeometry(out Signed192 coneTransverse, out Signed192 coneAxial);
 1899                values[0] = values[2] = Weighted(weight, coneTransverse);
 18100                values[1] = Weighted(weight, coneAxial);
 18101                break;
 102            default:
 103                // Authored compound definitions admit only this closed family.
 21104                GetCapsuleIntrinsic((LSCapsuleCollider)part, weight, values);
 105                break;
 106        }
 21107    }
 108
 109    private static void GetCapsuleIntrinsic(LSCapsuleCollider capsule, ExactMassWeight weight, Span<Signed576> values)
 110    {
 21111        capsule.GetMassPropertyGeometry(out Fixed64 radius, out Fixed64 axis);
 21112        Signed192 sphereAxial = ExactMassProperties.InertiaGeometryTerm(Fixed64.FromFraction(2, 5), radius, radius);
 21113        if (axis <= Fixed64.Epsilon)
 114        {
 1115            values[0] = values[1] = values[2] = Weighted(weight, sphereAxial);
 1116            return;
 117        }
 20118        LSCapsuleCollider.GetComponentInertiaGeometry(radius, axis, sphereAxial,
 20119            out Signed192 cylinderTransverse, out Signed192 sphereTransverse);
 20120        ExactMassWeight cylinderWeight = LSCapsuleCollider.GetCylinderWeight(radius, axis);
 20121        ExactMassWeight capWeight = LSCapsuleCollider.GetCapWeight(radius);
 20122        values[0] = values[2] = WideArithmetic.AddSigned576(
 20123            Weighted(cylinderWeight, cylinderTransverse), Weighted(capWeight, sphereTransverse));
 20124        values[1] = WideArithmetic.AddSigned576(
 20125            Weighted(cylinderWeight, ExactMassProperties.InertiaGeometryTerm(Fixed64.Half, radius, radius)),
 20126            Weighted(capWeight, sphereAxial));
 20127    }
 128
 129    private static void AddIntrinsic(ref CompoundInertiaTensor tensor, ReadOnlySpan<Signed576> values, FixedQuaternion r
 130    {
 333131        if (rotation == FixedQuaternion.Identity)
 132        {
 290133            tensor.Xx = AddPadded(tensor.Xx, values[0]);
 290134            tensor.Yy = AddPadded(tensor.Yy, values[1]);
 290135            tensor.Zz = AddPadded(tensor.Zz, values[2]);
 290136            tensor.Xy = AddPadded(tensor.Xy, values[3]);
 290137            tensor.Xz = AddPadded(tensor.Xz, values[4]);
 290138            tensor.Yz = AddPadded(tensor.Yz, values[5]);
 290139            return;
 140        }
 43141        Fixed3x3 matrix = rotation.ToMatrix3x3().Transpose();
 43142        Vector3d x = new(matrix.M11, matrix.M12, matrix.M13);
 43143        Vector3d y = new(matrix.M21, matrix.M22, matrix.M23);
 43144        Vector3d z = new(matrix.M31, matrix.M32, matrix.M33);
 43145        tensor.Xx = AddRotated(tensor.Xx, values, x, x);
 43146        tensor.Yy = AddRotated(tensor.Yy, values, y, y);
 43147        tensor.Zz = AddRotated(tensor.Zz, values, z, z);
 43148        tensor.Xy = AddRotated(tensor.Xy, values, x, y);
 43149        tensor.Xz = AddRotated(tensor.Xz, values, x, z);
 43150        tensor.Yz = AddRotated(tensor.Yz, values, y, z);
 43151    }
 152
 153    private static Signed704 AddPadded(Signed704 total, Signed576 value) =>
 1740154        WideArithmetic.AddSigned704(total, Signed704.ExtendValue(
 1740155            WideArithmetic.MultiplySigned576(value, Signed192.One, Signed192.One, Signed192.One)));
 156
 157    private static Signed704 AddRotated(Signed704 total, ReadOnlySpan<Signed576> values, Vector3d row, Vector3d column)
 158    {
 258159        Signed576 value = WideArithmetic.AddSigned576(
 258160            WideArithmetic.AddSigned576(RotationTerm(values[0], row.X, column.X), RotationTerm(values[1], row.Y, column.
 258161            RotationTerm(values[2], row.Z, column.Z));
 258162        value = WideArithmetic.AddSigned576(value, RotationProduct(values[3], row.X, column.Y, row.Y, column.X));
 258163        value = WideArithmetic.AddSigned576(value, RotationProduct(values[4], row.X, column.Z, row.Z, column.X));
 258164        value = WideArithmetic.AddSigned576(value, RotationProduct(values[5], row.Y, column.Z, row.Z, column.Y));
 258165        return WideArithmetic.AddSigned704(total, Signed704.ExtendValue(
 258166            WideArithmetic.MultiplySigned576(value, Signed192.One)));
 167    }
 168
 169    private static Signed576 RotationTerm(Signed576 value, Fixed64 first, Fixed64 second) =>
 774170        WideArithmetic.MultiplySigned576(value, Fixed64.GetExactRawProduct(first, second));
 171
 172    private static Signed576 RotationProduct(Signed576 value,
 173        Fixed64 first, Fixed64 second, Fixed64 third, Fixed64 fourth) =>
 774174        WideArithmetic.MultiplySigned576(value, WideArithmetic.AddSigned192(
 774175            Fixed64.GetExactRawProduct(first, second), Fixed64.GetExactRawProduct(third, fourth)));
 176
 177    private static void AddParallelAxis(ref CompoundInertiaTensor tensor, Signed320 weight,
 178        Signed320 dx, Signed320 dy, Signed320 dz)
 179    {
 333180        Signed704 x = WideArithmetic.MultiplySigned320(dx, dx, weight);
 333181        Signed704 y = WideArithmetic.MultiplySigned320(dy, dy, weight);
 333182        Signed704 z = WideArithmetic.MultiplySigned320(dz, dz, weight);
 333183        tensor.Xx = WideArithmetic.AddSigned704(tensor.Xx, WideArithmetic.AddSigned704(y, z));
 333184        tensor.Yy = WideArithmetic.AddSigned704(tensor.Yy, WideArithmetic.AddSigned704(x, z));
 333185        tensor.Zz = WideArithmetic.AddSigned704(tensor.Zz, WideArithmetic.AddSigned704(x, y));
 333186        tensor.Xy = WideArithmetic.SubtractSigned704(tensor.Xy, WideArithmetic.MultiplySigned320(dx, dy, weight));
 333187        tensor.Xz = WideArithmetic.SubtractSigned704(tensor.Xz, WideArithmetic.MultiplySigned320(dx, dz, weight));
 333188        tensor.Yz = WideArithmetic.SubtractSigned704(tensor.Yz, WideArithmetic.MultiplySigned320(dy, dz, weight));
 333189    }
 190
 191    private static void AddFirstMoments(ref CompoundInertiaTensor tensor, MeshMassIntegrals mesh,
 192        FixedQuaternion rotation, Signed320 dx, Signed320 dy, Signed320 dz)
 193    {
 80194        if (mesh.FirstX.IsZero && mesh.FirstY.IsZero && mesh.FirstZ.IsZero)
 60195            return;
 20196        Fixed3x3 matrix = rotation.ToMatrix3x3().Transpose();
 20197        Signed576 fx = RotateFirst(mesh, matrix.M11, matrix.M12, matrix.M13);
 20198        Signed576 fy = RotateFirst(mesh, matrix.M21, matrix.M22, matrix.M23);
 20199        Signed576 fz = RotateFirst(mesh, matrix.M31, matrix.M32, matrix.M33);
 200        // First moments retain the centroid's fractional residual. Omitting
 201        // these cross terms would make translation depend on a rounded centroid.
 20202        tensor.Xx = AddFirstDiagonal(tensor.Xx, fy, dy, fz, dz);
 20203        tensor.Yy = AddFirstDiagonal(tensor.Yy, fx, dx, fz, dz);
 20204        tensor.Zz = AddFirstDiagonal(tensor.Zz, fx, dx, fy, dy);
 20205        tensor.Xy = SubtractFirstProduct(tensor.Xy, fx, dy, fy, dx);
 20206        tensor.Xz = SubtractFirstProduct(tensor.Xz, fx, dz, fz, dx);
 20207        tensor.Yz = SubtractFirstProduct(tensor.Yz, fy, dz, fz, dy);
 20208    }
 209
 210    private static Signed576 RotateFirst(MeshMassIntegrals mesh, Fixed64 x, Fixed64 y, Fixed64 z) =>
 60211        WideArithmetic.AddSigned576(WideArithmetic.AddSigned576(
 60212            WideArithmetic.MultiplySigned576(mesh.FirstX, Signed192.Raw(x)),
 60213            WideArithmetic.MultiplySigned576(mesh.FirstY, Signed192.Raw(y))),
 60214            WideArithmetic.MultiplySigned576(mesh.FirstZ, Signed192.Raw(z)));
 215
 216    private static Signed576 FirstProduct(Signed576 first, Signed320 displacement) =>
 240217        WideArithmetic.MultiplySigned576(first, Signed192.NarrowValue(displacement));
 218
 219    private static Signed704 AddFirstDiagonal(Signed704 total,
 220        Signed576 first, Signed320 firstDisplacement, Signed576 second, Signed320 secondDisplacement)
 221    {
 60222        Signed576 shift = WideArithmetic.AddSigned576(
 60223            FirstProduct(first, firstDisplacement), FirstProduct(second, secondDisplacement));
 60224        return WideArithmetic.AddSigned704(total, Signed704.ExtendValue(WideArithmetic.AddSigned576(shift, shift)));
 225    }
 226
 227    private static Signed704 SubtractFirstProduct(Signed704 total,
 228        Signed576 first, Signed320 firstDisplacement, Signed576 second, Signed320 secondDisplacement) =>
 60229        WideArithmetic.SubtractSigned704(total, Signed704.ExtendValue(WideArithmetic.AddSigned576(
 60230            FirstProduct(first, firstDisplacement), FirstProduct(second, secondDisplacement))));
 231}

Methods/Properties

.cctor()
ScaleWeight(Gravitas.Colliders.ExactMassWeight)
Add(Gravitas.Colliders.CompoundInertiaTensor&,Gravitas.Colliders.LSCollider,Gravitas.Colliders.ExactMassWeight,FixedMathSharp.Vector3d)
AddMesh(Gravitas.Colliders.CompoundInertiaTensor&,Gravitas.Colliders.MeshMassIntegrals&,FixedMathSharp.FixedQuaternion,Gravitas.Colliders.ExactMassPoint3D,FixedMathSharp.Vector3d)
Difference(FixedMathSharp.Signed320,FixedMathSharp.Fixed64)
Weighted(Gravitas.Colliders.ExactMassWeight,FixedMathSharp.Signed192,System.Int32)
GetIntrinsic(Gravitas.Colliders.LSCollider,Gravitas.Colliders.ExactMassWeight,System.Span`1<FixedMathSharp.Signed576>)
GetCapsuleIntrinsic(Gravitas.Colliders.LSCapsuleCollider,Gravitas.Colliders.ExactMassWeight,System.Span`1<FixedMathSharp.Signed576>)
AddIntrinsic(Gravitas.Colliders.CompoundInertiaTensor&,System.ReadOnlySpan`1<FixedMathSharp.Signed576>,FixedMathSharp.FixedQuaternion)
AddPadded(FixedMathSharp.Signed704,FixedMathSharp.Signed576)
AddRotated(FixedMathSharp.Signed704,System.ReadOnlySpan`1<FixedMathSharp.Signed576>,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
RotationTerm(FixedMathSharp.Signed576,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
RotationProduct(FixedMathSharp.Signed576,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
AddParallelAxis(Gravitas.Colliders.CompoundInertiaTensor&,FixedMathSharp.Signed320,FixedMathSharp.Signed320,FixedMathSharp.Signed320,FixedMathSharp.Signed320)
AddFirstMoments(Gravitas.Colliders.CompoundInertiaTensor&,Gravitas.Colliders.MeshMassIntegrals,FixedMathSharp.FixedQuaternion,FixedMathSharp.Signed320,FixedMathSharp.Signed320,FixedMathSharp.Signed320)
RotateFirst(Gravitas.Colliders.MeshMassIntegrals,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
FirstProduct(FixedMathSharp.Signed576,FixedMathSharp.Signed320)
AddFirstDiagonal(FixedMathSharp.Signed704,FixedMathSharp.Signed576,FixedMathSharp.Signed320,FixedMathSharp.Signed576,FixedMathSharp.Signed320)
SubtractFirstProduct(FixedMathSharp.Signed704,FixedMathSharp.Signed576,FixedMathSharp.Signed320,FixedMathSharp.Signed576,FixedMathSharp.Signed320)