| | | 1 | | //======================================================================= |
| | | 2 | | // MeshRuntimeInertia.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 System.Runtime.CompilerServices; |
| | | 9 | | using FixedMathSharp; |
| | | 10 | | using EnclosedCoefficient = FixedMathSharp.EnclosedQ96Coefficient; |
| | | 11 | | |
| | | 12 | | namespace Gravitas.Colliders; |
| | | 13 | | |
| | | 14 | | /// <summary>Retains mesh geometry coefficients and certifies their final mass-applied rounding.</summary> |
| | | 15 | | internal sealed class MeshRuntimeInertia |
| | | 16 | | { |
| | | 17 | | private EnclosedCoefficient _xx, _yy, _zz, _xy, _xz, _yz; |
| | | 18 | | private Signed192 _firstX, _firstY, _firstZ; |
| | | 19 | | private Vector3d _reference; |
| | | 20 | | private bool _firstZero, _hasResult; |
| | | 21 | | private Fixed64 _lastMass; |
| | | 22 | | private Vector3d _lastReference; |
| | | 23 | | private Fixed3x3 _lastTensor; |
| | | 24 | | |
| | | 25 | | internal bool Ready; |
| | | 26 | | internal Vector3d Center; |
| | | 27 | | |
| | | 28 | | internal void Build(in MeshMassIntegrals integrals, Vector3d center) |
| | | 29 | | { |
| | 597 | 30 | | Signed320 denominator = CompoundInertiaMath3D.ScaleWeight(integrals.Weight); |
| | 597 | 31 | | _xx = new EnclosedCoefficient(WideArithmetic.DivideSigned576ToSigned192(integrals.Ixx, denominator), 9); |
| | 597 | 32 | | _yy = new EnclosedCoefficient(WideArithmetic.DivideSigned576ToSigned192(integrals.Iyy, denominator), 9); |
| | 597 | 33 | | _zz = new EnclosedCoefficient(WideArithmetic.DivideSigned576ToSigned192(integrals.Izz, denominator), 9); |
| | 597 | 34 | | _xy = new EnclosedCoefficient(WideArithmetic.DivideSigned576ToSigned192(integrals.Ixy, denominator), 9); |
| | 597 | 35 | | _xz = new EnclosedCoefficient(WideArithmetic.DivideSigned576ToSigned192(integrals.Ixz, denominator), 9); |
| | 597 | 36 | | _yz = new EnclosedCoefficient(WideArithmetic.DivideSigned576ToSigned192(integrals.Iyz, denominator), 9); |
| | 597 | 37 | | _firstX = WideArithmetic.DivideSigned576ToSigned192( |
| | 597 | 38 | | WideArithmetic.MultiplySigned576(integrals.FirstX, Signed192.One, Signed192.One), denominator); |
| | 597 | 39 | | _firstY = WideArithmetic.DivideSigned576ToSigned192( |
| | 597 | 40 | | WideArithmetic.MultiplySigned576(integrals.FirstY, Signed192.One, Signed192.One), denominator); |
| | 597 | 41 | | _firstZ = WideArithmetic.DivideSigned576ToSigned192( |
| | 597 | 42 | | WideArithmetic.MultiplySigned576(integrals.FirstZ, Signed192.One, Signed192.One), denominator); |
| | 597 | 43 | | _firstZero = integrals.FirstX.IsZero && integrals.FirstY.IsZero && integrals.FirstZ.IsZero; |
| | 597 | 44 | | _reference = integrals.Reference; |
| | 597 | 45 | | Center = center; |
| | 597 | 46 | | _hasResult = false; |
| | 597 | 47 | | Ready = true; |
| | 597 | 48 | | } |
| | | 49 | | |
| | | 50 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 51 | | internal Fixed3x3 Calculate(Fixed64 mass, Vector3d reference, in MeshMassIntegrals integrals) |
| | | 52 | | { |
| | 2918 | 53 | | if (_hasResult && mass == _lastMass && reference == _lastReference) |
| | 1157 | 54 | | return _lastTensor; |
| | 1761 | 55 | | Fixed3x3 tensor = CalculateUncached(mass, reference, integrals); |
| | 1761 | 56 | | _lastMass = mass; |
| | 1761 | 57 | | _lastReference = reference; |
| | 1761 | 58 | | _lastTensor = tensor; |
| | 1761 | 59 | | _hasResult = true; |
| | 1761 | 60 | | return tensor; |
| | | 61 | | } |
| | | 62 | | |
| | | 63 | | private Fixed3x3 CalculateUncached(Fixed64 mass, Vector3d reference, in MeshMassIntegrals integrals) |
| | | 64 | | { |
| | | 65 | | // Keep reference translation's wide temporaries off changing-mass calls. |
| | 2738 | 66 | | if (reference != _reference) return CalculateTranslated(mass, reference, integrals); |
| | 784 | 67 | | return TryMaterialize(mass, _xx, _yy, _zz, _xy, _xz, _yz, out Fixed3x3 tensor) |
| | 784 | 68 | | ? tensor : CalculateExact(mass, reference, integrals); |
| | | 69 | | } |
| | | 70 | | |
| | | 71 | | private Fixed3x3 CalculateTranslated(Fixed64 mass, Vector3d reference, in MeshMassIntegrals integrals) |
| | | 72 | | { |
| | 5862 | 73 | | Signed192 xx = _xx.Value, yy = _yy.Value, zz = _zz.Value, xy = _xy.Value, xz = _xz.Value, yz = _yz.Value; |
| | 977 | 74 | | if (!Vector3d.TrySubtract(_reference, reference, out Vector3d delta)) |
| | 8 | 75 | | return CalculateExact(mass, reference, integrals); |
| | 2907 | 76 | | Signed192 dx = Signed192.Raw(delta.X), dy = Signed192.Raw(delta.Y), dz = Signed192.Raw(delta.Z); |
| | 2907 | 77 | | Signed192 x2 = Parallel(delta.X, delta.X), y2 = Parallel(delta.Y, delta.Y), z2 = Parallel(delta.Z, delta.Z); |
| | 969 | 78 | | xx = WideArithmetic.AddSigned192(xx, WideArithmetic.AddSigned192(y2, z2)); |
| | 969 | 79 | | yy = WideArithmetic.AddSigned192(yy, WideArithmetic.AddSigned192(x2, z2)); |
| | 969 | 80 | | zz = WideArithmetic.AddSigned192(zz, WideArithmetic.AddSigned192(x2, y2)); |
| | 969 | 81 | | xy = WideArithmetic.SubtractSigned192(xy, Parallel(delta.X, delta.Y)); |
| | 969 | 82 | | xz = WideArithmetic.SubtractSigned192(xz, Parallel(delta.X, delta.Z)); |
| | 969 | 83 | | yz = WideArithmetic.SubtractSigned192(yz, Parallel(delta.Y, delta.Z)); |
| | 969 | 84 | | if (!_firstZero) |
| | | 85 | | { |
| | 109 | 86 | | xx = WideArithmetic.AddSigned192(xx, Twice(First(dy, _firstY), First(dz, _firstZ))); |
| | 109 | 87 | | yy = WideArithmetic.AddSigned192(yy, Twice(First(dx, _firstX), First(dz, _firstZ))); |
| | 109 | 88 | | zz = WideArithmetic.AddSigned192(zz, Twice(First(dx, _firstX), First(dy, _firstY))); |
| | 109 | 89 | | xy = WideArithmetic.SubtractSigned192(xy, WideArithmetic.AddSigned192(First(dx, _firstY), First(dy, _firstX) |
| | 109 | 90 | | xz = WideArithmetic.SubtractSigned192(xz, WideArithmetic.AddSigned192(First(dx, _firstZ), First(dz, _firstX) |
| | 109 | 91 | | yz = WideArithmetic.SubtractSigned192(yz, WideArithmetic.AddSigned192(First(dy, _firstZ), First(dz, _firstY) |
| | | 92 | | } |
| | 969 | 93 | | return TryMaterialize(mass, new EnclosedCoefficient(xx, 9), new EnclosedCoefficient(yy, 9), |
| | 969 | 94 | | new EnclosedCoefficient(zz, 9), new EnclosedCoefficient(xy, 9), new EnclosedCoefficient(xz, 9), |
| | 969 | 95 | | new EnclosedCoefficient(yz, 9), out Fixed3x3 tensor) |
| | 969 | 96 | | ? tensor : CalculateExact(mass, reference, integrals); |
| | | 97 | | } |
| | | 98 | | |
| | | 99 | | private static bool TryMaterialize(Fixed64 mass, in EnclosedCoefficient xx, in EnclosedCoefficient yy, in EnclosedCo |
| | | 100 | | in EnclosedCoefficient xy, in EnclosedCoefficient xz, in EnclosedCoefficient yz, out Fixed3x3 tensor) |
| | | 101 | | { |
| | | 102 | | // Intrinsic Q96 projection loses <1 unit. Each translated Q128 first |
| | | 103 | | // moment loses <2 Q96 units, so diagonal error is <1+2*(2+2)=9. |
| | | 104 | | // Parallel terms are exact. Coefficients fit 165 bits; mass fits 63. |
| | 1753 | 105 | | bool xFits = xx.TryMultiplyRaw(mass, out long x); |
| | | 106 | | bool yFits; |
| | | 107 | | long y; |
| | 4571 | 108 | | if (xx.Equals(yy)) { y = x; yFits = xFits; } |
| | 344 | 109 | | else yFits = yy.TryMultiplyRaw(mass, out y); |
| | | 110 | | bool zFits; |
| | | 111 | | long z; |
| | 4737 | 112 | | if (xx.Equals(zz)) { z = x; zFits = xFits; } |
| | 261 | 113 | | else zFits = zz.TryMultiplyRaw(mass, out z); |
| | 1753 | 114 | | bool fits = xFits & yFits & zFits |
| | 1753 | 115 | | & xy.TryMultiplyRaw(mass, out long a) |
| | 1753 | 116 | | & xz.TryMultiplyRaw(mass, out long b) |
| | 1753 | 117 | | & yz.TryMultiplyRaw(mass, out long c); |
| | 1753 | 118 | | if (fits) |
| | | 119 | | { |
| | 1737 | 120 | | tensor = new Fixed3x3( |
| | 1737 | 121 | | Fixed64.FromRaw(x), Fixed64.FromRaw(a), Fixed64.FromRaw(b), |
| | 1737 | 122 | | Fixed64.FromRaw(a), Fixed64.FromRaw(y), Fixed64.FromRaw(c), |
| | 1737 | 123 | | Fixed64.FromRaw(b), Fixed64.FromRaw(c), Fixed64.FromRaw(z)); |
| | 1737 | 124 | | return true; |
| | | 125 | | } |
| | 16 | 126 | | tensor = default; |
| | 16 | 127 | | return false; |
| | | 128 | | } |
| | | 129 | | |
| | | 130 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 131 | | private static Fixed3x3 CalculateExact(Fixed64 mass, Vector3d reference, in MeshMassIntegrals integrals) |
| | | 132 | | { |
| | | 133 | | // A half tie or unsupported certificate width still uses every retained bit. |
| | 24 | 134 | | CompoundInertiaTensor exact = default; |
| | 24 | 135 | | CompoundInertiaMath3D.AddMesh(ref exact, integrals, FixedQuaternion.Identity, |
| | 24 | 136 | | ExactMassPoint3D.FromPoint(integrals.Reference), reference); |
| | 24 | 137 | | return exact.MaterializeSaturated(mass, integrals.Weight); |
| | | 138 | | } |
| | | 139 | | |
| | | 140 | | private static Signed192 Parallel(Fixed64 first, Fixed64 second) => |
| | 5814 | 141 | | WideArithmetic.ShiftLeftSigned192By32(Fixed64.GetExactRawProduct(first, second)); |
| | | 142 | | |
| | | 143 | | private static Signed192 First(Signed192 displacement, Signed192 firstMoment) => |
| | 1308 | 144 | | WideArithmetic.TruncateSigned320By64(WideArithmetic.MultiplySigned192(displacement, firstMoment)); |
| | | 145 | | |
| | | 146 | | private static Signed192 Twice(Signed192 first, Signed192 second) |
| | | 147 | | { |
| | 327 | 148 | | Signed192 sum = WideArithmetic.AddSigned192(first, second); |
| | 327 | 149 | | return WideArithmetic.AddSigned192(sum, sum); |
| | | 150 | | } |
| | | 151 | | } |