| | | 1 | | //======================================================================= |
| | | 2 | | // CompoundMomentMath2D.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 | | |
| | | 8 | | using FixedMathSharp; |
| | | 9 | | |
| | | 10 | | namespace Gravitas.Colliders; |
| | | 11 | | |
| | | 12 | | /// <summary> |
| | | 13 | | /// Retains area-weighted intrinsic moments in Q224 with the common factor sixty, |
| | | 14 | | /// so compound mass is applied once after geometry and parallel-axis reduction. |
| | | 15 | | /// </summary> |
| | | 16 | | internal static class CompoundMomentMath2D |
| | | 17 | | { |
| | | 18 | | internal static Signed576 GetWeightedMoment(LSCollider2D part, ExactMassWeight weight, |
| | | 19 | | Fixed64 rotation, Signed320 dx, Signed320 dy, out Signed576 firstMomentContribution) |
| | | 20 | | { |
| | 312 | 21 | | firstMomentContribution = default; |
| | 312 | 22 | | if (part is LSCircleCollider2D circle) |
| | | 23 | | { |
| | 172 | 24 | | Fixed64 radius = circle.ScaledRadius; |
| | 172 | 25 | | return WideArithmetic.MultiplySigned576( |
| | 172 | 26 | | WideArithmetic.MultiplySigned320(weight.Numerator, |
| | 172 | 27 | | ExactMassProperties.InertiaGeometryTerm(Fixed64.Half, radius, radius)), 60); |
| | | 28 | | } |
| | 140 | 29 | | if (part is LSAABBoxCollider2D box) |
| | | 30 | | { |
| | 54 | 31 | | Vector2d half = box.ScaledHalfExtents; |
| | 54 | 32 | | Signed192 squared = WideArithmetic.AddSigned192( |
| | 54 | 33 | | Fixed64.GetExactRawProduct(half.X, half.X), |
| | 54 | 34 | | Fixed64.GetExactRawProduct(half.Y, half.Y)); |
| | 54 | 35 | | return WideArithmetic.MultiplySigned576( |
| | 54 | 36 | | WideArithmetic.MultiplySigned320(weight.Numerator, squared), Signed192.One, Signed192.Signed(20)); |
| | | 37 | | } |
| | 86 | 38 | | if (part is LSPolygonCollider2D polygon) |
| | | 39 | | { |
| | 65 | 40 | | PolygonMassProperties2D properties = polygon.GetMassProperties(); |
| | 65 | 41 | | firstMomentContribution = properties.CalculateWeightedFirstMomentContribution(rotation, dx, dy); |
| | 65 | 42 | | return properties.CalculateWeightedMoment(); |
| | | 43 | | } |
| | | 44 | | |
| | | 45 | | // Authored compound definitions admit only circle, box, polygon and capsule parts. |
| | 21 | 46 | | var capsule = (LSCapsuleCollider2D)part; |
| | 21 | 47 | | capsule.GetMassPropertyGeometry(out Fixed64 radiusValue, out Fixed64 length); |
| | 21 | 48 | | if (length <= Fixed64.Epsilon) |
| | | 49 | | { |
| | 3 | 50 | | return WideArithmetic.MultiplySigned576( |
| | 3 | 51 | | WideArithmetic.MultiplySigned320(weight.Numerator, |
| | 3 | 52 | | ExactMassProperties.InertiaGeometryTerm(Fixed64.Half, radiusValue, radiusValue)), 60); |
| | | 53 | | } |
| | 18 | 54 | | if (radiusValue == Fixed64.Zero) |
| | | 55 | | { |
| | | 56 | | // A collapsed detached stadium has zero geometric area. The compound |
| | | 57 | | // supplies nominal weight, while its intrinsic moment retains the rod limit. |
| | 5 | 58 | | return WideArithmetic.MultiplySigned576( |
| | 5 | 59 | | WideArithmetic.MultiplySigned320(weight.Numerator, |
| | 5 | 60 | | ExactMassProperties.InertiaGeometryTerm(Fixed64.One, length, length)), 5); |
| | | 61 | | } |
| | | 62 | | |
| | 13 | 63 | | ExactMassWeight rectangleWeight = ExactMassWeight.FromProduct(Fixed64.Two, radiusValue, length); |
| | 13 | 64 | | ExactMassWeight capWeight = ExactMassWeight.FromProduct(Fixed64.Pi, radiusValue, radiusValue); |
| | 13 | 65 | | LSCapsuleCollider2D.GetCenteredMomentGeometry(radiusValue, length, |
| | 13 | 66 | | out Signed192 rectangleGeometry, out Signed192 capGeometry); |
| | 13 | 67 | | return WideArithmetic.MultiplySigned576(WideArithmetic.AddSigned576( |
| | 13 | 68 | | WideArithmetic.MultiplySigned320(rectangleWeight.Numerator, rectangleGeometry), |
| | 13 | 69 | | WideArithmetic.MultiplySigned320(capWeight.Numerator, capGeometry)), 5); |
| | | 70 | | } |
| | | 71 | | } |