< Summary

Information
Class: Gravitas.Colliders.TriangleShellMassProperties
Assembly: Gravitas
File(s): /home/runner/work/Gravitas/Gravitas/src/Gravitas/Colliders/MassProperties/TriangleShellMassProperties.cs
Line coverage
100%
Covered lines: 301
Uncovered lines: 0
Coverable lines: 301
Total lines: 414
Line coverage: 100%
Branch coverage
100%
Covered branches: 42
Total branches: 42
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%
TryCreateUniformShell(...)100%3636100%
AddWeightedVertexSum(...)100%11100%
AddWeighted(...)100%11100%
GetProductSums(...)100%11100%
SquaredBarycentricSum(...)100%11100%
BarycentricCrossSum(...)100%11100%
TranslateSquaredMoment(...)100%11100%
TranslateProductMoment(...)100%11100%
ScaleCoordinate(...)100%11100%
GetTriangle(...)100%66100%

File(s)

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

#LineLine coverage
 1//=======================================================================
 2// TriangleShellMassProperties.cs
 3//=======================================================================
 4// MIT License, Copyright (c) 2024-present David Oravsky (mrdav30)
 5// See LICENSE file in the project root for full license information.
 6//=======================================================================
 7
 8using System;
 9
 10using FixedMathSharp;
 11using FixedMathSharp.Geometry;
 12
 13namespace Gravitas.Colliders;
 14
 15/// <summary>
 16/// Owns wide uniform thin-shell integration for indexed triangle surfaces.
 17/// </summary>
 18internal static partial class TriangleShellMassProperties
 19{
 220    private static readonly Signed192 Three = Signed192.Signed(3);
 221    private static readonly Signed192 Six = Signed192.Signed(6);
 222    private static readonly Signed192 Twelve = Signed192.Signed(12);
 23
 24    internal static bool TryCreateUniformShell(
 25        ReadOnlySpan<Vector3d> vertices,
 26        ReadOnlySpan<int> triangleIndices,
 27        out ExactMassWeight surfaceWeight,
 28        out Vector3d centerOfMass,
 29        out Fixed3x3 unitMassInertiaTensor,
 30        out MeshMassIntegrals integrals,
 31        in ScaledRigidFrame3d sourceFrame = default,
 32        bool exactScale = false)
 33    {
 478334        integrals = default;
 478335        if (triangleIndices.Length % FixedTriangle.VertexCount != 0)
 36        {
 137            throw new ArgumentException(
 138                "Triangle indices must contain complete index triplets.",
 139                nameof(triangleIndices));
 40        }
 41
 478242        Signed576 totalWeight = default;
 478243        Signed576 firstMomentX = default;
 478244        Signed576 firstMomentY = default;
 478245        Signed576 firstMomentZ = default;
 478246        Signed576 secondMomentX = default;
 478247        Signed576 secondMomentY = default;
 478248        Signed576 secondMomentZ = default;
 478249        Signed576 productMomentXY = default;
 478250        Signed576 productMomentXZ = default;
 478251        Signed576 productMomentYZ = default;
 30782052        for (int i = 0; i < triangleIndices.Length; i += FixedTriangle.VertexCount)
 53        {
 14913154            GetTriangle(
 14913155                vertices,
 14913156                triangleIndices,
 14913157                i,
 14913158                out FixedTriangle triangle);
 14912859            var coordinates = new TriangleCoordinates(triangle, sourceFrame, exactScale);
 60            ExactMassWeight weight;
 14912861            if (exactScale)
 62            {
 58563                WideDirection3d normal = WideTriangleRelations.GetScaledTriangleLocalNormal(triangle, sourceFrame);
 64                // Q128 area weights retain the exact scaled surface even when
 65                // its Q32.32 inspection vertices would collapse to a line.
 58566                Signed576 root = WideArithmetic.GetFloorSquareRootOfProduct(normal.SquaredLength, Signed192.Signed(9));
 58567                Signed192 numerator = WideArithmetic.DivideSigned576ToSigned192(root, new Signed320(0, 0, 0, 1, 0));
 58568                weight = new ExactMassWeight(Signed320.ExtendValue(numerator));
 69            }
 70            else
 71            {
 14854372                triangle.GetExactNormal(out _, out _, out _, out Signed320 squaredDoubleArea);
 14854373                weight = ExactMassProperties.CreateTriangleAreaWeight(squaredDoubleArea);
 74            }
 14912875            Signed320 weightNumerator = weight.Numerator;
 14912876            totalWeight = WideArithmetic.AddSigned576(
 14912877                totalWeight,
 14912878                Signed576.ExtendValue(weightNumerator));
 14912879            firstMomentX = AddWeightedVertexSum(
 14912880                firstMomentX,
 14912881                coordinates.AX,
 14912882                coordinates.BX,
 14912883                coordinates.CX,
 14912884                weightNumerator);
 14912885            firstMomentY = AddWeightedVertexSum(
 14912886                firstMomentY,
 14912887                coordinates.AY,
 14912888                coordinates.BY,
 14912889                coordinates.CY,
 14912890                weightNumerator);
 14912891            firstMomentZ = AddWeightedVertexSum(
 14912892                firstMomentZ,
 14912893                coordinates.AZ,
 14912894                coordinates.BZ,
 14912895                coordinates.CZ,
 14912896                weightNumerator);
 14912897            GetProductSums(
 14912898                coordinates,
 14912899                out Signed320 x2,
 149128100                out Signed320 y2,
 149128101                out Signed320 z2,
 149128102                out Signed320 xy,
 149128103                out Signed320 xz,
 149128104                out Signed320 yz);
 149128105            secondMomentX = AddWeighted(
 149128106                secondMomentX,
 149128107                x2,
 149128108                weightNumerator);
 149128109            secondMomentY = AddWeighted(
 149128110                secondMomentY,
 149128111                y2,
 149128112                weightNumerator);
 149128113            secondMomentZ = AddWeighted(
 149128114                secondMomentZ,
 149128115                z2,
 149128116                weightNumerator);
 149128117            productMomentXY = AddWeighted(
 149128118                productMomentXY,
 149128119                xy,
 149128120                weightNumerator);
 149128121            productMomentXZ = AddWeighted(
 149128122                productMomentXZ,
 149128123                xz,
 149128124                weightNumerator);
 149128125            productMomentYZ = AddWeighted(
 149128126                productMomentYZ,
 149128127                yz,
 149128128                weightNumerator);
 129        }
 130
 4779131        if (totalWeight.IsZero)
 132        {
 1133            surfaceWeight = default;
 1134            centerOfMass = default;
 1135            unitMassInertiaTensor = default;
 1136            integrals = default;
 1137            return false;
 138        }
 139
 4778140        surfaceWeight = new ExactMassWeight(
 4778141            Signed320.NarrowValue(totalWeight));
 4778142        integrals = MeshMassIntegrals.FromShell(surfaceWeight,
 4778143            firstMomentX, firstMomentY, firstMomentZ,
 4778144            secondMomentX, secondMomentY, secondMomentZ,
 4778145            productMomentXY, productMomentXZ, productMomentYZ, exactScale ? 64 : 0);
 4778146        Signed576 centerDenominator =
 4778147            WideArithmetic.MultiplySigned576(
 4778148                totalWeight,
 4778149                exactScale ? Signed192.NarrowValue(WideArithmetic.MultiplySigned192(Three, new Signed192(0, 1, 0))) : Th
 4778150        _ = Fixed64.TryGetSignedRawRatio(
 4778151            firstMomentX,
 4778152            centerDenominator,
 4778153            out Fixed64 centerX);
 4778154        _ = Fixed64.TryGetSignedRawRatio(
 4778155            firstMomentY,
 4778156            centerDenominator,
 4778157            out Fixed64 centerY);
 4778158        _ = Fixed64.TryGetSignedRawRatio(
 4778159            firstMomentZ,
 4778160            centerDenominator,
 4778161            out Fixed64 centerZ);
 4778162        centerOfMass = new Vector3d(centerX, centerY, centerZ);
 163
 4778164        Signed576 centeredX = TranslateSquaredMoment(
 4778165            secondMomentX,
 4778166            firstMomentX,
 4778167            totalWeight,
 4778168            exactScale ? ScaleCoordinate(centerX) : Signed192.Raw(centerX));
 4778169        Signed576 centeredY = TranslateSquaredMoment(
 4778170            secondMomentY,
 4778171            firstMomentY,
 4778172            totalWeight,
 4778173            exactScale ? ScaleCoordinate(centerY) : Signed192.Raw(centerY));
 4778174        Signed576 centeredZ = TranslateSquaredMoment(
 4778175            secondMomentZ,
 4778176            firstMomentZ,
 4778177            totalWeight,
 4778178            exactScale ? ScaleCoordinate(centerZ) : Signed192.Raw(centerZ));
 4778179        Signed576 centeredXY = TranslateProductMoment(
 4778180            productMomentXY,
 4778181            firstMomentX,
 4778182            firstMomentY,
 4778183            totalWeight,
 4778184            exactScale ? ScaleCoordinate(centerX) : Signed192.Raw(centerX),
 4778185            exactScale ? ScaleCoordinate(centerY) : Signed192.Raw(centerY));
 4778186        Signed576 centeredXZ = TranslateProductMoment(
 4778187            productMomentXZ,
 4778188            firstMomentX,
 4778189            firstMomentZ,
 4778190            totalWeight,
 4778191            exactScale ? ScaleCoordinate(centerX) : Signed192.Raw(centerX),
 4778192            exactScale ? ScaleCoordinate(centerZ) : Signed192.Raw(centerZ));
 4778193        Signed576 centeredYZ = TranslateProductMoment(
 4778194            productMomentYZ,
 4778195            firstMomentY,
 4778196            firstMomentZ,
 4778197            totalWeight,
 4778198            exactScale ? ScaleCoordinate(centerY) : Signed192.Raw(centerY),
 4778199            exactScale ? ScaleCoordinate(centerZ) : Signed192.Raw(centerZ));
 4778200        Signed576 inertiaXX =
 4778201            WideArithmetic.AddSigned576(centeredY, centeredZ);
 4778202        Signed576 inertiaYY =
 4778203            WideArithmetic.AddSigned576(centeredX, centeredZ);
 4778204        Signed576 inertiaZZ =
 4778205            WideArithmetic.AddSigned576(centeredX, centeredY);
 206
 4778207        Signed576 diagonalDenominator =
 4778208            WideArithmetic.MultiplySigned576(
 4778209                totalWeight,
 4778210                exactScale ? new Signed192(1UL << 32, 0, 0) : Signed192.One,
 4778211                Six);
 4778212        Signed576 productDenominator =
 4778213            WideArithmetic.MultiplySigned576(
 4778214                totalWeight,
 4778215                exactScale ? new Signed192(1UL << 32, 0, 0) : Signed192.One,
 4778216                Twelve);
 4778217        bool representable = Fixed64.TryGetSignedRawRatio(
 4778218                inertiaXX,
 4778219                diagonalDenominator,
 4778220                out Fixed64 tensorXX)
 4778221            & Fixed64.TryGetSignedRawRatio(
 4778222                inertiaYY,
 4778223                diagonalDenominator,
 4778224                out Fixed64 tensorYY)
 4778225            & Fixed64.TryGetSignedRawRatio(
 4778226                inertiaZZ,
 4778227                diagonalDenominator,
 4778228                out Fixed64 tensorZZ)
 4778229            & Fixed64.TryGetSignedRawRatio(
 4778230                WideArithmetic.SubtractSigned576(default, centeredXY),
 4778231                productDenominator,
 4778232                out Fixed64 tensorXY)
 4778233            & Fixed64.TryGetSignedRawRatio(
 4778234                WideArithmetic.SubtractSigned576(default, centeredXZ),
 4778235                productDenominator,
 4778236                out Fixed64 tensorXZ)
 4778237            & Fixed64.TryGetSignedRawRatio(
 4778238                WideArithmetic.SubtractSigned576(default, centeredYZ),
 4778239                productDenominator,
 4778240                out Fixed64 tensorYZ);
 4778241        if (!representable)
 242        {
 1243            surfaceWeight = default;
 1244            centerOfMass = default;
 1245            unitMassInertiaTensor = default;
 1246            integrals = default;
 1247            return false;
 248        }
 249
 4777250        unitMassInertiaTensor = new Fixed3x3(
 4777251            tensorXX, tensorXY, tensorXZ,
 4777252            tensorXY, tensorYY, tensorYZ,
 4777253            tensorXZ, tensorYZ, tensorZZ);
 4777254        return true;
 255    }
 256
 257    internal static Signed576 AddWeightedVertexSum(Signed576 total, Signed192 first, Signed192 second, Signed192 third, 
 258    {
 447384259        Signed320 sum = WideArithmetic.AddSigned320(WideArithmetic.AddSigned320(
 447384260            Signed320.ExtendValue(first), Signed320.ExtendValue(second)), Signed320.ExtendValue(third));
 447384261        return AddWeighted(total, sum, weight);
 262    }
 263
 264    private static Signed576 AddWeighted(
 265        Signed576 total,
 266        Signed320 value,
 267        Signed320 weight) =>
 1342152268        WideArithmetic.AddSigned576(
 1342152269            total,
 1342152270            WideArithmetic.MultiplySigned320(
 1342152271                value,
 1342152272                weight));
 273
 274    internal static void GetProductSums(in TriangleCoordinates triangle,
 275        out Signed320 x2, out Signed320 y2, out Signed320 z2, out Signed320 xy, out Signed320 xz, out Signed320 yz)
 276    {
 527976277        Signed192 ax = triangle.AX, ay = triangle.AY, az = triangle.AZ;
 527976278        Signed192 bx = triangle.BX, by = triangle.BY, bz = triangle.BZ;
 527976279        Signed192 cx = triangle.CX, cy = triangle.CY, cz = triangle.CZ;
 175992280        x2 = SquaredBarycentricSum(ax, bx, cx);
 175992281        y2 = SquaredBarycentricSum(ay, by, cy);
 175992282        z2 = SquaredBarycentricSum(az, bz, cz);
 175992283        xy = BarycentricCrossSum(ax, bx, cx, ay, by, cy);
 175992284        xz = BarycentricCrossSum(ax, bx, cx, az, bz, cz);
 175992285        yz = BarycentricCrossSum(ay, by, cy, az, bz, cz);
 175992286    }
 287
 288    private static Signed320 SquaredBarycentricSum(
 289        Signed192 first,
 290        Signed192 second,
 291        Signed192 third) =>
 527976292        WideArithmetic.AddSigned320(
 527976293            WideArithmetic.AddSigned320(
 527976294                WideArithmetic.AddSigned320(
 527976295                    WideArithmetic.MultiplySigned192(first, first),
 527976296                    WideArithmetic.MultiplySigned192(second, second)),
 527976297                WideArithmetic.AddSigned320(
 527976298                    WideArithmetic.MultiplySigned192(third, third),
 527976299                    WideArithmetic.MultiplySigned192(first, second))),
 527976300            WideArithmetic.AddSigned320(
 527976301                WideArithmetic.MultiplySigned192(first, third),
 527976302                WideArithmetic.MultiplySigned192(second, third)));
 303
 304    private static Signed320 BarycentricCrossSum(
 305        Signed192 firstA,
 306        Signed192 firstB,
 307        Signed192 firstC,
 308        Signed192 secondA,
 309        Signed192 secondB,
 310        Signed192 secondC)
 311    {
 527976312        Signed192 firstSum = Signed192.NarrowProven(
 527976313            WideArithmetic.AddSigned320(
 527976314                WideArithmetic.AddSigned320(
 527976315                    Signed320.ExtendValue(firstA),
 527976316                    Signed320.ExtendValue(firstB)),
 527976317                Signed320.ExtendValue(firstC)));
 527976318        Signed192 secondSum = Signed192.NarrowProven(
 527976319            WideArithmetic.AddSigned320(
 527976320                WideArithmetic.AddSigned320(
 527976321                    Signed320.ExtendValue(secondA),
 527976322                    Signed320.ExtendValue(secondB)),
 527976323                Signed320.ExtendValue(secondC)));
 527976324        Signed320 matching = WideArithmetic.AddSigned320(
 527976325            WideArithmetic.AddSigned320(
 527976326                WideArithmetic.MultiplySigned192(firstA, secondA),
 527976327                WideArithmetic.MultiplySigned192(firstB, secondB)),
 527976328            WideArithmetic.MultiplySigned192(firstC, secondC));
 527976329        return WideArithmetic.AddSigned320(
 527976330            WideArithmetic.MultiplySigned192(
 527976331                firstSum,
 527976332                secondSum),
 527976333            matching);
 334    }
 335
 336    private static Signed576 TranslateSquaredMoment(
 337        Signed576 secondMoment,
 338        Signed576 firstMoment,
 339        Signed576 totalWeight,
 340        Signed192 centerRaw)
 341    {
 14334342        Signed576 shifted = WideArithmetic.SubtractSigned576(
 14334343            secondMoment,
 14334344            WideArithmetic.MultiplySigned576(
 14334345                firstMoment,
 14334346                centerRaw,
 14334347                Signed192.Signed(4)));
 14334348        return WideArithmetic.AddSigned576(
 14334349            shifted,
 14334350            WideArithmetic.MultiplySigned576(
 14334351                totalWeight,
 14334352                centerRaw,
 14334353                centerRaw,
 14334354                Six));
 355    }
 356
 357    private static Signed576 TranslateProductMoment(
 358        Signed576 productMoment,
 359        Signed576 firstMoment,
 360        Signed576 secondMoment,
 361        Signed576 totalWeight,
 362        Signed192 firstCenterRaw,
 363        Signed192 secondCenterRaw)
 364    {
 14334365        Signed576 shifted = WideArithmetic.SubtractSigned576(
 14334366            productMoment,
 14334367            WideArithmetic.MultiplySigned576(
 14334368                secondMoment,
 14334369                firstCenterRaw,
 14334370                Signed192.Signed(4)));
 14334371        shifted = WideArithmetic.SubtractSigned576(
 14334372            shifted,
 14334373            WideArithmetic.MultiplySigned576(
 14334374                firstMoment,
 14334375                secondCenterRaw,
 14334376                Signed192.Signed(4)));
 14334377        return WideArithmetic.AddSigned576(
 14334378            shifted,
 14334379            WideArithmetic.MultiplySigned576(
 14334380                totalWeight,
 14334381                firstCenterRaw,
 14334382                secondCenterRaw,
 14334383                Twelve));
 384    }
 385
 819386    private static Signed192 ScaleCoordinate(Fixed64 value) => Signed192.NarrowValue(
 819387        WideArithmetic.MultiplySigned192(Signed192.Raw(value), new Signed192(0, 1, 0)));
 388
 389
 390
 391    private static void GetTriangle(
 392        ReadOnlySpan<Vector3d> vertices,
 393        ReadOnlySpan<int> triangleIndices,
 394        int index,
 395        out FixedTriangle triangle)
 396    {
 149131397        int first = triangleIndices[index];
 149131398        int second = triangleIndices[index + 1];
 149131399        int third = triangleIndices[index + 2];
 149131400        if ((uint)first >= (uint)vertices.Length
 149131401            || (uint)second >= (uint)vertices.Length
 149131402            || (uint)third >= (uint)vertices.Length)
 403        {
 3404            throw new ArgumentOutOfRangeException(
 3405                nameof(triangleIndices),
 3406                "Triangle indices must reference supplied vertices.");
 407        }
 408
 149128409        triangle = new FixedTriangle(
 149128410            vertices[first],
 149128411            vertices[second],
 149128412            vertices[third]);
 149128413    }
 414}

Methods/Properties

.cctor()
TryCreateUniformShell(System.ReadOnlySpan`1<FixedMathSharp.Vector3d>,System.ReadOnlySpan`1<System.Int32>,Gravitas.Colliders.ExactMassWeight&,FixedMathSharp.Vector3d&,FixedMathSharp.Fixed3x3&,Gravitas.Colliders.MeshMassIntegrals&,FixedMathSharp.Geometry.ScaledRigidFrame3d&,System.Boolean)
AddWeightedVertexSum(FixedMathSharp.Signed576,FixedMathSharp.Signed192,FixedMathSharp.Signed192,FixedMathSharp.Signed192,FixedMathSharp.Signed320)
AddWeighted(FixedMathSharp.Signed576,FixedMathSharp.Signed320,FixedMathSharp.Signed320)
GetProductSums(Gravitas.Colliders.TriangleCoordinates&,FixedMathSharp.Signed320&,FixedMathSharp.Signed320&,FixedMathSharp.Signed320&,FixedMathSharp.Signed320&,FixedMathSharp.Signed320&,FixedMathSharp.Signed320&)
SquaredBarycentricSum(FixedMathSharp.Signed192,FixedMathSharp.Signed192,FixedMathSharp.Signed192)
BarycentricCrossSum(FixedMathSharp.Signed192,FixedMathSharp.Signed192,FixedMathSharp.Signed192,FixedMathSharp.Signed192,FixedMathSharp.Signed192,FixedMathSharp.Signed192)
TranslateSquaredMoment(FixedMathSharp.Signed576,FixedMathSharp.Signed576,FixedMathSharp.Signed576,FixedMathSharp.Signed192)
TranslateProductMoment(FixedMathSharp.Signed576,FixedMathSharp.Signed576,FixedMathSharp.Signed576,FixedMathSharp.Signed576,FixedMathSharp.Signed192,FixedMathSharp.Signed192)
ScaleCoordinate(FixedMathSharp.Fixed64)
GetTriangle(System.ReadOnlySpan`1<FixedMathSharp.Vector3d>,System.ReadOnlySpan`1<System.Int32>,System.Int32,FixedMathSharp.Geometry.FixedTriangle&)