< Summary

Information
Class: Gravitas.Colliders.ExactMassProperties
Assembly: Gravitas
File(s): /home/runner/work/Gravitas/Gravitas/src/Gravitas/Colliders/MassProperties/ExactMassProperties.cs
Line coverage
100%
Covered lines: 445
Uncovered lines: 0
Coverable lines: 445
Total lines: 690
Line coverage: 100%
Branch coverage
100%
Covered branches: 66
Total branches: 66
Branch coverage: 100%
Method coverage

Feature is only available for sponsors

Upgrade to PRO version

Metrics

File(s)

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

#LineLine coverage
 1//=======================================================================
 2// ExactMassProperties.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 exact bounded arithmetic for semantic mass-property values.
 17/// </summary>
 18internal static class ExactMassProperties
 19{
 220    private static readonly Signed192 One = Signed192.One;
 221    private static readonly Signed320 Point3dDenominator =
 222        WideArithmetic.MultiplySigned192(One, One);
 223    private static readonly Signed576 WeightMeasureDenominator =
 224        PowerOfOne(3);
 25    // A common factor of six preserves tetrahedron volumes without dividing
 26    // their determinant before compound moments have been normalized.
 227    private static readonly Signed192 WeightFactor = Signed192.Signed(6);
 228    private static readonly Signed576 ParallelAxis2dDenominator =
 229        PowerOfOne(4);
 230    private static readonly Signed576 ParallelAxis3dDenominator =
 231        PowerOfOne(6);
 232    private static readonly Signed192 BoxMomentDenominator = Signed192.NarrowValue(
 233        WideArithmetic.MultiplySigned192(Fixed64.GetExactRawProduct(Fixed64.One, Fixed64.One), Signed192.Signed(3)));
 34
 35    // Geometry retains Q96 products until mass is applied. Three positive raw
 36    // factors fit 192 bits; applying a full-domain mass fits 320 bits.
 37    internal static Signed192 InertiaGeometryTerm(
 38        Fixed64 coefficient, Fixed64 first, Fixed64 second) =>
 5501939        Signed192.NarrowValue(WideArithmetic.MultiplySigned192(
 5501940            Signed192.Raw(coefficient), Fixed64.GetExactRawProduct(first, second)));
 41
 42    internal static Fixed64 InertiaMoment(Fixed64 mass, Signed192 geometry) =>
 2227443        MaterializeInertia(WideArithmetic.MultiplySigned192(Signed192.Raw(mass), geometry));
 44
 45    // A box diagonal contains both squared extents. Rounding each /3 term
 46    // separately loses half-even parity even when the complete moment fits.
 47    internal static Fixed64 InertiaBoxMoment(Fixed64 mass, Signed192 firstSquare, Signed192 secondSquare)
 48    {
 1414149        Signed192 squares = WideArithmetic.AddSigned192(firstSquare, secondSquare);
 50        // Two full-domain squared coordinates and one mass need at most 191 bits.
 1414151        Signed192 numerator = Signed192.NarrowValue(WideArithmetic.MultiplySigned192(Signed192.Raw(mass), squares));
 1414152        return Fixed64.TryGetSignedRawRatio(numerator, BoxMomentDenominator, out Fixed64 moment)
 1414153            ? moment : Fixed64.MaxValue;
 54    }
 55
 56    // Cancelled common radius factors leave linear weights. Keeping those
 57    // weights exact avoids quantizing a whole shape's internal component mass.
 58    internal static Fixed64 InertiaWeightedMoment(
 59        Fixed64 mass, Signed192 firstGeometry, Signed192 firstWeight,
 60        Signed192 secondGeometry, Signed192 secondWeight, uint divisor = 1)
 61    {
 795462        Signed320 weightedGeometry = WideArithmetic.AddSigned320(
 795463            WideArithmetic.MultiplySigned192(firstGeometry, firstWeight),
 795464            WideArithmetic.MultiplySigned192(secondGeometry, secondWeight));
 795465        Signed576 numerator = WideArithmetic.MultiplySigned576(
 795466            Signed576.ExtendValue(weightedGeometry), Signed192.Raw(mass));
 795467        Signed192 weight = WideArithmetic.AddSigned192(firstWeight, secondWeight);
 795468        if (divisor != 1)
 324069            weight = Signed192.NarrowValue(WideArithmetic.MultiplySigned192(weight, Signed192.Signed(divisor)));
 795470        Signed320 denominator = WideArithmetic.MultiplySigned192(
 795471            weight, Signed192.NarrowValue(WeightMeasureDenominator));
 72        // Geometry is Q96 and linear weights are Q64: numerator stays below
 73        // 330 bits, denominator below 200 bits, and only final inertia saturates.
 795474        return Fixed64.TryGetSignedRawRatio(numerator, denominator, out Fixed64 moment)
 795475            ? moment : Fixed64.MaxValue;
 76    }
 77
 78    private static Fixed64 MaterializeInertia(Signed320 numerator) =>
 2227479        Fixed64.TryRoundNonnegativeQ128(numerator, out Fixed64 moment)
 2227480            ? moment : Fixed64.MaxValue;
 81
 82    internal static ExactMassWeight CreateWeight(Fixed64 measure)
 83    {
 5384        EnsureNonNegative(measure, nameof(measure));
 5285        return CreateWeightCore(
 5286            measure,
 5287            Fixed64.One,
 5288            Fixed64.One,
 5289            Fixed64.One);
 90    }
 91
 92    internal static ExactMassWeight CreateWeight(
 93        Fixed64 first,
 94        Fixed64 second)
 95    {
 196        EnsureNonNegative(first, nameof(first));
 197        EnsureNonNegative(second, nameof(second));
 198        return CreateWeightCore(
 199            first,
 1100            second,
 1101            Fixed64.One,
 1102            Fixed64.One);
 103    }
 104
 105    internal static ExactMassWeight CreateWeight(
 106        Fixed64 first,
 107        Fixed64 second,
 108        Fixed64 third)
 109    {
 2809110        EnsureNonNegative(first, nameof(first));
 2809111        EnsureNonNegative(second, nameof(second));
 2809112        EnsureNonNegative(third, nameof(third));
 2809113        return CreateWeightCore(
 2809114            first,
 2809115            second,
 2809116            third,
 2809117            Fixed64.One);
 118    }
 119
 120    internal static ExactMassWeight CreateWeight(
 121        Fixed64 first,
 122        Fixed64 second,
 123        Fixed64 third,
 124        Fixed64 fourth)
 125    {
 45476126        EnsureNonNegative(first, nameof(first));
 45476127        EnsureNonNegative(second, nameof(second));
 45476128        EnsureNonNegative(third, nameof(third));
 45476129        EnsureNonNegative(fourth, nameof(fourth));
 45476130        return CreateWeightCore(first, second, third, fourth);
 131    }
 132
 133    internal static ExactMassWeight CreateAreaWeight(
 134        Signed320 signedDoubleArea)
 135    {
 1453136        Signed320 absoluteArea = signedDoubleArea.Sign < 0
 1453137            ? WideArithmetic.SubtractSigned320(
 1453138                default,
 1453139                signedDoubleArea)
 1453140            : signedDoubleArea;
 1453141        Signed320 halfScale = WideArithmetic.MultiplySigned192(
 1453142            Signed192.Raw(Fixed64.Half),
 1453143            Signed192.NarrowValue(WideArithmetic.MultiplySigned192(One, WeightFactor)));
 1453144        return new ExactMassWeight(
 1453145            Signed320.NarrowValue(
 1453146                WideArithmetic.MultiplySigned320(
 1453147                    absoluteArea,
 1453148                    halfScale)));
 149    }
 150
 151    internal static ExactMassWeight CreateTriangleAreaWeight(
 152        Signed320 squaredDoubleArea)
 153    {
 148543154        Signed320 scaledRoot =
 148543155            WideArithmetic.GetFloorSquareRootScaledByFixed64(
 148543156                Signed576.ExtendValue(squaredDoubleArea));
 148543157        Signed576 numerator = WideArithmetic.MultiplySigned320(
 148543158            scaledRoot,
 148543159            Signed320.ExtendValue(
 148543160                Signed192.Raw(Fixed64.Half)));
 148543161        numerator = WideArithmetic.MultiplySigned576(numerator, WeightFactor);
 148543162        return new ExactMassWeight(
 148543163            Signed320.NarrowValue(numerator));
 164    }
 165
 166    internal static bool TryGetMeasure(
 167        ExactMassWeight weight,
 168        out Fixed64 measure) =>
 35169        Fixed64.TryGetSignedRawRatio(
 35170            Signed576.ExtendValue(weight.Numerator),
 35171            WideArithmetic.MultiplySigned576(WeightMeasureDenominator, WeightFactor),
 35172            out measure);
 173
 174    internal static bool TryAdd(
 175        ExactMassWeight first,
 176        ExactMassWeight second,
 177        out ExactMassWeight result)
 178    {
 32711179        Signed576 sum = WideArithmetic.AddSigned576(
 32711180            Signed576.ExtendValue(first.Numerator),
 32711181            Signed576.ExtendValue(second.Numerator));
 32711182        if (!Signed320.TryNarrowSigned(sum, out Signed320 numerator)
 32711183            || numerator.Sign < 0)
 184        {
 2185            result = default;
 2186            return false;
 187        }
 188
 32709189        result = new ExactMassWeight(numerator);
 32709190        return true;
 191    }
 192
 193    internal static bool TryGetProportionalShare(
 194        ExactMassWeight weight,
 195        Fixed64 total,
 196        ExactMassWeight totalWeight,
 197        out Fixed64 share)
 198    {
 7199        if (total < Fixed64.Zero
 7200            || totalWeight.Numerator.Sign <= 0
 7201            || WideArithmetic.CompareNonNegative(
 7202                Signed576.ExtendValue(weight.Numerator),
 7203                Signed576.ExtendValue(totalWeight.Numerator)) > 0)
 204        {
 3205            share = default;
 3206            return false;
 207        }
 208
 4209        Signed576 numerator = WideArithmetic.MultiplySigned320(
 4210            weight.Numerator,
 4211            Signed320.ExtendValue(Signed192.Raw(total)));
 4212        return Fixed64.TryGetSignedRawRatio(
 4213            numerator,
 4214            Signed576.ExtendValue(totalWeight.Numerator),
 4215            out share);
 216    }
 217
 218    internal static ExactMassPoint3D CreatePoint(Vector3d point) =>
 30711219        new(
 30711220            Product(Signed192.Raw(point.X), One, One),
 30711221            Product(Signed192.Raw(point.Y), One, One),
 30711222            Product(Signed192.Raw(point.Z), One, One));
 223
 224    internal static ExactMassPoint3D CreatePoint(
 225        Vector3d outerPoint,
 226        Vector3d outerScale,
 227        Vector3d innerOffset,
 228        Vector3d innerScale,
 229        Vector3d innerDisplacement,
 230        FixedQuaternion innerRotation)
 231    {
 141414232        WideRationalBasis3d basis = new(innerRotation);
 141414233        Signed576 denominator = Signed576.ExtendValue(
 141414234            WideArithmetic.MultiplySigned192(
 141414235                basis.Denominator,
 141414236                Signed192.One));
 141414237        Signed320 x = WideArithmetic.GetSignedRatioWith64FractionBits(
 141414238            WideRationalBasis3d.GetComposedScaledCoordinateNumerator(
 141414239                outerPoint.X,
 141414240                outerScale.X,
 141414241                basis.Xx,
 141414242                basis.Yx,
 141414243                basis.Zx,
 141414244                basis.Denominator,
 141414245                innerOffset.X,
 141414246                innerScale.X,
 141414247                innerDisplacement),
 141414248            denominator);
 141414249        Signed320 y = WideArithmetic.GetSignedRatioWith64FractionBits(
 141414250            WideRationalBasis3d.GetComposedScaledCoordinateNumerator(
 141414251                outerPoint.Y,
 141414252                outerScale.Y,
 141414253                basis.Xy,
 141414254                basis.Yy,
 141414255                basis.Zy,
 141414256                basis.Denominator,
 141414257                innerOffset.Y,
 141414258                innerScale.Y,
 141414259                innerDisplacement),
 141414260            denominator);
 141414261        Signed320 z = WideArithmetic.GetSignedRatioWith64FractionBits(
 141414262            WideRationalBasis3d.GetComposedScaledCoordinateNumerator(
 141414263                outerPoint.Z,
 141414264                outerScale.Z,
 141414265                basis.Xz,
 141414266                basis.Yz,
 141414267                basis.Zz,
 141414268                basis.Denominator,
 141414269                innerOffset.Z,
 141414270                innerScale.Z,
 141414271                innerDisplacement),
 141414272            denominator);
 141414273        return new ExactMassPoint3D(x, y, z);
 274    }
 275
 276    internal static ExactMassPoint2D CreatePoint(Vector2d point) =>
 16313277        new(
 16313278            WideArithmetic.MultiplySigned192(Signed192.Raw(point.X), One),
 16313279            WideArithmetic.MultiplySigned192(Signed192.Raw(point.Y), One));
 280
 281    internal static ExactMassPoint2D CreatePoint(
 282        Vector2d outerPoint,
 283        Vector2d outerScale,
 284        Vector2d innerOffset,
 285        Vector2d innerScale,
 286        Vector2d innerDisplacement,
 287        Fixed64 innerRotation)
 288    {
 90578289        Fixed64 cosine = FixedMath.Cos(innerRotation);
 90578290        Fixed64 sine = FixedMath.Sin(innerRotation);
 90578291        Signed320 rotatedX = WideArithmetic.SubtractSigned320(
 90578292            WideArithmetic.MultiplySigned192(
 90578293                Signed192.Raw(innerDisplacement.X),
 90578294                Signed192.Raw(cosine)),
 90578295            WideArithmetic.MultiplySigned192(
 90578296                Signed192.Raw(innerDisplacement.Y),
 90578297                Signed192.Raw(sine)));
 90578298        Signed320 rotatedY = WideArithmetic.AddSigned320(
 90578299            WideArithmetic.MultiplySigned192(
 90578300                Signed192.Raw(innerDisplacement.X),
 90578301                Signed192.Raw(sine)),
 90578302            WideArithmetic.MultiplySigned192(
 90578303                Signed192.Raw(innerDisplacement.Y),
 90578304                Signed192.Raw(cosine)));
 90578305        return new ExactMassPoint2D(
 90578306            WideArithmetic.AddSigned320(
 90578307                WideArithmetic.AddSigned320(
 90578308                    WideArithmetic.MultiplySigned192(
 90578309                        Signed192.Raw(outerPoint.X),
 90578310                        Signed192.Raw(outerScale.X)),
 90578311                    WideArithmetic.MultiplySigned192(
 90578312                        Signed192.Raw(innerOffset.X),
 90578313                        Signed192.Raw(innerScale.X))),
 90578314                rotatedX),
 90578315            WideArithmetic.AddSigned320(
 90578316                WideArithmetic.AddSigned320(
 90578317                    WideArithmetic.MultiplySigned192(
 90578318                        Signed192.Raw(outerPoint.Y),
 90578319                        Signed192.Raw(outerScale.Y)),
 90578320                    WideArithmetic.MultiplySigned192(
 90578321                        Signed192.Raw(innerOffset.Y),
 90578322                        Signed192.Raw(innerScale.Y))),
 90578323                rotatedY));
 324    }
 325
 326    internal static bool TryGetPoint(
 327        ExactMassPoint3D point,
 328        out Vector3d result)
 329    {
 104021330        Signed576 denominator =
 104021331            Signed576.ExtendValue(Point3dDenominator);
 104021332        bool representable = Fixed64.TryGetSignedRawRatio(
 104021333                Signed576.ExtendValue(point.XNumerator),
 104021334                denominator,
 104021335                out Fixed64 x)
 104021336            & Fixed64.TryGetSignedRawRatio(
 104021337                Signed576.ExtendValue(point.YNumerator),
 104021338                denominator,
 104021339                out Fixed64 y)
 104021340            & Fixed64.TryGetSignedRawRatio(
 104021341                Signed576.ExtendValue(point.ZNumerator),
 104021342                denominator,
 104021343                out Fixed64 z);
 104021344        result = representable
 104021345            ? new Vector3d(x, y, z)
 104021346            : default;
 104021347        return representable;
 348    }
 349
 350    internal static bool TryGetPoint(
 351        ExactMassPoint2D point,
 352        out Vector2d result)
 353    {
 59649354        Signed576 denominator =
 59649355            Signed576.ExtendValue(Signed320.ExtendValue(One));
 59649356        bool representable = Fixed64.TryGetSignedRawRatio(
 59649357                Signed576.ExtendValue(point.XNumerator),
 59649358                denominator,
 59649359                out Fixed64 x)
 59649360            & Fixed64.TryGetSignedRawRatio(
 59649361                Signed576.ExtendValue(point.YNumerator),
 59649362                denominator,
 59649363                out Fixed64 y);
 59649364        result = representable
 59649365            ? new Vector2d(x, y)
 59649366            : default;
 59649367        return representable;
 368    }
 369
 370    internal static bool TryGetWeightedAverage(
 371        ReadOnlySpan<ExactMassPoint3D> points,
 372        ReadOnlySpan<ExactMassWeight> weights,
 373        out Vector3d average)
 374    {
 20413375        ValidateWeightedInputs(points.Length, weights.Length);
 20412376        Signed576 totalWeight = default;
 20412377        Signed576 x = default;
 20412378        Signed576 y = default;
 20412379        Signed576 z = default;
 83166380        for (int i = 0; i < points.Length; i++)
 381        {
 21171382            Signed320 weight = weights[i].Numerator;
 21171383            totalWeight = WideArithmetic.AddSigned576(
 21171384                totalWeight,
 21171385                Signed576.ExtendValue(weight));
 21171386            x = WideArithmetic.AddSigned576(
 21171387                x,
 21171388                WideArithmetic.MultiplySigned320(
 21171389                    points[i].XNumerator,
 21171390                    weight));
 21171391            y = WideArithmetic.AddSigned576(
 21171392                y,
 21171393                WideArithmetic.MultiplySigned320(
 21171394                    points[i].YNumerator,
 21171395                    weight));
 21171396            z = WideArithmetic.AddSigned576(
 21171397                z,
 21171398                WideArithmetic.MultiplySigned320(
 21171399                    points[i].ZNumerator,
 21171400                    weight));
 401        }
 402
 20412403        if (totalWeight.Sign <= 0)
 404        {
 1405            average = default;
 1406            return false;
 407        }
 408
 20411409        Signed576 denominator = WideArithmetic.MultiplySigned576(
 20411410            totalWeight,
 20411411            One,
 20411412            One);
 20411413        bool representable = Fixed64.TryGetSignedRawRatio(
 20411414                x,
 20411415                denominator,
 20411416                out Fixed64 resultX)
 20411417            & Fixed64.TryGetSignedRawRatio(
 20411418                y,
 20411419                denominator,
 20411420                out Fixed64 resultY)
 20411421            & Fixed64.TryGetSignedRawRatio(
 20411422                z,
 20411423                denominator,
 20411424                out Fixed64 resultZ);
 20411425        average = representable
 20411426            ? new Vector3d(resultX, resultY, resultZ)
 20411427            : default;
 20411428        return representable;
 429    }
 430
 431    internal static bool TryGetWeightedAverage(
 432        ReadOnlySpan<ExactMassPoint2D> points,
 433        ReadOnlySpan<ExactMassWeight> weights,
 434        out Vector2d average)
 435    {
 1770436        ValidateWeightedInputs(points.Length, weights.Length);
 1770437        Signed576 totalWeight = default;
 1770438        Signed576 x = default;
 1770439        Signed576 y = default;
 8248440        for (int i = 0; i < points.Length; i++)
 441        {
 2354442            Signed320 weight = weights[i].Numerator;
 2354443            totalWeight = WideArithmetic.AddSigned576(
 2354444                totalWeight,
 2354445                Signed576.ExtendValue(weight));
 2354446            x = WideArithmetic.AddSigned576(
 2354447                x,
 2354448                WideArithmetic.MultiplySigned320(
 2354449                    points[i].XNumerator,
 2354450                    weight));
 2354451            y = WideArithmetic.AddSigned576(
 2354452                y,
 2354453                WideArithmetic.MultiplySigned320(
 2354454                    points[i].YNumerator,
 2354455                    weight));
 456        }
 457
 1770458        if (totalWeight.Sign <= 0)
 459        {
 1460            average = default;
 1461            return false;
 462        }
 463
 1769464        Signed576 denominator = WideArithmetic.MultiplySigned576(
 1769465            totalWeight,
 1769466            One);
 1769467        bool representable = Fixed64.TryGetSignedRawRatio(
 1769468                x,
 1769469                denominator,
 1769470                out Fixed64 resultX)
 1769471            & Fixed64.TryGetSignedRawRatio(
 1769472                y,
 1769473                denominator,
 1769474                out Fixed64 resultY);
 1769475        average = representable
 1769476            ? new Vector2d(resultX, resultY)
 1769477            : default;
 1769478        return representable;
 479    }
 480
 481    internal static bool TryAddParallelAxisTensor(
 482        ExactMassPoint3D point,
 483        Fixed3x3 centerTensor,
 484        Fixed64 mass,
 485        Vector3d referencePoint,
 486        out Fixed3x3 tensor)
 487    {
 25884488        if (mass < Fixed64.Zero)
 489        {
 1490            tensor = default;
 1491            return false;
 492        }
 25883493        if (mass == Fixed64.Zero)
 494        {
 1495            tensor = centerTensor;
 1496            return true;
 497        }
 498
 25882499        Signed320 dx = WideArithmetic.SubtractSigned320(
 25882500            point.XNumerator,
 25882501            Product(Signed192.Raw(referencePoint.X), One, One));
 25882502        Signed320 dy = WideArithmetic.SubtractSigned320(
 25882503            point.YNumerator,
 25882504            Product(Signed192.Raw(referencePoint.Y), One, One));
 25882505        Signed320 dz = WideArithmetic.SubtractSigned320(
 25882506            point.ZNumerator,
 25882507            Product(Signed192.Raw(referencePoint.Z), One, One));
 25882508        if (dx.IsZero && dy.IsZero && dz.IsZero)
 509        {
 25853510            tensor = centerTensor;
 25853511            return true;
 512        }
 29513        Signed576 dxSquared = WideArithmetic.MultiplySigned320(dx, dx);
 29514        Signed576 dySquared = WideArithmetic.MultiplySigned320(dy, dy);
 29515        Signed576 dzSquared = WideArithmetic.MultiplySigned320(dz, dz);
 29516        bool representable = TryGetParallelAxisValue(
 29517                WideArithmetic.AddSigned576(dySquared, dzSquared),
 29518                mass,
 29519                ParallelAxis3dDenominator,
 29520                out Fixed64 xx)
 29521            & TryGetParallelAxisValue(
 29522                WideArithmetic.AddSigned576(dxSquared, dzSquared),
 29523                mass,
 29524                ParallelAxis3dDenominator,
 29525                out Fixed64 yy)
 29526            & TryGetParallelAxisValue(
 29527                WideArithmetic.AddSigned576(dxSquared, dySquared),
 29528                mass,
 29529                ParallelAxis3dDenominator,
 29530                out Fixed64 zz)
 29531            & TryGetParallelAxisValue(
 29532                WideArithmetic.MultiplySigned320(dx, dy),
 29533                mass,
 29534                ParallelAxis3dDenominator,
 29535                out Fixed64 xy)
 29536            & TryGetParallelAxisValue(
 29537                WideArithmetic.MultiplySigned320(dx, dz),
 29538                mass,
 29539                ParallelAxis3dDenominator,
 29540                out Fixed64 xz)
 29541            & TryGetParallelAxisValue(
 29542                WideArithmetic.MultiplySigned320(dy, dz),
 29543                mass,
 29544                ParallelAxis3dDenominator,
 29545                out Fixed64 yz);
 29546        if (!representable)
 547        {
 12548            tensor = default;
 12549            return false;
 550        }
 551
 17552        representable = Fixed64.TryAdd(centerTensor.M11, xx, out Fixed64 m11)
 17553            & Fixed64.TrySubtract(centerTensor.M12, xy, out Fixed64 m12)
 17554            & Fixed64.TrySubtract(centerTensor.M13, xz, out Fixed64 m13)
 17555            & Fixed64.TrySubtract(centerTensor.M21, xy, out Fixed64 m21)
 17556            & Fixed64.TryAdd(centerTensor.M22, yy, out Fixed64 m22)
 17557            & Fixed64.TrySubtract(centerTensor.M23, yz, out Fixed64 m23)
 17558            & Fixed64.TrySubtract(centerTensor.M31, xz, out Fixed64 m31)
 17559            & Fixed64.TrySubtract(centerTensor.M32, yz, out Fixed64 m32)
 17560            & Fixed64.TryAdd(centerTensor.M33, zz, out Fixed64 m33);
 17561        tensor = representable
 17562            ? new Fixed3x3(
 17563                m11, m12, m13,
 17564                m21, m22, m23,
 17565                m31, m32, m33)
 17566            : default;
 17567        return representable;
 568    }
 569
 570    internal static bool TryAddParallelAxisMoment(
 571        ExactMassPoint2D point,
 572        Fixed64 centerMoment,
 573        Fixed64 mass,
 574        Vector2d referencePoint,
 575        out Fixed64 moment)
 576    {
 16322577        if (mass < Fixed64.Zero)
 578        {
 1579            moment = default;
 1580            return false;
 581        }
 16321582        if (mass == Fixed64.Zero)
 583        {
 1584            moment = centerMoment;
 1585            return true;
 586        }
 587
 16320588        Signed320 dx = WideArithmetic.SubtractSigned320(
 16320589            point.XNumerator,
 16320590            WideArithmetic.MultiplySigned192(
 16320591                Signed192.Raw(referencePoint.X),
 16320592                One));
 16320593        Signed320 dy = WideArithmetic.SubtractSigned320(
 16320594            point.YNumerator,
 16320595            WideArithmetic.MultiplySigned192(
 16320596                Signed192.Raw(referencePoint.Y),
 16320597                One));
 16320598        if (dx.IsZero && dy.IsZero)
 599        {
 16279600            moment = centerMoment;
 16279601            return true;
 602        }
 41603        Signed576 squaredDistance = WideArithmetic.AddSigned576(
 41604            WideArithmetic.MultiplySigned320(dx, dx),
 41605            WideArithmetic.MultiplySigned320(dy, dy));
 41606        if (!TryGetParallelAxisValue(
 41607                squaredDistance,
 41608                mass,
 41609                ParallelAxis2dDenominator,
 41610                out Fixed64 shift))
 611        {
 12612            moment = default;
 12613            return false;
 614        }
 615
 29616        return Fixed64.TryAdd(centerMoment, shift, out moment);
 617    }
 618
 619    private static ExactMassWeight CreateWeightCore(
 620        Fixed64 first,
 621        Fixed64 second,
 622        Fixed64 third,
 623        Fixed64 fourth)
 624    {
 48338625        Signed576 product = WideArithmetic.MultiplySigned576(
 48338626            Signed576.ExtendValue(
 48338627                Signed320.ExtendValue(Signed192.Raw(first))),
 48338628            Signed192.Raw(second),
 48338629            Signed192.Raw(third),
 48338630            Signed192.Raw(fourth));
 48338631        return new ExactMassWeight(Signed320.NarrowValue(
 48338632            WideArithmetic.MultiplySigned576(product, WeightFactor)));
 633    }
 634
 635    private static bool TryGetParallelAxisValue(
 636        Signed576 squaredValue,
 637        Fixed64 mass,
 638        Signed576 denominator,
 639        out Fixed64 value) =>
 215640        Fixed64.TryGetSignedRawRatio(
 215641            WideArithmetic.MultiplySigned576(
 215642                squaredValue,
 215643                Signed192.Raw(mass)),
 215644            denominator,
 215645            out value);
 646
 647    private static Signed320 Product(
 648        Signed192 first,
 649        Signed192 second,
 650        Signed192 third) =>
 169779651        Signed320.NarrowValue(
 169779652            WideArithmetic.MultiplySigned576(
 169779653                Signed576.ExtendValue(
 169779654                    Signed320.ExtendValue(first)),
 169779655                second,
 169779656                third));
 657
 658    private static Signed576 PowerOfOne(int exponent)
 659    {
 6660        Signed576 result = Signed576.ExtendValue(
 6661            Signed320.ExtendValue(Signed192.Signed(1)));
 64662        for (int i = 0; i < exponent; i++)
 26663            result = WideArithmetic.MultiplySigned576(result, One);
 6664        return result;
 665    }
 666
 667    private static void EnsureNonNegative(
 668        Fixed64 value,
 669        string parameterName)
 670    {
 190386671        if (value < Fixed64.Zero)
 672        {
 1673            throw new ArgumentOutOfRangeException(
 1674                parameterName,
 1675                "Mass-property weight factors cannot be negative.");
 676        }
 190385677    }
 678
 679    private static void ValidateWeightedInputs(
 680        int pointCount,
 681        int weightCount)
 682    {
 22183683        if (pointCount != weightCount)
 684        {
 1685            throw new ArgumentException(
 1686                "Mass points and weights must have the same length.",
 1687                "weights");
 688        }
 22182689    }
 690}

Methods/Properties

.cctor()
InertiaGeometryTerm(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
InertiaMoment(FixedMathSharp.Fixed64,FixedMathSharp.Signed192)
InertiaBoxMoment(FixedMathSharp.Fixed64,FixedMathSharp.Signed192,FixedMathSharp.Signed192)
InertiaWeightedMoment(FixedMathSharp.Fixed64,FixedMathSharp.Signed192,FixedMathSharp.Signed192,FixedMathSharp.Signed192,FixedMathSharp.Signed192,System.UInt32)
MaterializeInertia(FixedMathSharp.Signed320)
CreateWeight(FixedMathSharp.Fixed64)
CreateWeight(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
CreateWeight(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
CreateWeight(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
CreateAreaWeight(FixedMathSharp.Signed320)
CreateTriangleAreaWeight(FixedMathSharp.Signed320)
TryGetMeasure(Gravitas.Colliders.ExactMassWeight,FixedMathSharp.Fixed64&)
TryAdd(Gravitas.Colliders.ExactMassWeight,Gravitas.Colliders.ExactMassWeight,Gravitas.Colliders.ExactMassWeight&)
TryGetProportionalShare(Gravitas.Colliders.ExactMassWeight,FixedMathSharp.Fixed64,Gravitas.Colliders.ExactMassWeight,FixedMathSharp.Fixed64&)
CreatePoint(FixedMathSharp.Vector3d)
CreatePoint(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion)
CreatePoint(FixedMathSharp.Vector2d)
CreatePoint(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64)
TryGetPoint(Gravitas.Colliders.ExactMassPoint3D,FixedMathSharp.Vector3d&)
TryGetPoint(Gravitas.Colliders.ExactMassPoint2D,FixedMathSharp.Vector2d&)
TryGetWeightedAverage(System.ReadOnlySpan`1<Gravitas.Colliders.ExactMassPoint3D>,System.ReadOnlySpan`1<Gravitas.Colliders.ExactMassWeight>,FixedMathSharp.Vector3d&)
TryGetWeightedAverage(System.ReadOnlySpan`1<Gravitas.Colliders.ExactMassPoint2D>,System.ReadOnlySpan`1<Gravitas.Colliders.ExactMassWeight>,FixedMathSharp.Vector2d&)
TryAddParallelAxisTensor(Gravitas.Colliders.ExactMassPoint3D,FixedMathSharp.Fixed3x3,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.Fixed3x3&)
TryAddParallelAxisMoment(Gravitas.Colliders.ExactMassPoint2D,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64&)
CreateWeightCore(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
TryGetParallelAxisValue(FixedMathSharp.Signed576,FixedMathSharp.Fixed64,FixedMathSharp.Signed576,FixedMathSharp.Fixed64&)
Product(FixedMathSharp.Signed192,FixedMathSharp.Signed192,FixedMathSharp.Signed192)
PowerOfOne(System.Int32)
EnsureNonNegative(FixedMathSharp.Fixed64,System.String)
ValidateWeightedInputs(System.Int32,System.Int32)