| | | 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 | | |
| | | 8 | | using System; |
| | | 9 | | |
| | | 10 | | using FixedMathSharp; |
| | | 11 | | using FixedMathSharp.Geometry; |
| | | 12 | | |
| | | 13 | | namespace Gravitas.Colliders; |
| | | 14 | | |
| | | 15 | | /// <summary> |
| | | 16 | | /// Retains convex polygon area, centroid and centered moment integrals without scalar saturation. |
| | | 17 | | /// </summary> |
| | | 18 | | internal 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 | | { |
| | 1453 | 35 | | Weight = weight; |
| | 1453 | 36 | | Centroid = centroid; |
| | 1453 | 37 | | _centeredIntegral = centeredIntegral; |
| | 1453 | 38 | | _centeredFirstX = centeredFirstX; |
| | 1453 | 39 | | _centeredFirstY = centeredFirstY; |
| | 1453 | 40 | | _momentDenominator = momentDenominator; |
| | 1453 | 41 | | } |
| | | 42 | | |
| | | 43 | | internal static PolygonMassProperties2D Create(ReadOnlySpan<Vector2d> vertices) |
| | | 44 | | { |
| | 1457 | 45 | | if (!WideConvex2dRelations.TryGetSignedDoubleAreaAndCentroid( |
| | 1457 | 46 | | vertices, out Signed320 signedDoubleArea, out Vector2d centroid)) |
| | 4 | 47 | | return default; |
| | | 48 | | |
| | 1453 | 49 | | Signed320 integral = default; |
| | 1453 | 50 | | Signed320 firstX = default; |
| | 1453 | 51 | | Signed320 firstY = default; |
| | 13966 | 52 | | for (int i = 0; i < vertices.Length; i++) |
| | | 53 | | { |
| | 5530 | 54 | | Vector2d first = vertices[i]; |
| | 5530 | 55 | | Vector2d second = vertices[(i + 1) % vertices.Length]; |
| | 5530 | 56 | | Signed192 ax = WideArithmetic.SubtractSigned192( |
| | 5530 | 57 | | Signed192.Raw(first.X), Signed192.Raw(centroid.X)); |
| | 5530 | 58 | | Signed192 ay = WideArithmetic.SubtractSigned192( |
| | 5530 | 59 | | Signed192.Raw(first.Y), Signed192.Raw(centroid.Y)); |
| | 5530 | 60 | | Signed192 bx = WideArithmetic.SubtractSigned192( |
| | 5530 | 61 | | Signed192.Raw(second.X), Signed192.Raw(centroid.X)); |
| | 5530 | 62 | | Signed192 by = WideArithmetic.SubtractSigned192( |
| | 5530 | 63 | | Signed192.Raw(second.Y), Signed192.Raw(centroid.Y)); |
| | 5530 | 64 | | Signed320 cross = WideArithmetic.SubtractSigned320( |
| | 5530 | 65 | | WideArithmetic.MultiplySigned192(ax, by), WideArithmetic.MultiplySigned192(ay, bx)); |
| | 5530 | 66 | | Signed320 term = WideArithmetic.AddSigned320( |
| | 5530 | 67 | | SquaredSum(ax, bx), SquaredSum(ay, by)); |
| | 5530 | 68 | | integral = WideArithmetic.AddSigned320(integral, |
| | 5530 | 69 | | Signed320.NarrowValue(WideArithmetic.MultiplySigned320(cross, term))); |
| | 5530 | 70 | | firstX = WideArithmetic.AddSigned320(firstX, Signed320.NarrowValue( |
| | 5530 | 71 | | WideArithmetic.MultiplySigned320(cross, WideArithmetic.AddSigned192(ax, bx)))); |
| | 5530 | 72 | | firstY = WideArithmetic.AddSigned320(firstY, Signed320.NarrowValue( |
| | 5530 | 73 | | 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. |
| | 1453 | 78 | | Signed320 denominator = Signed320.NarrowValue(WideArithmetic.MultiplySigned576( |
| | 1453 | 79 | | Signed576.ExtendValue(signedDoubleArea), |
| | 1453 | 80 | | Signed192.One, Signed192.One, Signed192.Signed(6))); |
| | 1453 | 81 | | return new PolygonMassProperties2D( |
| | 1453 | 82 | | ExactMassProperties.CreateAreaWeight(signedDoubleArea), centroid, integral, firstX, firstY, denominator); |
| | | 83 | | } |
| | | 84 | | |
| | | 85 | | internal Fixed64 CalculateMoment(Fixed64 mass) |
| | | 86 | | { |
| | 13148 | 87 | | if (_momentDenominator.IsZero) |
| | 2 | 88 | | 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. |
| | 13146 | 92 | | return Fixed64.TryGetSignedRawRatio( |
| | 13146 | 93 | | WideArithmetic.MultiplySigned320(_centeredIntegral, Signed192.Raw(mass)), |
| | 13146 | 94 | | Signed576.ExtendValue(_momentDenominator), out Fixed64 moment) |
| | 13146 | 95 | | ? moment |
| | 13146 | 96 | | : 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. |
| | 65 | 107 | | Signed576 integral = Signed576.ExtendValue(_centeredIntegral); |
| | 65 | 108 | | if (_momentDenominator.Sign < 0) |
| | 22 | 109 | | integral = WideArithmetic.SubtractSigned576(default, integral); |
| | 65 | 110 | | return WideArithmetic.MultiplySigned576(integral, |
| | 65 | 111 | | 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. |
| | 65 | 117 | | Signed576 dot = CalculateFirstMomentDot(rotation, dx, dy); |
| | 65 | 118 | | if (_momentDenominator.Sign < 0) |
| | 22 | 119 | | dot = WideArithmetic.SubtractSigned576(default, dot); |
| | 65 | 120 | | return WideArithmetic.MultiplySigned576(dot, |
| | 65 | 121 | | Signed192.One, Signed192.One, Signed192.Signed(120)); |
| | | 122 | | } |
| | | 123 | | |
| | | 124 | | private Signed576 CalculateFirstMomentDot(Fixed64 rotation, Signed320 dx, Signed320 dy) |
| | | 125 | | { |
| | 247 | 126 | | if (_centeredFirstX.IsZero && _centeredFirstY.IsZero) |
| | 147 | 127 | | 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. |
| | 100 | 132 | | Signed320 firstX = _centeredFirstX; |
| | 100 | 133 | | Signed320 firstY = _centeredFirstY; |
| | 100 | 134 | | Signed192 cosine = Signed192.Raw(FixedMath.Cos(rotation)); |
| | 100 | 135 | | Signed192 sine = Signed192.Raw(FixedMath.Sin(rotation)); |
| | 100 | 136 | | Signed320 rotatedX = Signed320.NarrowValue(WideArithmetic.SubtractSigned576( |
| | 100 | 137 | | WideArithmetic.MultiplySigned320(firstX, cosine), WideArithmetic.MultiplySigned320(firstY, sine))); |
| | 100 | 138 | | Signed320 rotatedY = Signed320.NarrowValue(WideArithmetic.AddSigned576( |
| | 100 | 139 | | 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. |
| | 100 | 142 | | return WideArithmetic.AddSigned576( |
| | 100 | 143 | | 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. |
| | 182 | 152 | | Signed192 areaFactor = WideArithmetic.TruncateSigned320By64(_momentDenominator); |
| | 182 | 153 | | Signed320 squaredDistance = Signed320.NarrowValue(WideArithmetic.AddSigned576( |
| | 182 | 154 | | WideArithmetic.MultiplySigned320(dx, dx), WideArithmetic.MultiplySigned320(dy, dy))); |
| | 182 | 155 | | Signed576 parallelAxis = WideArithmetic.MultiplySigned320(squaredDistance, areaFactor); |
| | 182 | 156 | | Signed576 intrinsic = WideArithmetic.MultiplySigned576( |
| | 182 | 157 | | Signed576.ExtendValue(_centeredIntegral), Signed192.One, Signed192.One); |
| | 182 | 158 | | Signed576 geometry = WideArithmetic.AddSigned576( |
| | 182 | 159 | | WideArithmetic.AddSigned576(intrinsic, parallelAxis), |
| | 182 | 160 | | WideArithmetic.MultiplySigned576(CalculateFirstMomentDot(rotation, dx, dy), 4)); |
| | 182 | 161 | | Signed320 denominator = Signed320.NarrowValue(WideArithmetic.MultiplySigned576( |
| | 182 | 162 | | Signed576.ExtendValue(_momentDenominator), Signed192.One, Signed192.One)); |
| | 182 | 163 | | return Fixed64.TryGetSignedRawRatio( |
| | 182 | 164 | | WideArithmetic.MultiplySigned576(geometry, Signed192.Raw(mass)), |
| | 182 | 165 | | denominator, out moment); |
| | | 166 | | } |
| | | 167 | | |
| | | 168 | | private static Signed320 SquaredSum(Signed192 first, Signed192 second) => |
| | 11060 | 169 | | WideArithmetic.AddSigned320( |
| | 11060 | 170 | | WideArithmetic.AddSigned320(WideArithmetic.MultiplySigned192(first, first), |
| | 11060 | 171 | | WideArithmetic.MultiplySigned192(first, second)), |
| | 11060 | 172 | | WideArithmetic.MultiplySigned192(second, second)); |
| | | 173 | | } |