< Summary

Information
Class: Gravitas.CollisionHandling.DynamicCcdCandidateIndex2D
Assembly: Gravitas
File(s): /home/runner/work/Gravitas/Gravitas/src/Gravitas/CollisionHandling/Continuous/Candidates/DynamicCcdCandidateIndex2D.cs
Line coverage
100%
Covered lines: 163
Uncovered lines: 0
Coverable lines: 163
Total lines: 315
Line coverage: 100%
Branch coverage
100%
Covered branches: 94
Total branches: 94
Branch coverage: 100%
Method coverage

Feature is only available for sponsors

Upgrade to PRO version

Metrics

MethodBranch coverage Crap Score Cyclomatic complexity Line coverage
.ctor(...)100%66100%
get_Count()100%11100%
Clear()100%22100%
Add(...)100%22100%
AddOrUpdate(...)100%88100%
Remove(...)100%88100%
Sort()100%44100%
Query(...)100%1212100%
CreateSweptCircleBounds(...)100%11100%
CreateBoundsBetween(...)100%11100%
IncludeExtent(...)100%66100%
RemoveExtent(...)100%44100%
RebuildExtents()100%22100%
FindFirstCandidateIndex(...)100%44100%
IsOrderedAt(...)100%66100%
HeapSort()100%44100%
SiftDown(...)100%1010100%
Swap(...)100%22100%
Compare(...)100%88100%
.ctor(...)100%11100%
Intersects(...)100%66100%

File(s)

/home/runner/work/Gravitas/Gravitas/src/Gravitas/CollisionHandling/Continuous/Candidates/DynamicCcdCandidateIndex2D.cs

#LineLine coverage
 1//=======================================================================
 2// DynamicCcdCandidateIndex2D.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
 8using System.Runtime.CompilerServices;
 9using FixedMathSharp;
 10using SwiftCollections;
 11
 12namespace Gravitas.CollisionHandling;
 13
 14/// <summary>
 15/// Indexes planar bounds by minimum X for CCD and projected overlap queries.
 16/// </summary>
 17internal sealed class DynamicCcdCandidateIndex2D
 18{
 19    private readonly SwiftList<Entry> _entries;
 20    private readonly SwiftDictionary<int, int>? _entryIndices;
 21    private Fixed64 _maxExtentX;
 22    private int _maxExtentXCount;
 23    private int _unrepresentableExtentXCount;
 3058124    private bool _isSorted = true;
 25
 3058126    public DynamicCcdCandidateIndex2D(int capacity = 0, bool supportsUpdates = false)
 27    {
 3058128        _entries = capacity > 0 ? new SwiftList<Entry>(capacity) : new SwiftList<Entry>();
 3058129        _entryIndices = !supportsUpdates
 3058130            ? null
 3058131            : capacity > 0
 3058132                ? new SwiftDictionary<int, int>(capacity)
 3058133                : new SwiftDictionary<int, int>();
 3058134    }
 35
 149136    public int Count => _entries.Count;
 37
 38    public void Clear()
 39    {
 5001840        _entries.FastClear();
 5001841        _entryIndices?.Clear();
 5001842        _maxExtentX = Fixed64.Zero;
 5001843        _maxExtentXCount = 0;
 5001844        _unrepresentableExtentXCount = 0;
 5001845        _isSorted = true;
 5001846    }
 47
 48    public void Add(int dynamicId, DynamicCcdPlanarBounds bounds)
 49    {
 2910250        _entryIndices?.Add(dynamicId, _entries.Count);
 2910251        _entries.Add(new Entry(dynamicId, bounds));
 2910252        IncludeExtent(bounds.MinX, bounds.MaxX);
 53
 2910254        _isSorted = false;
 2910255    }
 56
 57    public void AddOrUpdate(int dynamicId, DynamicCcdPlanarBounds bounds)
 58    {
 14218459        SwiftDictionary<int, int>? entryIndices = _entryIndices;
 14218460        ChronicleGuard.ThrowIfTrue(
 14218461            entryIndices == null,
 14218462            nameof(DynamicCcdCandidateIndex2D),
 14218463            "Candidate index was not configured for updates.");
 14218464        if (entryIndices.TryGetValue(dynamicId, out int index))
 65        {
 12874566            Entry previous = _entries[index];
 12874567            _entries[index] = new Entry(dynamicId, bounds);
 12874568            bool remainsSorted = _isSorted && IsOrderedAt(index);
 12874569            if (!DynamicCcdExtentMetadata.IsEquivalent(
 12874570                    previous.MinX,
 12874571                    previous.MaxX,
 12874572                    bounds.MinX,
 12874573                    bounds.MaxX))
 74            {
 396275                if (RemoveExtent(previous.MinX, previous.MaxX))
 262776                    RebuildExtents();
 77                else
 133578                    IncludeExtent(bounds.MinX, bounds.MaxX);
 79            }
 80
 12874581            _isSorted = remainsSorted;
 12874582            return;
 83        }
 84
 1343985        Add(dynamicId, bounds);
 1343986    }
 87
 88    public bool Remove(int dynamicId)
 89    {
 358090        SwiftDictionary<int, int>? entryIndices = _entryIndices;
 358091        if (entryIndices == null || !entryIndices.TryGetValue(dynamicId, out int index))
 306892            return false;
 93
 51294        Entry removed = _entries[index];
 51295        int lastIndex = _entries.Count - 1;
 51296        if (index != lastIndex)
 97        {
 12998            Entry moved = _entries[lastIndex];
 12999            _entries[index] = moved;
 129100            entryIndices[moved.DynamicId] = index;
 101        }
 102
 512103        _entries.RemoveAt(lastIndex);
 512104        entryIndices.Remove(dynamicId);
 512105        if (RemoveExtent(removed.MinX, removed.MaxX))
 247106            RebuildExtents();
 512107        _isSorted = false;
 512108        return true;
 109    }
 110
 111    public void Sort()
 112    {
 79910113        if (!_isSorted && _entries.Count > 1)
 2309114            HeapSort();
 115
 79910116        _isSorted = true;
 79910117    }
 118
 119    public void Query(DynamicCcdPlanarBounds queryBounds, SwiftList<int> results)
 120    {
 57108121        results.FastClear();
 57108122        if (_entries.Count == 0)
 1792123            return;
 124
 55316125        Sort();
 55316126        Fixed64 scanMinX = _unrepresentableExtentXCount > 0
 55316127            || !Fixed64.TrySubtract(queryBounds.MinX, _maxExtentX, out Fixed64 representableScanMinX)
 55316128                ? Fixed64.MinValue
 55316129                : representableScanMinX;
 55316130        int index = FindFirstCandidateIndex(scanMinX);
 335326131        for (; index < _entries.Count; index++)
 132        {
 158436133            Entry entry = _entries[index];
 158436134            if (entry.MinX > queryBounds.MaxX)
 135                break;
 136
 140005137            if (entry.Intersects(queryBounds))
 139433138                results.Add(entry.DynamicId);
 139        }
 55316140    }
 141
 142    public static DynamicCcdPlanarBounds CreateSweptCircleBounds(Vector2d start, Vector2d displacement, Fixed64 radius)
 2168143        => CreateBoundsBetween(start, start + displacement, radius);
 144
 145    public static DynamicCcdPlanarBounds CreateBoundsBetween(
 146        Vector2d start,
 147        Vector2d end,
 148        Fixed64 radius)
 149    {
 17879150        Fixed64 minX = FixedMath.Min(start.X, end.X) - radius;
 17879151        Fixed64 maxX = FixedMath.Max(start.X, end.X) + radius;
 17879152        Fixed64 minZ = FixedMath.Min(start.Y, end.Y) - radius;
 17879153        Fixed64 maxZ = FixedMath.Max(start.Y, end.Y) + radius;
 17879154        return new DynamicCcdPlanarBounds(minX, minZ, maxX, maxZ);
 155    }
 156
 157    private void IncludeExtent(Fixed64 minX, Fixed64 maxX)
 158    {
 33274159        if (!Fixed64.TrySubtract(maxX, minX, out Fixed64 extentX))
 160        {
 7161            _unrepresentableExtentXCount++;
 7162            return;
 163        }
 164
 33267165        if (extentX > _maxExtentX)
 166        {
 24352167            _maxExtentX = extentX;
 24352168            _maxExtentXCount = 1;
 169        }
 8915170        else if (extentX == _maxExtentX)
 171        {
 4989172            _maxExtentXCount++;
 173        }
 8915174    }
 175
 176    private bool RemoveExtent(Fixed64 minX, Fixed64 maxX)
 177    {
 4474178        if (!Fixed64.TrySubtract(maxX, minX, out Fixed64 extentX))
 179        {
 2180            _unrepresentableExtentXCount--;
 2181            return false;
 182        }
 183
 4472184        return extentX == _maxExtentX && --_maxExtentXCount == 0;
 185    }
 186
 187    private void RebuildExtents()
 188    {
 2874189        _maxExtentX = Fixed64.Zero;
 2874190        _maxExtentXCount = 0;
 2874191        _unrepresentableExtentXCount = 0;
 11422192        for (int i = 0; i < _entries.Count; i++)
 2837193            IncludeExtent(_entries[i].MinX, _entries[i].MaxX);
 2874194    }
 195
 196    private int FindFirstCandidateIndex(Fixed64 minX)
 197    {
 55316198        int low = 0;
 55316199        int high = _entries.Count;
 181418200        while (low < high)
 201        {
 126102202            int middle = low + ((high - low) >> 1);
 126102203            if (_entries[middle].MinX < minX)
 25019204                low = middle + 1;
 205            else
 101083206                high = middle;
 207        }
 208
 55316209        return low;
 210    }
 211
 212    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 213    private bool IsOrderedAt(int index)
 214    {
 71032215        Entry entry = _entries[index];
 71032216        return (index == 0 || Compare(_entries[index - 1], entry) <= 0)
 71032217            && (index == _entries.Count - 1 || Compare(entry, _entries[index + 1]) <= 0);
 218    }
 219
 220    private void HeapSort()
 221    {
 2309222        int count = _entries.Count;
 13822223        for (int start = (count >> 1) - 1; start >= 0; start--)
 4602224            SiftDown(start, count);
 225
 20748226        for (int end = count - 1; end > 0; end--)
 227        {
 8065228            Swap(0, end);
 8065229            SiftDown(0, end);
 230        }
 2309231    }
 232
 233    private void SiftDown(int root, int count)
 234    {
 18634235        while (true)
 236        {
 31301237            int child = (root << 1) + 1;
 31301238            if (child >= count)
 11304239                return;
 240
 19997241            int swapIndex = root;
 19997242            if (Compare(_entries[swapIndex], _entries[child]) < 0)
 18191243                swapIndex = child;
 244
 19997245            int right = child + 1;
 19997246            if (right < count && Compare(_entries[swapIndex], _entries[right]) < 0)
 7691247                swapIndex = right;
 248
 19997249            if (swapIndex == root)
 1363250                return;
 251
 18634252            Swap(root, swapIndex);
 18634253            root = swapIndex;
 254        }
 255    }
 256
 257    private void Swap(int first, int second)
 258    {
 26699259        Entry firstEntry = _entries[first];
 26699260        Entry secondEntry = _entries[second];
 26699261        _entries[first] = secondEntry;
 26699262        _entries[second] = firstEntry;
 26699263        if (_entryIndices != null)
 264        {
 18568265            _entryIndices[secondEntry.DynamicId] = first;
 18568266            _entryIndices[firstEntry.DynamicId] = second;
 267        }
 26699268    }
 269
 270    private static int Compare(Entry x, Entry y)
 271    {
 155090272        int result = x.MinX.CompareTo(y.MinX);
 155090273        if (result != 0)
 152003274            return result;
 275
 3087276        result = x.MinZ.CompareTo(y.MinZ);
 3087277        if (result != 0)
 2988278            return result;
 279
 99280        result = x.MaxX.CompareTo(y.MaxX);
 99281        if (result != 0)
 11282            return result;
 283
 88284        result = x.MaxZ.CompareTo(y.MaxZ);
 88285        if (result != 0)
 6286            return result;
 287
 82288        return x.DynamicId.CompareTo(y.DynamicId);
 289    }
 290
 291    private readonly struct Entry
 292    {
 293        public Entry(int dynamicId, DynamicCcdPlanarBounds bounds)
 294        {
 157847295            DynamicId = dynamicId;
 157847296            MinX = bounds.MinX;
 157847297            MinZ = bounds.MinZ;
 157847298            MaxX = bounds.MaxX;
 157847299            MaxZ = bounds.MaxZ;
 157847300        }
 301
 302        public int DynamicId { get; }
 303        public Fixed64 MinX { get; }
 304        public Fixed64 MinZ { get; }
 305        public Fixed64 MaxX { get; }
 306        public Fixed64 MaxZ { get; }
 307
 308        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 309        public bool Intersects(DynamicCcdPlanarBounds queryBounds)
 310        {
 140005311            return !(MinX > queryBounds.MaxX || MaxX < queryBounds.MinX ||
 140005312                     MinZ > queryBounds.MaxZ || MaxZ < queryBounds.MinZ);
 313        }
 314    }
 315}