| | | 1 | | //======================================================================= |
| | | 2 | | // GridSpatialIndex.cs |
| | | 3 | | //======================================================================= |
| | | 4 | | // MIT License, Copyright (c) 2024–present David Oravsky (mrdav30) |
| | | 5 | | // See LICENSE file in the project root for full license information. |
| | | 6 | | //======================================================================= |
| | | 7 | | |
| | | 8 | | using FixedMathSharp; |
| | | 9 | | using SwiftCollections; |
| | | 10 | | using SwiftCollections.Query; |
| | | 11 | | |
| | | 12 | | namespace GridForge.Grids; |
| | | 13 | | |
| | | 14 | | /// <summary> |
| | | 15 | | /// Owns deterministic top-level grid spatial classification and lookup. |
| | | 16 | | /// </summary> |
| | | 17 | | internal sealed class GridSpatialIndex |
| | | 18 | | { |
| | | 19 | | private const int InitialCapacity = 16; |
| | | 20 | | internal const ulong DefaultHashCellBudget = 64UL; |
| | | 21 | | |
| | | 22 | | private readonly ulong _cellBudget; |
| | | 23 | | private readonly SwiftFixedSpatialHash<ushort> _ordinaryGrids; |
| | | 24 | | private readonly SwiftFixedBVH<ushort> _oversizedGrids; |
| | | 25 | | private readonly SwiftHashSet<ushort> _oversizedSlots; |
| | | 26 | | private readonly SwiftFixedBVH<ushort> _contactEnvelopes; |
| | | 27 | | private readonly SwiftHashSet<ushort> _contactEnvelopeSlots; |
| | | 28 | | |
| | | 29 | | internal GridSpatialIndex(int cellSize) |
| | 802 | 30 | | : this(cellSize, DefaultHashCellBudget) |
| | | 31 | | { |
| | 802 | 32 | | } |
| | | 33 | | |
| | 820 | 34 | | internal GridSpatialIndex(int cellSize, ulong cellBudget) |
| | | 35 | | { |
| | 820 | 36 | | _cellBudget = cellBudget; |
| | 820 | 37 | | _ordinaryGrids = new SwiftFixedSpatialHash<ushort>(InitialCapacity, (Fixed64)cellSize); |
| | 820 | 38 | | _oversizedGrids = new SwiftFixedBVH<ushort>(InitialCapacity); |
| | 820 | 39 | | _oversizedSlots = new SwiftHashSet<ushort>(); |
| | 820 | 40 | | _contactEnvelopes = new SwiftFixedBVH<ushort>(InitialCapacity); |
| | 820 | 41 | | _contactEnvelopeSlots = new SwiftHashSet<ushort>(); |
| | 820 | 42 | | } |
| | | 43 | | |
| | 5 | 44 | | internal int Count => OrdinaryCount + OversizedCount; |
| | | 45 | | |
| | 10 | 46 | | internal int OrdinaryCount => _ordinaryGrids.Count; |
| | | 47 | | |
| | 10 | 48 | | internal int OversizedCount => _oversizedGrids.Count; |
| | | 49 | | |
| | | 50 | | internal bool Insert(ushort gridIndex, FixedBoundVolume bounds) => |
| | 18 | 51 | | Insert(gridIndex, bounds, bounds); |
| | | 52 | | |
| | | 53 | | internal bool Insert( |
| | | 54 | | ushort gridIndex, |
| | | 55 | | FixedBoundVolume bounds, |
| | | 56 | | FixedBoundVolume? contactEnvelope) |
| | | 57 | | { |
| | 937 | 58 | | if (_ordinaryGrids.Contains(gridIndex) || _oversizedSlots.Contains(gridIndex)) |
| | 2 | 59 | | return false; |
| | | 60 | | |
| | 935 | 61 | | if (FitsHashCellBudget(bounds)) |
| | 903 | 62 | | _ordinaryGrids.Insert(gridIndex, bounds); |
| | | 63 | | else |
| | | 64 | | { |
| | 32 | 65 | | _oversizedGrids.Insert(gridIndex, bounds); |
| | 32 | 66 | | _oversizedSlots.Add(gridIndex); |
| | | 67 | | } |
| | | 68 | | |
| | 935 | 69 | | if (contactEnvelope.HasValue) |
| | | 70 | | { |
| | 921 | 71 | | _contactEnvelopes.Insert(gridIndex, contactEnvelope.Value); |
| | 921 | 72 | | _contactEnvelopeSlots.Add(gridIndex); |
| | | 73 | | } |
| | | 74 | | |
| | 935 | 75 | | return true; |
| | | 76 | | } |
| | | 77 | | |
| | | 78 | | internal bool Remove(ushort gridIndex) |
| | | 79 | | { |
| | | 80 | | bool removed; |
| | 62 | 81 | | if (!_oversizedSlots.Contains(gridIndex)) |
| | 56 | 82 | | removed = _ordinaryGrids.Remove(gridIndex); |
| | | 83 | | else |
| | | 84 | | { |
| | 6 | 85 | | _oversizedGrids.Remove(gridIndex); |
| | 6 | 86 | | _oversizedSlots.Remove(gridIndex); |
| | 6 | 87 | | removed = true; |
| | | 88 | | } |
| | | 89 | | |
| | 62 | 90 | | if (removed && _contactEnvelopeSlots.Remove(gridIndex)) |
| | 59 | 91 | | _contactEnvelopes.Remove(gridIndex); |
| | | 92 | | |
| | 62 | 93 | | return removed; |
| | | 94 | | } |
| | | 95 | | |
| | | 96 | | internal void Clear() |
| | | 97 | | { |
| | 793 | 98 | | _ordinaryGrids.Clear(); |
| | 793 | 99 | | _oversizedGrids.Clear(); |
| | 793 | 100 | | _oversizedSlots.Clear(); |
| | 793 | 101 | | _contactEnvelopes.Clear(); |
| | 793 | 102 | | _contactEnvelopeSlots.Clear(); |
| | 793 | 103 | | } |
| | | 104 | | |
| | | 105 | | internal void CollectCandidates( |
| | | 106 | | FixedBoundVolume queryBounds, |
| | | 107 | | SwiftBucket<VoxelGrid> activeGrids, |
| | | 108 | | SwiftList<ushort> candidates) |
| | | 109 | | { |
| | 1882 | 110 | | candidates.Clear(); |
| | 1882 | 111 | | if (activeGrids.Count == 0) |
| | 1 | 112 | | return; |
| | | 113 | | |
| | 1881 | 114 | | if (ShouldScanActiveGrids(queryBounds, activeGrids.Count)) |
| | | 115 | | { |
| | 7668 | 116 | | foreach (VoxelGrid grid in activeGrids) |
| | | 117 | | { |
| | 2138 | 118 | | var gridBounds = new FixedBoundVolume(grid.BoundsMin, grid.BoundsMax); |
| | 2138 | 119 | | if (gridBounds.Intersects(queryBounds)) |
| | 1911 | 120 | | candidates.Add(grid.GridIndex); |
| | | 121 | | } |
| | | 122 | | } |
| | | 123 | | else |
| | | 124 | | { |
| | 185 | 125 | | if (_ordinaryGrids.Count > 0) |
| | 184 | 126 | | _ordinaryGrids.Query(queryBounds, candidates); |
| | | 127 | | |
| | 185 | 128 | | if (_oversizedGrids.Count > 0) |
| | 17 | 129 | | _oversizedGrids.Query(queryBounds, candidates); |
| | | 130 | | } |
| | | 131 | | |
| | 1881 | 132 | | if (candidates.Count > 1) |
| | 211 | 133 | | candidates.SortInPlace(); |
| | 1881 | 134 | | } |
| | | 135 | | |
| | | 136 | | internal void CollectContactCandidates( |
| | | 137 | | FixedBoundVolume queryBounds, |
| | | 138 | | SwiftList<ushort> candidates) |
| | | 139 | | { |
| | 907 | 140 | | candidates.Clear(); |
| | 907 | 141 | | if (_contactEnvelopes.Count > 0) |
| | 904 | 142 | | _contactEnvelopes.Query(queryBounds, candidates); |
| | | 143 | | |
| | 907 | 144 | | if (candidates.Count > 1) |
| | 184 | 145 | | candidates.SortInPlace(); |
| | 907 | 146 | | } |
| | | 147 | | |
| | | 148 | | internal void CollectPointCandidates( |
| | | 149 | | Vector3d point, |
| | | 150 | | SwiftList<ushort> candidates) |
| | | 151 | | { |
| | 154 | 152 | | candidates.Clear(); |
| | 154 | 153 | | if (_ordinaryGrids.Count > 0) |
| | 151 | 154 | | _ordinaryGrids.CollectPointCandidates(point, candidates); |
| | | 155 | | |
| | 154 | 156 | | if (_oversizedGrids.Count > 0) |
| | 4 | 157 | | _oversizedGrids.Query(new FixedBoundVolume(point, point), candidates); |
| | | 158 | | |
| | 154 | 159 | | if (candidates.Count > 1) |
| | 16 | 160 | | candidates.SortInPlace(); |
| | 154 | 161 | | } |
| | | 162 | | |
| | | 163 | | internal bool FitsHashCellBudget(FixedBoundVolume bounds) |
| | | 164 | | { |
| | 944 | 165 | | if (_cellBudget == 0UL) |
| | 1 | 166 | | return false; |
| | | 167 | | |
| | 943 | 168 | | GetCellRange(bounds, out SwiftSpatialHashCellIndex minCell, out SwiftSpatialHashCellIndex maxCell); |
| | 943 | 169 | | ulong xCount = GetCellCount(minCell.X, maxCell.X); |
| | 943 | 170 | | if (xCount > _cellBudget) |
| | 34 | 171 | | return false; |
| | | 172 | | |
| | 909 | 173 | | ulong yCount = GetCellCount(minCell.Y, maxCell.Y); |
| | 909 | 174 | | if (yCount > _cellBudget / xCount) |
| | 1 | 175 | | return false; |
| | | 176 | | |
| | 908 | 177 | | ulong xyCount = xCount * yCount; |
| | 908 | 178 | | ulong zCount = GetCellCount(minCell.Z, maxCell.Z); |
| | 908 | 179 | | return zCount <= _cellBudget / xyCount; |
| | | 180 | | } |
| | | 181 | | |
| | | 182 | | internal void GetCellRange( |
| | | 183 | | FixedBoundVolume bounds, |
| | | 184 | | out SwiftSpatialHashCellIndex minCell, |
| | | 185 | | out SwiftSpatialHashCellIndex maxCell) |
| | | 186 | | { |
| | 2836 | 187 | | minCell = _ordinaryGrids.GetCellIndex(bounds.Min); |
| | 2836 | 188 | | maxCell = _ordinaryGrids.GetCellIndex(bounds.Max); |
| | 2836 | 189 | | } |
| | | 190 | | |
| | | 191 | | internal bool ShouldScanActiveGrids( |
| | | 192 | | FixedBoundVolume queryBounds, |
| | | 193 | | int activeGridCount) |
| | | 194 | | { |
| | 1886 | 195 | | GetCellRange(queryBounds, out SwiftSpatialHashCellIndex minCell, out SwiftSpatialHashCellIndex maxCell); |
| | 1886 | 196 | | ulong count = (ulong)activeGridCount; |
| | 1886 | 197 | | ulong xCount = GetCellCount(minCell.X, maxCell.X); |
| | 1886 | 198 | | if (xCount > count) |
| | 1250 | 199 | | return true; |
| | | 200 | | |
| | 636 | 201 | | ulong yCount = GetCellCount(minCell.Y, maxCell.Y); |
| | 636 | 202 | | if (yCount > count / xCount) |
| | 335 | 203 | | return true; |
| | | 204 | | |
| | 301 | 205 | | ulong xyCount = xCount * yCount; |
| | 301 | 206 | | ulong zCount = GetCellCount(minCell.Z, maxCell.Z); |
| | 301 | 207 | | return zCount > count / xyCount; |
| | | 208 | | } |
| | | 209 | | |
| | | 210 | | private static ulong GetCellCount(int minimum, int maximum) => |
| | 5583 | 211 | | (ulong)((long)maximum - minimum + 1L); |
| | | 212 | | } |