| | | 1 | | //======================================================================= |
| | | 2 | | // CollisionPositionCorrection.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.Diagnostics; |
| | | 9 | | using FixedMathSharp; |
| | | 10 | | |
| | | 11 | | namespace Gravitas.CollisionHandling; |
| | | 12 | | |
| | | 13 | | /// <summary>Retains compact correction rounding only when mass weighting cannot amplify it.</summary> |
| | | 14 | | internal static class CollisionPositionCorrection |
| | | 15 | | { |
| | | 16 | | internal static bool TryGetCompactScale( |
| | | 17 | | Fixed64 depth, |
| | | 18 | | Fixed64 inverseMassA, |
| | | 19 | | Fixed64 inverseMassB, |
| | | 20 | | out Fixed64 scale) |
| | | 21 | | { |
| | | 22 | | // Callers provide nonnegative inverse masses with a positive total. |
| | | 23 | | // Later mass multiplication must not amplify common-scale or normal rounding. |
| | 58548 | 24 | | if (!Fixed64.TryAdd(inverseMassA, inverseMassB, out Fixed64 totalInverseMass) |
| | 58548 | 25 | | || inverseMassA > Fixed64.One || inverseMassB > Fixed64.One |
| | 58548 | 26 | | || Fixed64.CompareProducts(depth, Fixed64.One, totalInverseMass, Fixed64.MaxValue) > 0) |
| | | 27 | | { |
| | 113 | 28 | | scale = default; |
| | 113 | 29 | | return false; |
| | | 30 | | } |
| | | 31 | | |
| | 58435 | 32 | | scale = depth / totalInverseMass; |
| | | 33 | | // Rounded common scales must also fit when multiplied back by mass. |
| | 58435 | 34 | | return Fixed64.CompareProducts(scale, totalInverseMass, Fixed64.MaxValue, Fixed64.One) <= 0; |
| | | 35 | | } |
| | | 36 | | |
| | | 37 | | internal static Fixed64 GetWeightedDistance( |
| | | 38 | | Fixed64 depth, |
| | | 39 | | Fixed64 inverseMass, |
| | | 40 | | Fixed64 inverseMassA, |
| | | 41 | | Fixed64 inverseMassB) |
| | | 42 | | { |
| | | 43 | | // Callers select one of the two masses; these exact fractions need no wide division. |
| | 263 | 44 | | if (inverseMass == Fixed64.Zero) return Fixed64.Zero; |
| | 226 | 45 | | if (inverseMassA == Fixed64.Zero || inverseMassB == Fixed64.Zero) return depth; |
| | 190 | 46 | | if (inverseMassA == inverseMassB) return depth * Fixed64.Half; |
| | | 47 | | |
| | | 48 | | // A nonnegative mass fraction cannot exceed one, so the weighted depth fits |
| | | 49 | | // even when the shared scale or the sum of inverse masses does not. |
| | 114 | 50 | | bool fits = Fixed64.TryAdd(inverseMassA, inverseMassB, out Fixed64 totalInverseMass) |
| | 114 | 51 | | ? Fixed64.TryMultiplyDivide(depth, inverseMass, totalInverseMass, out Fixed64 distance) |
| | 114 | 52 | | : Fixed64.TryMultiplyDivideBySum( |
| | 114 | 53 | | depth, inverseMass, Fixed64.One, Fixed64.One, |
| | 114 | 54 | | inverseMassA, inverseMassB, Fixed64.Zero, Fixed64.Zero, |
| | 114 | 55 | | out distance); |
| | | 56 | | Debug.Assert(fits); |
| | 114 | 57 | | return distance; |
| | | 58 | | } |
| | | 59 | | } |