| | | 1 | | //======================================================================= |
| | | 2 | | // CompoundInertiaTensor.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; |
| | | 9 | | using FixedMathSharp; |
| | | 10 | | |
| | | 11 | | namespace Gravitas.Colliders; |
| | | 12 | | |
| | | 13 | | /// <summary>Stores an aggregate's unmaterialized weighted tensor about its requested reference.</summary> |
| | | 14 | | // Full-domain shape weights fit 256 bits and semantic displacements fit 162. |
| | | 15 | | // Squared shifts, factor sixty and an int-bounded part sum stay below 650 bits; |
| | | 16 | | // applying full-domain mass fits 704. Intrinsic/rotation terms fit 576 bits. |
| | | 17 | | internal struct CompoundInertiaTensor |
| | | 18 | | { |
| | | 19 | | internal Signed704 Xx, Yy, Zz, Xy, Xz, Yz; |
| | | 20 | | |
| | | 21 | | internal Fixed3x3 Materialize(Fixed64 mass, ExactMassWeight weight) |
| | | 22 | | { |
| | 2427 | 23 | | Signed576 denominator = WideArithmetic.MultiplySigned320( |
| | 2427 | 24 | | CompoundInertiaMath3D.ScaleWeight(weight), CompoundInertiaMath3D.Q192); |
| | 2427 | 25 | | bool representable = TryMoment(Xx, mass, denominator, out Fixed64 xx) |
| | 2427 | 26 | | & TryMoment(Yy, mass, denominator, out Fixed64 yy) |
| | 2427 | 27 | | & TryMoment(Zz, mass, denominator, out Fixed64 zz) |
| | 2427 | 28 | | & TryMoment(Xy, mass, denominator, out Fixed64 xy) |
| | 2427 | 29 | | & TryMoment(Xz, mass, denominator, out Fixed64 xz) |
| | 2427 | 30 | | & TryMoment(Yz, mass, denominator, out Fixed64 yz); |
| | 2427 | 31 | | if (!representable) |
| | 2 | 32 | | throw new InvalidOperationException("The compound collider's inertia tensor is outside the Fixed64 scalar do |
| | 2425 | 33 | | return new Fixed3x3(xx, xy, xz, xy, yy, yz, xz, yz, zz); |
| | | 34 | | } |
| | | 35 | | |
| | | 36 | | private static bool TryMoment(Signed704 geometry, Fixed64 mass, Signed576 denominator, out Fixed64 moment) => |
| | 14850 | 37 | | Fixed64.TryGetSignedRawRatio( |
| | 14850 | 38 | | WideArithmetic.MultiplySigned704ToSigned832(geometry, Signed192.Raw(mass)), |
| | 14850 | 39 | | Signed832.ExtendValue(denominator), numeratorLeftShift: 0, out moment); |
| | | 40 | | |
| | | 41 | | internal Fixed3x3 MaterializeSaturated(Fixed64 mass, ExactMassWeight weight) |
| | | 42 | | { |
| | 48 | 43 | | Signed576 denominator = WideArithmetic.MultiplySigned320( |
| | 48 | 44 | | CompoundInertiaMath3D.ScaleWeight(weight), CompoundInertiaMath3D.Q192); |
| | 96 | 45 | | Fixed64 xx = SaturatedMoment(Xx, mass, denominator), yy = SaturatedMoment(Yy, mass, denominator); |
| | 96 | 46 | | Fixed64 zz = SaturatedMoment(Zz, mass, denominator), xy = SaturatedMoment(Xy, mass, denominator); |
| | 96 | 47 | | Fixed64 xz = SaturatedMoment(Xz, mass, denominator), yz = SaturatedMoment(Yz, mass, denominator); |
| | 48 | 48 | | return new Fixed3x3(xx, xy, xz, xy, yy, yz, xz, yz, zz); |
| | | 49 | | } |
| | | 50 | | |
| | | 51 | | private static Fixed64 SaturatedMoment(Signed704 geometry, Fixed64 mass, Signed576 denominator) => |
| | 288 | 52 | | TryMoment(geometry, mass, denominator, out Fixed64 moment) ? moment |
| | 288 | 53 | | : (geometry.Sign < 0) != (mass < Fixed64.Zero) ? Fixed64.MinValue : Fixed64.MaxValue; |
| | | 54 | | } |