< Summary

Information
Class: Gravitas.CollisionHandling.SmallContactNormalSolver
Assembly: Gravitas
File(s): /home/runner/work/Gravitas/Gravitas/src/Gravitas/CollisionHandling/Response/SmallContactNormalSolver.cs
Line coverage
100%
Covered lines: 127
Uncovered lines: 0
Coverable lines: 127
Total lines: 238
Line coverage: 100%
Branch coverage
100%
Covered branches: 96
Total branches: 96
Branch coverage: 100%
Method coverage

Feature is only available for sponsors

Upgrade to PRO version

Metrics

MethodBranch coverage Crap Score Cyclomatic complexity Line coverage
.cctor()100%11100%
TrySolveTranslations(...)100%44100%
TrySolve(...)100%4444100%
TrySolveActiveRows(...)100%3232100%
FindNegativeImpulse(...)100%44100%
FindViolatedRow(...)100%1212100%

File(s)

/home/runner/work/Gravitas/Gravitas/src/Gravitas/CollisionHandling/Response/SmallContactNormalSolver.cs

#LineLine coverage
 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
 10using System;
 11using FixedMathSharp;
 12
 13namespace Gravitas.CollisionHandling;
 14
 15/// <summary>Solves a bounded unilateral normal response from a small scalar effective-mass matrix.</summary>
 16internal 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.
 123    private static readonly Signed192 WideResidualTolerance = WideArithmetic.ShiftLeftSigned192By32(
 124        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    {
 57629        Span<Fixed64> matrix = stackalloc Fixed64[4];
 57630        bool prepared = true;
 345631        for (int i = 0; i < 2; i++)
 576032        for (int j = 0; j <= i; j++)
 33        {
 172834            bool terms = Vector3d.TryDot(lowerAxes[i], lowerAxes[j], out Fixed64 low)
 172835                & Vector3d.TryDot(upperAxes[i], upperAxes[j], out Fixed64 high);
 172836            terms &= Fixed64.TryMultiplyDivide(low, lowerInverseMass, Fixed64.One, out low)
 172837                & Fixed64.TryMultiplyDivide(high, upperInverseMass, Fixed64.One, out high);
 172838            prepared &= terms & Fixed64.TryAdd(low, high, out Fixed64 entry);
 172839            matrix[i * 2 + j] = matrix[j * 2 + i] = entry;
 40        }
 41
 57642        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    {
 118448        if (rowCount is < 1 or > MaxRows || matrix.Length < rowCount * rowCount
 118449            || freeSpeeds.Length < rowCount || impulses.Length < rowCount)
 50        {
 551            return false;
 52        }
 53
 117954        matrix = matrix[..(rowCount * rowCount)];
 117955        freeSpeeds = freeSpeeds[..rowCount];
 117956        Fixed64 maxDiagonal = Fixed64.Zero;
 825257        for (int row = 0; row < rowCount; row++)
 58        {
 294959            Fixed64 diagonal = matrix[row * rowCount + row];
 294960            if (diagonal <= Fixed64.Zero)
 161                return false;
 294862            if (diagonal > maxDiagonal)
 231563                maxDiagonal = diagonal;
 1282864            for (int column = 0; column < row; column++)
 346765                if (matrix[row * rowCount + column] != matrix[column * rowCount + row])
 166                    return false;
 67        }
 68
 69        // Reject unresolved pivots instead of perturbing the effective mass matrix.
 117770        Fixed64 pivotTolerance = Fixed64.FromRaw(Math.Max(FixedMath.DEFAULT_TOLERANCE_L,
 117771            maxDiagonal.m_rawValue >> 24));
 117772        int setCount = 1 << rowCount;
 117773        Span<bool> visited = stackalloc bool[setCount];
 117774        visited.Clear();
 117775        Span<Fixed64> candidate = stackalloc Fixed64[rowCount];
 117776        Span<Fixed64> working = stackalloc Fixed64[rowCount * rowCount];
 117777        Span<Fixed64> speeds = stackalloc Fixed64[rowCount];
 117778        Span<int> order = stackalloc int[rowCount];
 79
 117780        int mask = 0;
 603481        for (int attempt = 0; attempt < 2 * rowCount && !visited[mask]; attempt++)
 82        {
 301683            visited[mask] = true;
 301684            if (!TrySolveActiveRows(matrix, freeSpeeds, mask, pivotTolerance, candidate, working, speeds, order))
 85                break;
 86
 300487            int leaving = FindNegativeImpulse(candidate);
 300488            if (leaving >= 0)
 89            {
 590                mask &= ~(1 << leaving);
 591                continue;
 92            }
 93
 299994            int entering = FindViolatedRow(matrix, freeSpeeds, candidate);
 299995            if (entering == Satisfied)
 96            {
 116297                candidate.CopyTo(impulses);
 116298                return true;
 99            }
 1837100            if (entering == Invalid)
 101                break;
 1835102            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.
 158107        for (mask = 1; mask < setCount; mask++)
 108        {
 66109            if (visited[mask]
 66110                || !TrySolveActiveRows(matrix, freeSpeeds, mask, pivotTolerance, candidate, working, speeds, order)
 66111                || FindNegativeImpulse(candidate) >= 0
 66112                || FindViolatedRow(matrix, freeSpeeds, candidate) != Satisfied)
 113            {
 114                continue;
 115            }
 116
 2117            candidate.CopyTo(impulses);
 2118            return true;
 119        }
 13120        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    {
 3056127        candidate.Clear();
 3056128        if (mask == 0)
 1177129            return true;
 130
 1879131        int stride = candidate.Length;
 1879132        matrix.CopyTo(working);
 1879133        freeSpeeds.CopyTo(speeds);
 1879134        int count = 0;
 16544135        for (int row = 0; row < stride; row++)
 6393136            if ((mask & (1 << row)) != 0)
 3602137                order[count++] = row;
 138
 10870139        for (int step = 0; step < count; step++)
 140        {
 3582141            int best = step;
 12500142            for (int i = step + 1; i < count; i++)
 2668143                if (working[order[i] * (stride + 1)] > working[order[best] * (stride + 1)])
 1226144                    best = i;
 3582145            (order[step], order[best]) = (order[best], order[step]);
 146
 3582147            int pivot = order[step];
 3582148            int pivotBase = pivot * stride;
 3582149            Fixed64 diagonal = working[pivotBase + pivot];
 3582150            if (diagonal <= pivotTolerance)
 23151                return false;
 152
 12414153            for (int i = step + 1; i < count; i++)
 154            {
 2651155                int row = order[i];
 2651156                int rowBase = row * stride;
 2651157                if (!Fixed64.TryMultiplyDivide(-Fixed64.One, working[rowBase + pivot], diagonal, out Fixed64 factor)
 2651158                    || !Fixed64.TryMultiplyAdd(factor, speeds[pivot], speeds[row], out speeds[row]))
 159                {
 2160                    return false;
 161                }
 162
 13364163                for (int j = i; j < count; j++)
 164                {
 4034165                    int column = order[j];
 4034166                    if (!Fixed64.TryMultiplyAdd(factor, working[pivotBase + column],
 4034167                            working[rowBase + column], out Fixed64 reduced))
 168                    {
 1169                        return false;
 170                    }
 4033171                    working[rowBase + column] = working[column * stride + row] = reduced;
 172                }
 173            }
 174        }
 175
 10798176        for (int step = count - 1; step >= 0; step--)
 177        {
 3548178            int row = order[step];
 3548179            int rowBase = row * stride;
 3548180            Fixed64 speed = speeds[row];
 12372181            for (int i = step + 1; i < count; i++)
 182            {
 2639183                int column = order[i];
 2639184                if (!Fixed64.TryMultiplyAdd(working[rowBase + column], candidate[column], speed, out speed))
 1185                    return false;
 186            }
 3547187            if (!Fixed64.TryMultiplyDivide(-Fixed64.One, speed, working[rowBase + row], out candidate[row]))
 1188                return false;
 189        }
 1851190        return true;
 191    }
 192
 193    private static int FindNegativeImpulse(ReadOnlySpan<Fixed64> candidate)
 194    {
 3028195        Fixed64 lowest = Fixed64.Zero;
 3028196        int leaving = -1;
 24560197        for (int row = 0; row < candidate.Length; row++)
 198        {
 9252199            if (candidate[row] >= lowest)
 200                continue;
 11201            lowest = candidate[row];
 11202            leaving = row;
 203        }
 3028204        return leaving;
 205    }
 206
 207    private static int FindViolatedRow(ReadOnlySpan<Fixed64> matrix, ReadOnlySpan<Fixed64> freeSpeeds,
 208        ReadOnlySpan<Fixed64> impulses)
 209    {
 3018210        int stride = freeSpeeds.Length;
 3018211        Signed192 negativeTolerance = WideArithmetic.SubtractSigned192(default, WideResidualTolerance);
 3018212        Signed192 lowest = negativeTolerance;
 3018213        int entering = Satisfied;
 24436214        for (int row = 0; row < stride; row++)
 215        {
 216            // One exact sum keeps rounding in the solve from masking a closing row.
 9202217            Signed192 speed = WideArithmetic.ShiftLeftSigned192By32(Signed192.Raw(freeSpeeds[row]));
 98456218            for (int column = 0; column < stride; column++)
 40026219                speed = WideArithmetic.AddSigned192(speed,
 40026220                    Fixed64.GetExactRawProduct(matrix[row * stride + column], impulses[column]));
 221
 9202222            if (impulses[row] > Fixed64.Zero)
 223            {
 3513224                if (WideArithmetic.SubtractSigned192(speed, negativeTolerance).Sign < 0
 3513225                    || WideArithmetic.SubtractSigned192(speed, WideResidualTolerance).Sign > 0)
 226                {
 2227                    return Invalid;
 228                }
 229            }
 5689230            else if (WideArithmetic.SubtractSigned192(speed, lowest).Sign < 0)
 231            {
 2066232                lowest = speed;
 2066233                entering = row;
 234            }
 235        }
 3016236        return entering;
 237    }
 238}