< Summary

Information
Class: Gravitas.CollisionHandling.DynamicCcdCandidateIndex
Assembly: Gravitas
File(s): /home/runner/work/Gravitas/Gravitas/src/Gravitas/CollisionHandling/Continuous/Candidates/DynamicCcdCandidateIndex.cs
Line coverage
100%
Covered lines: 169
Uncovered lines: 0
Coverable lines: 169
Total lines: 329
Line coverage: 100%
Branch coverage
100%
Covered branches: 102
Total branches: 102
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%
CreateSweptSphereBounds(...)100%11100%
CreateSweptBounds(...)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%1212100%
.ctor(...)100%11100%
Intersects(...)100%1010100%

File(s)

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

#LineLine coverage
 1//=======================================================================
 2// DynamicCcdCandidateIndex.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;
 11using SwiftCollections.Query;
 12
 13namespace Gravitas.CollisionHandling;
 14
 15/// <summary>Indexes swept 3D bounds by minimum X for broad-phase CCD candidate queries.</summary>
 16internal sealed class DynamicCcdCandidateIndex
 17{
 18    private readonly SwiftList<Entry> _entries;
 19    private readonly SwiftDictionary<int, int>? _entryIndices;
 20    private Fixed64 _maxExtentX;
 21    private int _maxExtentXCount;
 22    private int _unrepresentableExtentXCount;
 4077223    private bool _isSorted = true;
 24
 4077225    public DynamicCcdCandidateIndex(int capacity = 0, bool supportsUpdates = false)
 26    {
 4077227        _entries = capacity > 0 ? new SwiftList<Entry>(capacity) : new SwiftList<Entry>();
 4077228        _entryIndices = !supportsUpdates
 4077229            ? null
 4077230            : capacity > 0
 4077231                ? new SwiftDictionary<int, int>(capacity)
 4077232                : new SwiftDictionary<int, int>();
 4077233    }
 34
 474335    public int Count => _entries.Count;
 36
 37    public void Clear()
 38    {
 17634439        _entries.FastClear();
 17634440        _entryIndices?.Clear();
 17634441        _maxExtentX = Fixed64.Zero;
 17634442        _maxExtentXCount = 0;
 17634443        _unrepresentableExtentXCount = 0;
 17634444        _isSorted = true;
 17634445    }
 46
 47    public void Add(int dynamicId, FixedBoundVolume bounds)
 48    {
 8149949        _entryIndices?.Add(dynamicId, _entries.Count);
 8149950        _entries.Add(new Entry(dynamicId, bounds));
 8149951        IncludeExtent(bounds.Min.X, bounds.Max.X);
 52
 8149953        _isSorted = false;
 8149954    }
 55
 56    public void AddOrUpdate(int dynamicId, FixedBoundVolume bounds)
 57    {
 125358        SwiftDictionary<int, int>? entryIndices = _entryIndices;
 125359        ChronicleGuard.ThrowIfTrue(
 125360            entryIndices == null,
 125361            nameof(DynamicCcdCandidateIndex),
 125362            "Candidate index was not configured for updates.");
 125363        if (entryIndices.TryGetValue(dynamicId, out int index))
 64        {
 32365            Entry previous = _entries[index];
 32366            _entries[index] = new Entry(dynamicId, bounds);
 32367            bool remainsSorted = _isSorted && IsOrderedAt(index);
 32368            if (!DynamicCcdExtentMetadata.IsEquivalent(
 32369                    previous.MinX,
 32370                    previous.MaxX,
 32371                    bounds.Min.X,
 32372                    bounds.Max.X))
 73            {
 10774                if (RemoveExtent(previous.MinX, previous.MaxX))
 8875                    RebuildExtents();
 76                else
 1977                    IncludeExtent(bounds.Min.X, bounds.Max.X);
 78            }
 79
 32380            _isSorted = remainsSorted;
 32381            return;
 82        }
 83
 93084        Add(dynamicId, bounds);
 93085    }
 86
 87    public bool Remove(int dynamicId)
 88    {
 1589        SwiftDictionary<int, int>? entryIndices = _entryIndices;
 1590        if (entryIndices == null || !entryIndices.TryGetValue(dynamicId, out int index))
 691            return false;
 92
 993        Entry removed = _entries[index];
 994        int lastIndex = _entries.Count - 1;
 995        if (index != lastIndex)
 96        {
 397            Entry moved = _entries[lastIndex];
 398            _entries[index] = moved;
 399            entryIndices[moved.DynamicId] = index;
 100        }
 101
 9102        _entries.RemoveAt(lastIndex);
 9103        entryIndices.Remove(dynamicId);
 9104        if (RemoveExtent(removed.MinX, removed.MaxX))
 6105            RebuildExtents();
 9106        _isSorted = false;
 9107        return true;
 108    }
 109
 110    public void Sort()
 111    {
 97275112        if (!_isSorted && _entries.Count > 1)
 3846113            HeapSort();
 114
 97275115        _isSorted = true;
 97275116    }
 117
 118    public void Query(FixedBoundVolume queryBounds, SwiftList<int> results)
 119    {
 35553120        results.FastClear();
 35553121        if (_entries.Count == 0)
 17828122            return;
 123
 17725124        Sort();
 17725125        Fixed64 scanMinX = _unrepresentableExtentXCount > 0
 17725126            || !Fixed64.TrySubtract(queryBounds.Min.X, _maxExtentX, out Fixed64 representableScanMinX)
 17725127                ? Fixed64.MinValue
 17725128                : representableScanMinX;
 17725129        int index = FindFirstCandidateIndex(scanMinX);
 424025130        for (; index < _entries.Count; index++)
 131        {
 214902132            Entry entry = _entries[index];
 214902133            if (entry.MinX > queryBounds.Max.X)
 134                break;
 135
 203150136            if (entry.Intersects(queryBounds))
 18123137                results.Add(entry.DynamicId);
 138        }
 17725139    }
 140
 141    public static FixedBoundVolume CreateSweptSphereBounds(Vector3d start, Vector3d displacement, Fixed64 radius)
 16591142        => CreateBoundsBetween(start, start + displacement, Vector3d.One * radius);
 143
 144    public static FixedBoundVolume CreateSweptBounds(
 145        Vector3d start,
 146        Vector3d displacement,
 147        Vector3d extents) =>
 1148        CreateBoundsBetween(start, start + displacement, extents);
 149
 150    public static FixedBoundVolume CreateBoundsBetween(
 151        Vector3d start,
 152        Vector3d end,
 153        Vector3d extents)
 154    {
 91620155        return new FixedBoundVolume(Vector3d.Min(start, end) - extents, Vector3d.Max(start, end) + extents);
 156    }
 157
 158    private void IncludeExtent(Fixed64 minX, Fixed64 maxX)
 159    {
 81640160        if (!Fixed64.TrySubtract(maxX, minX, out Fixed64 extentX))
 161        {
 6162            _unrepresentableExtentXCount++;
 6163            return;
 164        }
 165
 81634166        if (extentX > _maxExtentX)
 167        {
 64889168            _maxExtentX = extentX;
 64889169            _maxExtentXCount = 1;
 170        }
 16745171        else if (extentX == _maxExtentX)
 172        {
 12217173            _maxExtentXCount++;
 174        }
 16745175    }
 176
 177    private bool RemoveExtent(Fixed64 minX, Fixed64 maxX)
 178    {
 116179        if (!Fixed64.TrySubtract(maxX, minX, out Fixed64 extentX))
 180        {
 2181            _unrepresentableExtentXCount--;
 2182            return false;
 183        }
 184
 114185        return extentX == _maxExtentX && --_maxExtentXCount == 0;
 186    }
 187
 188    private void RebuildExtents()
 189    {
 94190        _maxExtentX = Fixed64.Zero;
 94191        _maxExtentXCount = 0;
 94192        _unrepresentableExtentXCount = 0;
 432193        for (int i = 0; i < _entries.Count; i++)
 122194            IncludeExtent(_entries[i].MinX, _entries[i].MaxX);
 94195    }
 196
 197    private int FindFirstCandidateIndex(Fixed64 minX)
 198    {
 17725199        int low = 0;
 17725200        int high = _entries.Count;
 123362201        while (low < high)
 202        {
 105637203            int middle = low + ((high - low) >> 1);
 105637204            if (_entries[middle].MinX < minX)
 67748205                low = middle + 1;
 206            else
 37889207                high = middle;
 208        }
 209
 17725210        return low;
 211    }
 212
 213    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 214    private bool IsOrderedAt(int index)
 215    {
 252216        Entry entry = _entries[index];
 252217        return (index == 0 || Compare(_entries[index - 1], entry) <= 0)
 252218            && (index == _entries.Count - 1 || Compare(entry, _entries[index + 1]) <= 0);
 219    }
 220
 221    private void HeapSort()
 222    {
 3846223        int count = _entries.Count;
 30112224        for (int start = (count >> 1) - 1; start >= 0; start--)
 11210225            SiftDown(start, count);
 226
 45968227        for (int end = count - 1; end > 0; end--)
 228        {
 19138229            Swap(0, end);
 19138230            SiftDown(0, end);
 231        }
 3846232    }
 233
 234    private void SiftDown(int root, int count)
 235    {
 60738236        while (true)
 237        {
 91086238            int child = (root << 1) + 1;
 91086239            if (child >= count)
 26774240                return;
 241
 64312242            int swapIndex = root;
 64312243            if (Compare(_entries[swapIndex], _entries[child]) < 0)
 59014244                swapIndex = child;
 245
 64312246            int right = child + 1;
 64312247            if (right < count && Compare(_entries[swapIndex], _entries[right]) < 0)
 25287248                swapIndex = right;
 249
 64312250            if (swapIndex == root)
 3574251                return;
 252
 60738253            Swap(root, swapIndex);
 60738254            root = swapIndex;
 255        }
 256    }
 257
 258    private void Swap(int first, int second)
 259    {
 79876260        Entry firstEntry = _entries[first];
 79876261        Entry secondEntry = _entries[second];
 79876262        _entries[first] = secondEntry;
 79876263        _entries[second] = firstEntry;
 79876264        if (_entryIndices != null)
 265        {
 128266            _entryIndices[secondEntry.DynamicId] = first;
 128267            _entryIndices[firstEntry.DynamicId] = second;
 268        }
 79876269    }
 270
 271    private static int Compare(Entry x, Entry y)
 272    {
 120004273        int result = x.MinX.CompareTo(y.MinX);
 120004274        if (result != 0)
 97476275            return result;
 276
 22528277        result = x.MinY.CompareTo(y.MinY);
 22528278        if (result != 0)
 844279            return result;
 280
 21684281        result = x.MinZ.CompareTo(y.MinZ);
 21684282        if (result != 0)
 21652283            return result;
 284
 32285        result = x.MaxX.CompareTo(y.MaxX);
 32286        if (result != 0)
 7287            return result;
 288
 25289        result = x.MaxY.CompareTo(y.MaxY);
 25290        if (result != 0)
 2291            return result;
 292
 23293        result = x.MaxZ.CompareTo(y.MaxZ);
 23294        if (result != 0)
 3295            return result;
 296
 20297        return x.DynamicId.CompareTo(y.DynamicId);
 298    }
 299
 300    private readonly struct Entry
 301    {
 302        public Entry(int dynamicId, FixedBoundVolume bounds)
 303        {
 81822304            DynamicId = dynamicId;
 81822305            MinX = bounds.Min.X;
 81822306            MinY = bounds.Min.Y;
 81822307            MinZ = bounds.Min.Z;
 81822308            MaxX = bounds.Max.X;
 81822309            MaxY = bounds.Max.Y;
 81822310            MaxZ = bounds.Max.Z;
 81822311        }
 312
 313        public int DynamicId { get; }
 314        public Fixed64 MinX { get; }
 315        public Fixed64 MinY { get; }
 316        public Fixed64 MinZ { get; }
 317        public Fixed64 MaxX { get; }
 318        public Fixed64 MaxY { get; }
 319        public Fixed64 MaxZ { get; }
 320
 321        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 322        public bool Intersects(FixedBoundVolume queryBounds)
 323        {
 203150324            return !(MinX > queryBounds.Max.X || MaxX < queryBounds.Min.X ||
 203150325                     MinY > queryBounds.Max.Y || MaxY < queryBounds.Min.Y ||
 203150326                     MinZ > queryBounds.Max.Z || MaxZ < queryBounds.Min.Z);
 327        }
 328    }
 329}