< Summary

Information
Class: Gravitas.Colliders.PolygonMassProperties2D
Assembly: Gravitas
File(s): /home/runner/work/Gravitas/Gravitas/src/Gravitas/Colliders/MassProperties/PolygonMassProperties2D.cs
Line coverage
100%
Covered lines: 86
Uncovered lines: 0
Coverable lines: 86
Total lines: 173
Line coverage: 100%
Branch coverage
100%
Covered branches: 16
Total branches: 16
Branch coverage: 100%
Method coverage

Feature is only available for sponsors

Upgrade to PRO version

Metrics

MethodBranch coverage Crap Score Cyclomatic complexity Line coverage
.ctor(...)100%11100%
Create(...)100%44100%
CalculateMoment(...)100%44100%
CalculateWeightedMoment()100%22100%
CalculateWeightedFirstMomentContribution(...)100%22100%
CalculateFirstMomentDot(...)100%44100%
TryCalculateMoment(...)100%11100%
SquaredSum(...)100%11100%

File(s)

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

#LineLine coverage
 1//=======================================================================
 2// PolygonMassProperties2D.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/// Retains convex polygon area, centroid and centered moment integrals without scalar saturation.
 17/// </summary>
 18internal readonly struct PolygonMassProperties2D
 19{
 20    internal readonly ExactMassWeight Weight;
 21    internal readonly Vector2d Centroid;
 22    private readonly Signed320 _centeredIntegral;
 23    private readonly Signed320 _centeredFirstX;
 24    private readonly Signed320 _centeredFirstY;
 25    private readonly Signed320 _momentDenominator;
 26
 27    private PolygonMassProperties2D(
 28        ExactMassWeight weight,
 29        Vector2d centroid,
 30        Signed320 centeredIntegral,
 31        Signed320 centeredFirstX,
 32        Signed320 centeredFirstY,
 33        Signed320 momentDenominator)
 34    {
 145335        Weight = weight;
 145336        Centroid = centroid;
 145337        _centeredIntegral = centeredIntegral;
 145338        _centeredFirstX = centeredFirstX;
 145339        _centeredFirstY = centeredFirstY;
 145340        _momentDenominator = momentDenominator;
 145341    }
 42
 43    internal static PolygonMassProperties2D Create(ReadOnlySpan<Vector2d> vertices)
 44    {
 145745        if (!WideConvex2dRelations.TryGetSignedDoubleAreaAndCentroid(
 145746                vertices, out Signed320 signedDoubleArea, out Vector2d centroid))
 447            return default;
 48
 145349        Signed320 integral = default;
 145350        Signed320 firstX = default;
 145351        Signed320 firstY = default;
 1396652        for (int i = 0; i < vertices.Length; i++)
 53        {
 553054            Vector2d first = vertices[i];
 553055            Vector2d second = vertices[(i + 1) % vertices.Length];
 553056            Signed192 ax = WideArithmetic.SubtractSigned192(
 553057                Signed192.Raw(first.X), Signed192.Raw(centroid.X));
 553058            Signed192 ay = WideArithmetic.SubtractSigned192(
 553059                Signed192.Raw(first.Y), Signed192.Raw(centroid.Y));
 553060            Signed192 bx = WideArithmetic.SubtractSigned192(
 553061                Signed192.Raw(second.X), Signed192.Raw(centroid.X));
 553062            Signed192 by = WideArithmetic.SubtractSigned192(
 553063                Signed192.Raw(second.Y), Signed192.Raw(centroid.Y));
 553064            Signed320 cross = WideArithmetic.SubtractSigned320(
 553065                WideArithmetic.MultiplySigned192(ax, by), WideArithmetic.MultiplySigned192(ay, bx));
 553066            Signed320 term = WideArithmetic.AddSigned320(
 553067                SquaredSum(ax, bx), SquaredSum(ay, by));
 553068            integral = WideArithmetic.AddSigned320(integral,
 553069                Signed320.NarrowValue(WideArithmetic.MultiplySigned320(cross, term)));
 553070            firstX = WideArithmetic.AddSigned320(firstX, Signed320.NarrowValue(
 553071                WideArithmetic.MultiplySigned320(cross, WideArithmetic.AddSigned192(ax, bx))));
 553072            firstY = WideArithmetic.AddSigned320(firstY, Signed320.NarrowValue(
 553073                WideArithmetic.MultiplySigned320(cross, WideArithmetic.AddSigned192(ay, by))));
 74        }
 75
 76        // Degree-four raw-coordinate products plus an int-bounded vertex count
 77        // fit 320 bits. Retain their ratio until mass is known; density can round to zero.
 145378        Signed320 denominator = Signed320.NarrowValue(WideArithmetic.MultiplySigned576(
 145379            Signed576.ExtendValue(signedDoubleArea),
 145380            Signed192.One, Signed192.One, Signed192.Signed(6)));
 145381        return new PolygonMassProperties2D(
 145382            ExactMassProperties.CreateAreaWeight(signedDoubleArea), centroid, integral, firstX, firstY, denominator);
 83    }
 84
 85    internal Fixed64 CalculateMoment(Fixed64 mass)
 86    {
 1314887        if (_momentDenominator.IsZero)
 288            return Fixed64.Zero;
 89
 90        // I = mass * integral / (6 * signedDoubleArea * Q^2), about the
 91        // published Q32.32 centroid. Winding signs cancel before final rounding.
 1314692        return Fixed64.TryGetSignedRawRatio(
 1314693            WideArithmetic.MultiplySigned320(_centeredIntegral, Signed192.Raw(mass)),
 1314694            Signed576.ExtendValue(_momentDenominator), out Fixed64 moment)
 1314695            ? moment
 1314696            : Fixed64.MaxValue;
 97    }
 98
 99    /// <summary>
 100    /// Gets the centered integral weighted by semantic area and the compound's
 101    /// common moment factor of sixty, retaining Q224 products without division.
 102    /// </summary>
 103    internal Signed576 CalculateWeightedMoment()
 104    {
 105        // Scaled area is 3*|doubleArea|*Q^2. Its area factor cancels
 106        // I/m = integral/(6*doubleArea*Q^2), leaving 30*|integral|*Q^3.
 65107        Signed576 integral = Signed576.ExtendValue(_centeredIntegral);
 65108        if (_momentDenominator.Sign < 0)
 22109            integral = WideArithmetic.SubtractSigned576(default, integral);
 65110        return WideArithmetic.MultiplySigned576(integral,
 65111            Signed192.One, Signed192.One, Signed192.One, Signed192.Signed(30));
 112    }
 113
 114    internal Signed576 CalculateWeightedFirstMomentContribution(Fixed64 rotation, Signed320 dx, Signed320 dy)
 115    {
 116        // Area weighting and the compound factor sixty give 120*Q² times the dot product.
 65117        Signed576 dot = CalculateFirstMomentDot(rotation, dx, dy);
 65118        if (_momentDenominator.Sign < 0)
 22119            dot = WideArithmetic.SubtractSigned576(default, dot);
 65120        return WideArithmetic.MultiplySigned576(dot,
 65121            Signed192.One, Signed192.One, Signed192.Signed(120));
 122    }
 123
 124    private Signed576 CalculateFirstMomentDot(Fixed64 rotation, Signed320 dx, Signed320 dy)
 125    {
 247126        if (_centeredFirstX.IsZero && _centeredFirstY.IsZero)
 147127            return default;
 128
 129        // The published centroid is rounded, so its centered first moments need
 130        // not vanish. With d = centroid-reference, add W|d|² + 2*d·F(c); omitting F loses accuracy
 131        // whenever the requested reference differs from that rounded centroid.
 100132        Signed320 firstX = _centeredFirstX;
 100133        Signed320 firstY = _centeredFirstY;
 100134        Signed192 cosine = Signed192.Raw(FixedMath.Cos(rotation));
 100135        Signed192 sine = Signed192.Raw(FixedMath.Sin(rotation));
 100136        Signed320 rotatedX = Signed320.NarrowValue(WideArithmetic.SubtractSigned576(
 100137            WideArithmetic.MultiplySigned320(firstX, cosine), WideArithmetic.MultiplySigned320(firstY, sine)));
 100138        Signed320 rotatedY = Signed320.NarrowValue(WideArithmetic.AddSigned576(
 100139            WideArithmetic.MultiplySigned320(firstX, sine), WideArithmetic.MultiplySigned320(firstY, cosine)));
 140        // Degree-three edge sums, an int-bounded vertex count, and these fixed
 141        // factors fit 320 bits before rotation and 576 bits after translation.
 100142        return WideArithmetic.AddSigned576(
 100143            WideArithmetic.MultiplySigned320(rotatedX, dx), WideArithmetic.MultiplySigned320(rotatedY, dy));
 144    }
 145
 146    internal bool TryCalculateMoment(Fixed64 mass, Fixed64 rotation,
 147        Signed320 dx, Signed320 dy, out Fixed64 moment)
 148    {
 149        // Cancel compound weighting before evaluation: A=6*signedDoubleAreaRaw,
 150        // N=I*Q²+A*|d|²+4*d·Rot(F), D=A*Q^4, with d in Q64.
 151        // Semantic coordinates fit 130 bits; N*mass fits 576 and D fits 320.
 182152        Signed192 areaFactor = WideArithmetic.TruncateSigned320By64(_momentDenominator);
 182153        Signed320 squaredDistance = Signed320.NarrowValue(WideArithmetic.AddSigned576(
 182154            WideArithmetic.MultiplySigned320(dx, dx), WideArithmetic.MultiplySigned320(dy, dy)));
 182155        Signed576 parallelAxis = WideArithmetic.MultiplySigned320(squaredDistance, areaFactor);
 182156        Signed576 intrinsic = WideArithmetic.MultiplySigned576(
 182157            Signed576.ExtendValue(_centeredIntegral), Signed192.One, Signed192.One);
 182158        Signed576 geometry = WideArithmetic.AddSigned576(
 182159            WideArithmetic.AddSigned576(intrinsic, parallelAxis),
 182160            WideArithmetic.MultiplySigned576(CalculateFirstMomentDot(rotation, dx, dy), 4));
 182161        Signed320 denominator = Signed320.NarrowValue(WideArithmetic.MultiplySigned576(
 182162            Signed576.ExtendValue(_momentDenominator), Signed192.One, Signed192.One));
 182163        return Fixed64.TryGetSignedRawRatio(
 182164            WideArithmetic.MultiplySigned576(geometry, Signed192.Raw(mass)),
 182165            denominator, out moment);
 166    }
 167
 168    private static Signed320 SquaredSum(Signed192 first, Signed192 second) =>
 11060169        WideArithmetic.AddSigned320(
 11060170            WideArithmetic.AddSigned320(WideArithmetic.MultiplySigned192(first, first),
 11060171                WideArithmetic.MultiplySigned192(first, second)),
 11060172            WideArithmetic.MultiplySigned192(second, second));
 173}