| | | 1 | | //======================================================================= |
| | | 2 | | // SmallContactNormalSolver.cs |
| | | 3 | | //======================================================================= |
| | | 4 | | // |
| | | 5 | | // MIT License, Copyright (c) 2026-present David Oravsky (mrdav30) |
| | | 6 | | // See LICENSE file in the project root for full license information. |
| | | 7 | | // |
| | | 8 | | //======================================================================= |
| | | 9 | | |
| | | 10 | | using System; |
| | | 11 | | using FixedMathSharp; |
| | | 12 | | |
| | | 13 | | namespace Gravitas.CollisionHandling; |
| | | 14 | | |
| | | 15 | | /// <summary>Solves a bounded unilateral normal response from a small scalar effective-mass matrix.</summary> |
| | | 16 | | internal static class SmallContactNormalSolver |
| | | 17 | | { |
| | | 18 | | internal const int MaxRows = 8; |
| | | 19 | | private const int Satisfied = -1; |
| | | 20 | | private const int Invalid = -2; |
| | | 21 | | |
| | | 22 | | // Q32.32 residual allowance in the units of the supplied right-hand side. |
| | 1 | 23 | | private static readonly Signed192 WideResidualTolerance = WideArithmetic.ShiftLeftSigned192By32( |
| | 1 | 24 | | Signed192.Raw(Fixed64.FromRaw(1L << 12))); |
| | | 25 | | |
| | | 26 | | internal static bool TrySolveTranslations(ReadOnlySpan<Vector3d> lowerAxes, ReadOnlySpan<Vector3d> upperAxes, |
| | | 27 | | Fixed64 lowerInverseMass, Fixed64 upperInverseMass, ReadOnlySpan<Fixed64> depths, Span<Fixed64> impulses) |
| | | 28 | | { |
| | 576 | 29 | | Span<Fixed64> matrix = stackalloc Fixed64[4]; |
| | 576 | 30 | | bool prepared = true; |
| | 3456 | 31 | | for (int i = 0; i < 2; i++) |
| | 5760 | 32 | | for (int j = 0; j <= i; j++) |
| | | 33 | | { |
| | 1728 | 34 | | bool terms = Vector3d.TryDot(lowerAxes[i], lowerAxes[j], out Fixed64 low) |
| | 1728 | 35 | | & Vector3d.TryDot(upperAxes[i], upperAxes[j], out Fixed64 high); |
| | 1728 | 36 | | terms &= Fixed64.TryMultiplyDivide(low, lowerInverseMass, Fixed64.One, out low) |
| | 1728 | 37 | | & Fixed64.TryMultiplyDivide(high, upperInverseMass, Fixed64.One, out high); |
| | 1728 | 38 | | prepared &= terms & Fixed64.TryAdd(low, high, out Fixed64 entry); |
| | 1728 | 39 | | matrix[i * 2 + j] = matrix[j * 2 + i] = entry; |
| | | 40 | | } |
| | | 41 | | |
| | 576 | 42 | | return prepared & TrySolve(2, matrix, depths, impulses); |
| | | 43 | | } |
| | | 44 | | |
| | | 45 | | internal static bool TrySolve(int rowCount, ReadOnlySpan<Fixed64> matrix, |
| | | 46 | | ReadOnlySpan<Fixed64> freeSpeeds, Span<Fixed64> impulses) |
| | | 47 | | { |
| | 1184 | 48 | | if (rowCount is < 1 or > MaxRows || matrix.Length < rowCount * rowCount |
| | 1184 | 49 | | || freeSpeeds.Length < rowCount || impulses.Length < rowCount) |
| | | 50 | | { |
| | 5 | 51 | | return false; |
| | | 52 | | } |
| | | 53 | | |
| | 1179 | 54 | | matrix = matrix[..(rowCount * rowCount)]; |
| | 1179 | 55 | | freeSpeeds = freeSpeeds[..rowCount]; |
| | 1179 | 56 | | Fixed64 maxDiagonal = Fixed64.Zero; |
| | 8252 | 57 | | for (int row = 0; row < rowCount; row++) |
| | | 58 | | { |
| | 2949 | 59 | | Fixed64 diagonal = matrix[row * rowCount + row]; |
| | 2949 | 60 | | if (diagonal <= Fixed64.Zero) |
| | 1 | 61 | | return false; |
| | 2948 | 62 | | if (diagonal > maxDiagonal) |
| | 2315 | 63 | | maxDiagonal = diagonal; |
| | 12828 | 64 | | for (int column = 0; column < row; column++) |
| | 3467 | 65 | | if (matrix[row * rowCount + column] != matrix[column * rowCount + row]) |
| | 1 | 66 | | return false; |
| | | 67 | | } |
| | | 68 | | |
| | | 69 | | // Reject unresolved pivots instead of perturbing the effective mass matrix. |
| | 1177 | 70 | | Fixed64 pivotTolerance = Fixed64.FromRaw(Math.Max(FixedMath.DEFAULT_TOLERANCE_L, |
| | 1177 | 71 | | maxDiagonal.m_rawValue >> 24)); |
| | 1177 | 72 | | int setCount = 1 << rowCount; |
| | 1177 | 73 | | Span<bool> visited = stackalloc bool[setCount]; |
| | 1177 | 74 | | visited.Clear(); |
| | 1177 | 75 | | Span<Fixed64> candidate = stackalloc Fixed64[rowCount]; |
| | 1177 | 76 | | Span<Fixed64> working = stackalloc Fixed64[rowCount * rowCount]; |
| | 1177 | 77 | | Span<Fixed64> speeds = stackalloc Fixed64[rowCount]; |
| | 1177 | 78 | | Span<int> order = stackalloc int[rowCount]; |
| | | 79 | | |
| | 1177 | 80 | | int mask = 0; |
| | 6034 | 81 | | for (int attempt = 0; attempt < 2 * rowCount && !visited[mask]; attempt++) |
| | | 82 | | { |
| | 3016 | 83 | | visited[mask] = true; |
| | 3016 | 84 | | if (!TrySolveActiveRows(matrix, freeSpeeds, mask, pivotTolerance, candidate, working, speeds, order)) |
| | | 85 | | break; |
| | | 86 | | |
| | 3004 | 87 | | int leaving = FindNegativeImpulse(candidate); |
| | 3004 | 88 | | if (leaving >= 0) |
| | | 89 | | { |
| | 5 | 90 | | mask &= ~(1 << leaving); |
| | 5 | 91 | | continue; |
| | | 92 | | } |
| | | 93 | | |
| | 2999 | 94 | | int entering = FindViolatedRow(matrix, freeSpeeds, candidate); |
| | 2999 | 95 | | if (entering == Satisfied) |
| | | 96 | | { |
| | 1162 | 97 | | candidate.CopyTo(impulses); |
| | 1162 | 98 | | return true; |
| | | 99 | | } |
| | 1837 | 100 | | if (entering == Invalid) |
| | | 101 | | break; |
| | 1835 | 102 | | mask |= 1 << entering; |
| | | 103 | | } |
| | | 104 | | |
| | | 105 | | // A semidefinite system can have several valid impulse distributions. |
| | | 106 | | // Try the remaining independent bases in deterministic row-mask order. |
| | 158 | 107 | | for (mask = 1; mask < setCount; mask++) |
| | | 108 | | { |
| | 66 | 109 | | if (visited[mask] |
| | 66 | 110 | | || !TrySolveActiveRows(matrix, freeSpeeds, mask, pivotTolerance, candidate, working, speeds, order) |
| | 66 | 111 | | || FindNegativeImpulse(candidate) >= 0 |
| | 66 | 112 | | || FindViolatedRow(matrix, freeSpeeds, candidate) != Satisfied) |
| | | 113 | | { |
| | | 114 | | continue; |
| | | 115 | | } |
| | | 116 | | |
| | 2 | 117 | | candidate.CopyTo(impulses); |
| | 2 | 118 | | return true; |
| | | 119 | | } |
| | 13 | 120 | | return false; |
| | | 121 | | } |
| | | 122 | | |
| | | 123 | | private static bool TrySolveActiveRows(ReadOnlySpan<Fixed64> matrix, ReadOnlySpan<Fixed64> freeSpeeds, |
| | | 124 | | int mask, Fixed64 pivotTolerance, Span<Fixed64> candidate, Span<Fixed64> working, |
| | | 125 | | Span<Fixed64> speeds, Span<int> order) |
| | | 126 | | { |
| | 3056 | 127 | | candidate.Clear(); |
| | 3056 | 128 | | if (mask == 0) |
| | 1177 | 129 | | return true; |
| | | 130 | | |
| | 1879 | 131 | | int stride = candidate.Length; |
| | 1879 | 132 | | matrix.CopyTo(working); |
| | 1879 | 133 | | freeSpeeds.CopyTo(speeds); |
| | 1879 | 134 | | int count = 0; |
| | 16544 | 135 | | for (int row = 0; row < stride; row++) |
| | 6393 | 136 | | if ((mask & (1 << row)) != 0) |
| | 3602 | 137 | | order[count++] = row; |
| | | 138 | | |
| | 10870 | 139 | | for (int step = 0; step < count; step++) |
| | | 140 | | { |
| | 3582 | 141 | | int best = step; |
| | 12500 | 142 | | for (int i = step + 1; i < count; i++) |
| | 2668 | 143 | | if (working[order[i] * (stride + 1)] > working[order[best] * (stride + 1)]) |
| | 1226 | 144 | | best = i; |
| | 3582 | 145 | | (order[step], order[best]) = (order[best], order[step]); |
| | | 146 | | |
| | 3582 | 147 | | int pivot = order[step]; |
| | 3582 | 148 | | int pivotBase = pivot * stride; |
| | 3582 | 149 | | Fixed64 diagonal = working[pivotBase + pivot]; |
| | 3582 | 150 | | if (diagonal <= pivotTolerance) |
| | 23 | 151 | | return false; |
| | | 152 | | |
| | 12414 | 153 | | for (int i = step + 1; i < count; i++) |
| | | 154 | | { |
| | 2651 | 155 | | int row = order[i]; |
| | 2651 | 156 | | int rowBase = row * stride; |
| | 2651 | 157 | | if (!Fixed64.TryMultiplyDivide(-Fixed64.One, working[rowBase + pivot], diagonal, out Fixed64 factor) |
| | 2651 | 158 | | || !Fixed64.TryMultiplyAdd(factor, speeds[pivot], speeds[row], out speeds[row])) |
| | | 159 | | { |
| | 2 | 160 | | return false; |
| | | 161 | | } |
| | | 162 | | |
| | 13364 | 163 | | for (int j = i; j < count; j++) |
| | | 164 | | { |
| | 4034 | 165 | | int column = order[j]; |
| | 4034 | 166 | | if (!Fixed64.TryMultiplyAdd(factor, working[pivotBase + column], |
| | 4034 | 167 | | working[rowBase + column], out Fixed64 reduced)) |
| | | 168 | | { |
| | 1 | 169 | | return false; |
| | | 170 | | } |
| | 4033 | 171 | | working[rowBase + column] = working[column * stride + row] = reduced; |
| | | 172 | | } |
| | | 173 | | } |
| | | 174 | | } |
| | | 175 | | |
| | 10798 | 176 | | for (int step = count - 1; step >= 0; step--) |
| | | 177 | | { |
| | 3548 | 178 | | int row = order[step]; |
| | 3548 | 179 | | int rowBase = row * stride; |
| | 3548 | 180 | | Fixed64 speed = speeds[row]; |
| | 12372 | 181 | | for (int i = step + 1; i < count; i++) |
| | | 182 | | { |
| | 2639 | 183 | | int column = order[i]; |
| | 2639 | 184 | | if (!Fixed64.TryMultiplyAdd(working[rowBase + column], candidate[column], speed, out speed)) |
| | 1 | 185 | | return false; |
| | | 186 | | } |
| | 3547 | 187 | | if (!Fixed64.TryMultiplyDivide(-Fixed64.One, speed, working[rowBase + row], out candidate[row])) |
| | 1 | 188 | | return false; |
| | | 189 | | } |
| | 1851 | 190 | | return true; |
| | | 191 | | } |
| | | 192 | | |
| | | 193 | | private static int FindNegativeImpulse(ReadOnlySpan<Fixed64> candidate) |
| | | 194 | | { |
| | 3028 | 195 | | Fixed64 lowest = Fixed64.Zero; |
| | 3028 | 196 | | int leaving = -1; |
| | 24560 | 197 | | for (int row = 0; row < candidate.Length; row++) |
| | | 198 | | { |
| | 9252 | 199 | | if (candidate[row] >= lowest) |
| | | 200 | | continue; |
| | 11 | 201 | | lowest = candidate[row]; |
| | 11 | 202 | | leaving = row; |
| | | 203 | | } |
| | 3028 | 204 | | return leaving; |
| | | 205 | | } |
| | | 206 | | |
| | | 207 | | private static int FindViolatedRow(ReadOnlySpan<Fixed64> matrix, ReadOnlySpan<Fixed64> freeSpeeds, |
| | | 208 | | ReadOnlySpan<Fixed64> impulses) |
| | | 209 | | { |
| | 3018 | 210 | | int stride = freeSpeeds.Length; |
| | 3018 | 211 | | Signed192 negativeTolerance = WideArithmetic.SubtractSigned192(default, WideResidualTolerance); |
| | 3018 | 212 | | Signed192 lowest = negativeTolerance; |
| | 3018 | 213 | | int entering = Satisfied; |
| | 24436 | 214 | | for (int row = 0; row < stride; row++) |
| | | 215 | | { |
| | | 216 | | // One exact sum keeps rounding in the solve from masking a closing row. |
| | 9202 | 217 | | Signed192 speed = WideArithmetic.ShiftLeftSigned192By32(Signed192.Raw(freeSpeeds[row])); |
| | 98456 | 218 | | for (int column = 0; column < stride; column++) |
| | 40026 | 219 | | speed = WideArithmetic.AddSigned192(speed, |
| | 40026 | 220 | | Fixed64.GetExactRawProduct(matrix[row * stride + column], impulses[column])); |
| | | 221 | | |
| | 9202 | 222 | | if (impulses[row] > Fixed64.Zero) |
| | | 223 | | { |
| | 3513 | 224 | | if (WideArithmetic.SubtractSigned192(speed, negativeTolerance).Sign < 0 |
| | 3513 | 225 | | || WideArithmetic.SubtractSigned192(speed, WideResidualTolerance).Sign > 0) |
| | | 226 | | { |
| | 2 | 227 | | return Invalid; |
| | | 228 | | } |
| | | 229 | | } |
| | 5689 | 230 | | else if (WideArithmetic.SubtractSigned192(speed, lowest).Sign < 0) |
| | | 231 | | { |
| | 2066 | 232 | | lowest = speed; |
| | 2066 | 233 | | entering = row; |
| | | 234 | | } |
| | | 235 | | } |
| | 3016 | 236 | | return entering; |
| | | 237 | | } |
| | | 238 | | } |