| | | 1 | | //======================================================================= |
| | | 2 | | // MeshMassIntegrals.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>Retains weighted first moments and inertia about a mesh reference before scalar projection.</summary> |
| | | 13 | | internal readonly struct MeshMassIntegrals |
| | | 14 | | { |
| | | 15 | | internal readonly ExactMassWeight Weight; |
| | | 16 | | internal readonly Vector3d Reference; |
| | | 17 | | // First moments use 60*Q192; inertia uses 60*Q224. These factors cancel |
| | | 18 | | // tetrahedron/shell divisors. Exact affine sums project once into these |
| | | 19 | | // guard fractions; final compound normalization retains that finite cache precision. |
| | | 20 | | internal readonly Signed576 FirstX, FirstY, FirstZ; |
| | | 21 | | internal readonly Signed576 Ixx, Iyy, Izz, Ixy, Ixz, Iyz; |
| | | 22 | | |
| | | 23 | | private MeshMassIntegrals(ExactMassWeight weight, Vector3d reference, |
| | | 24 | | Signed576 firstX, Signed576 firstY, Signed576 firstZ, |
| | | 25 | | Signed576 ixx, Signed576 iyy, Signed576 izz, |
| | | 26 | | Signed576 ixy, Signed576 ixz, Signed576 iyz) |
| | | 27 | | { |
| | 5104 | 28 | | Weight = weight; |
| | 5104 | 29 | | Reference = reference; |
| | 15312 | 30 | | FirstX = firstX; FirstY = firstY; FirstZ = firstZ; |
| | 15312 | 31 | | Ixx = ixx; Iyy = iyy; Izz = izz; |
| | 15312 | 32 | | Ixy = ixy; Ixz = ixz; Iyz = iyz; |
| | 5104 | 33 | | } |
| | | 34 | | |
| | | 35 | | internal static MeshMassIntegrals FromSolid(Vector3d reference, Signed576 volume, |
| | | 36 | | Signed576 firstX, Signed576 firstY, Signed576 firstZ, |
| | | 37 | | Signed576 x2, Signed576 y2, Signed576 z2, |
| | | 38 | | Signed576 xy, Signed576 xz, Signed576 yz, int coordinateFractionBits = 0) |
| | | 39 | | { |
| | 326 | 40 | | int sign = volume.Sign; |
| | 326 | 41 | | Signed576 magnitude = sign < 0 ? WideArithmetic.SubtractSigned576(default, volume) : volume; |
| | 326 | 42 | | if (coordinateFractionBits != 0) |
| | | 43 | | { |
| | 6 | 44 | | var exactWeight = new ExactMassWeight(Signed320.NarrowValue(Project(magnitude, 3 * coordinateFractionBits - |
| | 6 | 45 | | return new MeshMassIntegrals(exactWeight, reference, |
| | 6 | 46 | | Project(Scale(firstX, 15 * sign, Signed192.Signed(1)), 4 * coordinateFractionBits - 64), |
| | 6 | 47 | | Project(Scale(firstY, 15 * sign, Signed192.Signed(1)), 4 * coordinateFractionBits - 64), |
| | 6 | 48 | | Project(Scale(firstZ, 15 * sign, Signed192.Signed(1)), 4 * coordinateFractionBits - 64), |
| | 6 | 49 | | Project(Scale(WideArithmetic.AddSigned576(y2, z2), 6 * sign, Signed192.Signed(1)), 5 * coordinateFractio |
| | 6 | 50 | | Project(Scale(WideArithmetic.AddSigned576(x2, z2), 6 * sign, Signed192.Signed(1)), 5 * coordinateFractio |
| | 6 | 51 | | Project(Scale(WideArithmetic.AddSigned576(x2, y2), 6 * sign, Signed192.Signed(1)), 5 * coordinateFractio |
| | 6 | 52 | | Project(Scale(xy, -3 * sign, Signed192.Signed(1)), 5 * coordinateFractionBits - 64), |
| | 6 | 53 | | Project(Scale(xz, -3 * sign, Signed192.Signed(1)), 5 * coordinateFractionBits - 64), |
| | 6 | 54 | | Project(Scale(yz, -3 * sign, Signed192.Signed(1)), 5 * coordinateFractionBits - 64)); |
| | | 55 | | } |
| | 320 | 56 | | var weight = new ExactMassWeight(Signed320.NarrowValue( |
| | 320 | 57 | | WideArithmetic.MultiplySigned576(magnitude, Signed192.One))); |
| | 320 | 58 | | Signed192 qSquared = Signed192.NarrowValue( |
| | 320 | 59 | | WideArithmetic.MultiplySigned192(Signed192.One, Signed192.One)); |
| | 320 | 60 | | return new MeshMassIntegrals(weight, reference, |
| | 320 | 61 | | Scale(firstX, 15 * sign, qSquared), Scale(firstY, 15 * sign, qSquared), Scale(firstZ, 15 * sign, qSquared), |
| | 320 | 62 | | Scale(WideArithmetic.AddSigned576(y2, z2), 6 * sign, qSquared), |
| | 320 | 63 | | Scale(WideArithmetic.AddSigned576(x2, z2), 6 * sign, qSquared), |
| | 320 | 64 | | Scale(WideArithmetic.AddSigned576(x2, y2), 6 * sign, qSquared), |
| | 320 | 65 | | Scale(xy, -3 * sign, qSquared), Scale(xz, -3 * sign, qSquared), Scale(yz, -3 * sign, qSquared)); |
| | | 66 | | } |
| | | 67 | | |
| | | 68 | | internal static MeshMassIntegrals FromShell(ExactMassWeight weight, |
| | | 69 | | Signed576 firstX, Signed576 firstY, Signed576 firstZ, |
| | | 70 | | Signed576 x2, Signed576 y2, Signed576 z2, |
| | | 71 | | Signed576 xy, Signed576 xz, Signed576 yz, int coordinateFractionBits = 0) => |
| | 4778 | 72 | | new(weight, Vector3d.Zero, |
| | 4778 | 73 | | Project(Scale(firstX, 20, Signed192.One), coordinateFractionBits), Project(Scale(firstY, 20, Signed192.One), |
| | 4778 | 74 | | Project(Scale(WideArithmetic.AddSigned576(y2, z2), 10, Signed192.One), 2 * coordinateFractionBits), |
| | 4778 | 75 | | Project(Scale(WideArithmetic.AddSigned576(x2, z2), 10, Signed192.One), 2 * coordinateFractionBits), |
| | 4778 | 76 | | Project(Scale(WideArithmetic.AddSigned576(x2, y2), 10, Signed192.One), 2 * coordinateFractionBits), |
| | 4778 | 77 | | Project(Scale(xy, -5, Signed192.One), 2 * coordinateFractionBits), Project(Scale(xz, -5, Signed192.One), 2 * |
| | | 78 | | |
| | | 79 | | // Public centroid/tensor projection uses unrounded sums. The retained runtime |
| | | 80 | | // cache has fixed Q192/Q224 guard precision, projected only after summation. |
| | | 81 | | internal static Signed576 Project(Signed576 value, int fractionBits) |
| | | 82 | | { |
| | 85245 | 83 | | if (fractionBits == 0) return value; |
| | 879 | 84 | | Signed576 denominator = Signed576.ExtendValue(Signed320.ExtendValue(Signed192.Signed(1))); |
| | 11586 | 85 | | for (int bit = 0; bit < fractionBits + 64; bit += 32) |
| | 4914 | 86 | | denominator = WideArithmetic.MultiplySigned576(denominator, Signed192.One); |
| | | 87 | | // Admitted geometry and int-bounded accumulation keep projected moments |
| | | 88 | | // below 320 bits. The shared ratio owner rounds the complete value once. |
| | 879 | 89 | | return Signed576.ExtendValue(WideArithmetic.GetSignedRatioWith64FractionBits(value, denominator)); |
| | | 90 | | } |
| | | 91 | | |
| | | 92 | | private static Signed576 Scale(Signed576 value, long coefficient, Signed192 padding) => |
| | 45936 | 93 | | WideArithmetic.MultiplySigned576(value, Signed192.Signed(coefficient), padding); |
| | | 94 | | } |