| | | 1 | | using System; |
| | | 2 | | using System.Collections.Generic; |
| | | 3 | | using System.Runtime.CompilerServices; |
| | | 4 | | using System.Threading; |
| | | 5 | | |
| | | 6 | | namespace SwiftCollections.Query |
| | | 7 | | { |
| | | 8 | | /// <summary> |
| | | 9 | | /// Represents a Bounding Volume Hierarchy (BVH) optimized for spatial queries. |
| | | 10 | | /// </summary> |
| | | 11 | | public class SwiftBVH<T> |
| | | 12 | | { |
| | | 13 | | #region Static & Constants |
| | | 14 | | |
| | | 15 | | // Thread-local stack for queries |
| | 1 | 16 | | private static readonly ThreadLocal<SwiftIntStack> _threadLocalNodeStack = |
| | 3 | 17 | | new ThreadLocal<SwiftIntStack>(() => new SwiftIntStack(0)); |
| | | 18 | | |
| | | 19 | | private const int _childBalanceThreshold = 2; |
| | | 20 | | |
| | | 21 | | #endregion |
| | | 22 | | |
| | | 23 | | #region Fields |
| | | 24 | | |
| | | 25 | | private SwiftBVHNode<T>[] _nodePool; |
| | | 26 | | private int _peakIndex; |
| | | 27 | | private int _leafCount; |
| | | 28 | | |
| | | 29 | | private int[] _buckets; // Maps hash indices to node indices |
| | | 30 | | private int _bucketMask; // Always _nodePool.Length - 1 |
| | | 31 | | |
| | 61 | 32 | | private readonly SwiftIntStack _freeIndices = new SwiftIntStack(); |
| | | 33 | | |
| | | 34 | | private int _rootNodeIndex; |
| | | 35 | | |
| | 61 | 36 | | private ReaderWriterLockSlim _bvhLock = new ReaderWriterLockSlim(); |
| | | 37 | | |
| | | 38 | | #endregion |
| | | 39 | | |
| | | 40 | | #region Constructor |
| | | 41 | | |
| | | 42 | | /// <summary> |
| | | 43 | | /// Initializes a new instance of the <see cref="SwiftBVH{T}"/> class with the specified capacity. |
| | | 44 | | /// </summary> |
| | 61 | 45 | | public SwiftBVH(int capacity) |
| | 61 | 46 | | { |
| | 61 | 47 | | capacity = SwiftHashTools.NextPowerOfTwo(capacity); |
| | 61 | 48 | | _nodePool = new SwiftBVHNode<T>[capacity].Populate(() => |
| | 39569 | 49 | | new SwiftBVHNode<T>() { ParentIndex = -1, LeftChildIndex = -1, RightChildIndex = -1 }); |
| | 39569 | 50 | | _buckets = new int[capacity].Populate(() => -1); |
| | | 51 | | |
| | 61 | 52 | | _bucketMask = capacity - 1; |
| | 61 | 53 | | _rootNodeIndex = -1; |
| | | 54 | | |
| | 61 | 55 | | _freeIndices = new SwiftIntStack(SwiftIntStack.DefaultCapacity); |
| | 61 | 56 | | } |
| | | 57 | | |
| | | 58 | | #endregion |
| | | 59 | | |
| | | 60 | | #region Properties |
| | | 61 | | |
| | | 62 | | /// <summary> |
| | | 63 | | /// Gets the underlying pool of nodes used in the BVH. |
| | | 64 | | /// </summary> |
| | 16083 | 65 | | public SwiftBVHNode<T>[] NodePool => _nodePool; |
| | | 66 | | |
| | | 67 | | /// <summary> |
| | | 68 | | /// Gets the root node of the BVH. |
| | | 69 | | /// </summary> |
| | 6 | 70 | | public SwiftBVHNode<T> RootNode => _rootNodeIndex >= 0 && _nodePool[_rootNodeIndex].IsAllocated |
| | 6 | 71 | | ? _nodePool[_rootNodeIndex] |
| | 6 | 72 | | : SwiftBVHNode<T>.Default; |
| | | 73 | | |
| | | 74 | | /// <summary> |
| | | 75 | | /// Gets the index of the root node in the BVH. |
| | | 76 | | /// </summary> |
| | 1958 | 77 | | public int RootNodeIndex => _rootNodeIndex; |
| | | 78 | | |
| | | 79 | | /// <summary> |
| | | 80 | | /// Gets the total number of leaf nodes in the BVH. |
| | | 81 | | /// </summary> |
| | 5 | 82 | | public int Count => _leafCount; |
| | | 83 | | |
| | | 84 | | #endregion |
| | | 85 | | |
| | | 86 | | #region Collection Manipulation |
| | | 87 | | |
| | | 88 | | /// <summary> |
| | | 89 | | /// Allocates a new node with the specified value, bounds, and leaf status. |
| | | 90 | | /// Reuses indices from the freelist when available. |
| | | 91 | | /// </summary> |
| | | 92 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 93 | | private int AllocateNode(T value, IBoundVolume bounds, bool isLeaf) |
| | 57504 | 94 | | { |
| | | 95 | | int index; |
| | | 96 | | |
| | | 97 | | // Check if there are any reusable indices in the freelist |
| | 57504 | 98 | | if (_freeIndices.Count > 0) |
| | 2 | 99 | | index = _freeIndices.Pop(); // Reuse an available index |
| | | 100 | | else |
| | 57502 | 101 | | { |
| | 57502 | 102 | | if (_peakIndex + 1 >= _nodePool.Length) |
| | 57 | 103 | | Resize(_nodePool.Length * 2); |
| | | 104 | | |
| | | 105 | | // Allocate a new index if freelist is empty |
| | 57502 | 106 | | index = _peakIndex++; |
| | 57502 | 107 | | } |
| | | 108 | | |
| | 57504 | 109 | | ref SwiftBVHNode<T> node = ref _nodePool[index]; |
| | 57504 | 110 | | node.Reset(); // Explicit reset |
| | 57504 | 111 | | node.MyIndex = index; |
| | 57504 | 112 | | node.Value = value; |
| | 57504 | 113 | | node.Bounds = bounds; |
| | | 114 | | |
| | 57504 | 115 | | if (isLeaf) |
| | 28782 | 116 | | { |
| | 28782 | 117 | | node.IsLeaf = isLeaf; |
| | 28782 | 118 | | _leafCount++; |
| | 28782 | 119 | | } |
| | | 120 | | |
| | 57504 | 121 | | node.IsAllocated = true; |
| | | 122 | | |
| | 57504 | 123 | | return index; |
| | 57504 | 124 | | } |
| | | 125 | | |
| | | 126 | | /// <summary> |
| | | 127 | | /// Inserts a bounding volume with an associated value into the BVH. |
| | | 128 | | /// Ensures tree balance and updates hash buckets. |
| | | 129 | | /// </summary> |
| | | 130 | | public bool Insert(T value, IBoundVolume bounds) |
| | 28782 | 131 | | { |
| | 28782 | 132 | | SwiftThrowHelper.ThrowIfNull(bounds, nameof(bounds)); |
| | | 133 | | |
| | 28782 | 134 | | _bvhLock.EnterWriteLock(); |
| | | 135 | | try |
| | 28782 | 136 | | { |
| | 28782 | 137 | | int newNodeIndex = AllocateNode(value, bounds, true); // Allocate new node as a leaf |
| | 28782 | 138 | | _rootNodeIndex = InsertIntoTree(_rootNodeIndex, newNodeIndex); |
| | 28782 | 139 | | InsertIntoBuckets(value, newNodeIndex); |
| | 28782 | 140 | | return true; |
| | | 141 | | } |
| | | 142 | | finally |
| | 28782 | 143 | | { |
| | 28782 | 144 | | _bvhLock.ExitWriteLock(); |
| | 28782 | 145 | | } |
| | 28782 | 146 | | } |
| | | 147 | | |
| | | 148 | | /// <summary> |
| | | 149 | | /// Inserts a node into the tree while maintaining tree balance. |
| | | 150 | | /// Adjusts parent-child relationships as necessary. |
| | | 151 | | /// </summary> |
| | | 152 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 153 | | private int InsertIntoTree(int parentNodeIndex, int newNodeIndex) |
| | 328562 | 154 | | { |
| | 328562 | 155 | | if (parentNodeIndex < 0 || !_nodePool[parentNodeIndex].IsAllocated) |
| | 60 | 156 | | return newNodeIndex; |
| | | 157 | | |
| | 328502 | 158 | | ref SwiftBVHNode<T> parentNode = ref _nodePool[parentNodeIndex]; |
| | 328502 | 159 | | ref SwiftBVHNode<T> newNode = ref _nodePool[newNodeIndex]; |
| | 328502 | 160 | | if (parentNode.IsLeaf) |
| | 28722 | 161 | | { |
| | | 162 | | // Create a new parent node |
| | 28722 | 163 | | int newParentIndex = AllocateNode(default, parentNode.Bounds.Union(newNode.Bounds), false); |
| | 28722 | 164 | | ref SwiftBVHNode<T> newParentNode = ref _nodePool[newParentIndex]; |
| | 28722 | 165 | | newParentNode.ParentIndex = parentNode.ParentIndex; |
| | | 166 | | |
| | 28722 | 167 | | newParentNode.LeftChildIndex = parentNodeIndex; |
| | 28722 | 168 | | newParentNode.RightChildIndex = newNodeIndex; |
| | | 169 | | |
| | 28722 | 170 | | parentNode.ParentIndex = newParentIndex; |
| | 28722 | 171 | | newNode.ParentIndex = newParentIndex; |
| | | 172 | | |
| | 28722 | 173 | | newParentNode.SubtreeSize = 1 + parentNode.SubtreeSize + newNode.SubtreeSize; |
| | 28722 | 174 | | return newParentIndex; |
| | | 175 | | } |
| | | 176 | | |
| | | 177 | | // Determines the optimal child for insertion based on balance and cost metrics. |
| | 299780 | 178 | | SwiftBVHNode<T> leftChild = parentNode.HasLeftChild |
| | 299780 | 179 | | ? _nodePool[parentNode.LeftChildIndex] |
| | 299780 | 180 | | : SwiftBVHNode<T>.Default; |
| | 299780 | 181 | | SwiftBVHNode<T> rightChild = parentNode.HasRightChild |
| | 299780 | 182 | | ? _nodePool[parentNode.RightChildIndex] |
| | 299780 | 183 | | : SwiftBVHNode<T>.Default; |
| | | 184 | | |
| | | 185 | | // Compute balance metrics |
| | 299780 | 186 | | int leftSize = leftChild.IsAllocated ? leftChild.SubtreeSize : 0; |
| | 299780 | 187 | | int rightSize = rightChild.IsAllocated ? rightChild.SubtreeSize : 0; |
| | | 188 | | |
| | | 189 | | bool isInsertingLeft; |
| | 299780 | 190 | | if (Math.Abs(leftSize - rightSize) > _childBalanceThreshold) |
| | 122980 | 191 | | { |
| | | 192 | | // Insert into the smaller subtree to maintain balance |
| | 122980 | 193 | | if (leftSize < rightSize) |
| | 115775 | 194 | | isInsertingLeft = true; |
| | | 195 | | else |
| | 7205 | 196 | | isInsertingLeft = false; |
| | 122980 | 197 | | } |
| | | 198 | | else |
| | 176800 | 199 | | { |
| | | 200 | | // Compute cost metrics |
| | 176800 | 201 | | int leftCost = parentNode.HasLeftChild |
| | 176800 | 202 | | ? leftChild.Bounds.GetCost(newNode.Bounds) |
| | 176800 | 203 | | : 0; |
| | | 204 | | |
| | 176800 | 205 | | int rightCost = parentNode.HasRightChild |
| | 176800 | 206 | | ? rightChild.Bounds.GetCost(newNode.Bounds) |
| | 176800 | 207 | | : 0; |
| | | 208 | | |
| | | 209 | | // Insert into the child with the least volume increase |
| | 176800 | 210 | | if (leftCost < rightCost) |
| | 19859 | 211 | | isInsertingLeft = true; |
| | | 212 | | else |
| | 156941 | 213 | | isInsertingLeft = false; |
| | 176800 | 214 | | } |
| | | 215 | | |
| | 299780 | 216 | | if (isInsertingLeft) |
| | 135634 | 217 | | { |
| | 135634 | 218 | | parentNode.LeftChildIndex = InsertIntoTree(parentNode.LeftChildIndex, newNodeIndex); |
| | 135634 | 219 | | leftChild = parentNode.HasLeftChild |
| | 135634 | 220 | | ? _nodePool[parentNode.LeftChildIndex] |
| | 135634 | 221 | | : SwiftBVHNode<T>.Default; |
| | 135634 | 222 | | } |
| | | 223 | | else |
| | 164146 | 224 | | { |
| | 164146 | 225 | | parentNode.RightChildIndex = InsertIntoTree(parentNode.RightChildIndex, newNodeIndex); |
| | 164146 | 226 | | rightChild = parentNode.HasRightChild |
| | 164146 | 227 | | ? _nodePool[parentNode.RightChildIndex] |
| | 164146 | 228 | | : SwiftBVHNode<T>.Default; |
| | 164146 | 229 | | } |
| | | 230 | | |
| | 299780 | 231 | | parentNode.Bounds = GetCombinedBounds(leftChild, rightChild); |
| | 299780 | 232 | | parentNode.SubtreeSize = 1 |
| | 299780 | 233 | | + (leftChild.IsAllocated ? leftChild.SubtreeSize : 0) |
| | 299780 | 234 | | + (rightChild.IsAllocated ? rightChild.SubtreeSize : 0); |
| | | 235 | | |
| | 299780 | 236 | | return parentNodeIndex; |
| | 328562 | 237 | | } |
| | | 238 | | |
| | | 239 | | /// <summary> |
| | | 240 | | /// Inserts a value into the hash bucket for fast lookup. |
| | | 241 | | /// Handles collisions with linear probing. |
| | | 242 | | /// </summary> |
| | | 243 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 244 | | private void InsertIntoBuckets(T value, int nodeIndex) |
| | 142436 | 245 | | { |
| | 142436 | 246 | | int hash = value.GetHashCode() & 0x7FFFFFFF; |
| | 142436 | 247 | | int bucketIndex = hash & _bucketMask; |
| | | 248 | | |
| | | 249 | | // Linear probe to find an empty slot |
| | 151145 | 250 | | while (_buckets[bucketIndex] != -1) |
| | 8709 | 251 | | bucketIndex = (bucketIndex + 1) & _bucketMask; |
| | | 252 | | |
| | 142436 | 253 | | _buckets[bucketIndex] = nodeIndex; |
| | 142436 | 254 | | } |
| | | 255 | | |
| | | 256 | | /// <summary> |
| | | 257 | | /// Updates the bounding volume of a node and propagates changes up the tree. |
| | | 258 | | /// Ensures consistency in parent bounds and subtree sizes. |
| | | 259 | | /// </summary> |
| | | 260 | | public void UpdateEntryBounds(T value, IBoundVolume newBounds) |
| | 2005 | 261 | | { |
| | 2005 | 262 | | SwiftThrowHelper.ThrowIfNull(value, nameof(value)); |
| | 2005 | 263 | | SwiftThrowHelper.ThrowIfNull(newBounds, nameof(newBounds)); |
| | | 264 | | |
| | 2005 | 265 | | int index = FindEntry(value); |
| | 2005 | 266 | | if (index == -1) return; |
| | | 267 | | |
| | 2005 | 268 | | _bvhLock.EnterWriteLock(); |
| | | 269 | | try |
| | 2005 | 270 | | { |
| | 2005 | 271 | | ref SwiftBVHNode<T> node = ref _nodePool[index]; |
| | 2006 | 272 | | if (!node.IsAllocated) return; // Skip update if node has been removed |
| | | 273 | | |
| | 2004 | 274 | | IBoundVolume oldBounds = node.Bounds; |
| | 2004 | 275 | | if (oldBounds != null && oldBounds.Equals(newBounds)) |
| | 0 | 276 | | return; // Skip unnecessary updates |
| | | 277 | | |
| | 2004 | 278 | | node.Bounds = newBounds; |
| | | 279 | | |
| | | 280 | | // Propagate changes up the tree |
| | 2004 | 281 | | int parentIndex = node.ParentIndex; |
| | 7545 | 282 | | while (parentIndex != -1) |
| | 7249 | 283 | | { |
| | 7249 | 284 | | ref SwiftBVHNode<T> parent = ref _nodePool[parentIndex]; |
| | 7249 | 285 | | SwiftBVHNode<T> leftChild = parent.HasLeftChild |
| | 7249 | 286 | | ? _nodePool[parent.LeftChildIndex] |
| | 7249 | 287 | | : SwiftBVHNode<T>.Default; |
| | 7249 | 288 | | SwiftBVHNode<T> rightChild = parent.HasRightChild |
| | 7249 | 289 | | ? _nodePool[parent.RightChildIndex] |
| | 7249 | 290 | | : SwiftBVHNode<T>.Default; |
| | | 291 | | |
| | 7249 | 292 | | IBoundVolume newParentBounds = GetCombinedBounds(leftChild, rightChild); |
| | 7249 | 293 | | if (parent.Bounds.Equals(newParentBounds)) |
| | 1708 | 294 | | break; // No further updates needed |
| | | 295 | | |
| | 5541 | 296 | | parent.Bounds = newParentBounds; |
| | 5541 | 297 | | parentIndex = parent.ParentIndex; |
| | 5541 | 298 | | } |
| | 2004 | 299 | | } |
| | | 300 | | finally |
| | 2005 | 301 | | { |
| | 2005 | 302 | | _bvhLock.ExitWriteLock(); |
| | 2005 | 303 | | } |
| | 2005 | 304 | | } |
| | | 305 | | |
| | | 306 | | /// <summary> |
| | | 307 | | /// Removes a value and its associated bounding volume from the BVH. |
| | | 308 | | /// Updates tree structure and clears hash bucket entries. |
| | | 309 | | /// </summary> |
| | | 310 | | public bool Remove(T value) |
| | 1859 | 311 | | { |
| | 1859 | 312 | | SwiftThrowHelper.ThrowIfNull(value, nameof(value)); |
| | | 313 | | |
| | 1859 | 314 | | int nodeIndex = FindEntry(value); |
| | 1863 | 315 | | if (nodeIndex == -1) return false; |
| | | 316 | | |
| | 1855 | 317 | | _bvhLock.EnterWriteLock(); |
| | | 318 | | try |
| | 1855 | 319 | | { |
| | | 320 | | // If the node is the root and the only node, reset the BVH |
| | 1855 | 321 | | if (nodeIndex == RootNodeIndex && _leafCount == 1) |
| | 2 | 322 | | { |
| | 2 | 323 | | Clear(); |
| | 2 | 324 | | return true; |
| | | 325 | | } |
| | | 326 | | |
| | 1853 | 327 | | RemoveFromBuckets(value); // Ensure the bucket is cleared before further operations |
| | | 328 | | |
| | | 329 | | // Remove node and update tree structure |
| | 1853 | 330 | | RemoveFromTree(nodeIndex); |
| | | 331 | | |
| | 1853 | 332 | | return true; |
| | | 333 | | } |
| | | 334 | | finally |
| | 1855 | 335 | | { |
| | 1855 | 336 | | _bvhLock.ExitWriteLock(); |
| | 1855 | 337 | | } |
| | 1859 | 338 | | } |
| | | 339 | | |
| | | 340 | | /// <summary> |
| | | 341 | | /// Removes an entry from the hash buckets, resolving collisions as necessary. |
| | | 342 | | /// Throws an exception if the value is not found. |
| | | 343 | | /// </summary> |
| | | 344 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 345 | | private void RemoveFromBuckets(T value) |
| | 1853 | 346 | | { |
| | 1853 | 347 | | int hash = value.GetHashCode() & 0x7FFFFFFF; |
| | 1853 | 348 | | int bucketIndex = hash & _bucketMask; |
| | | 349 | | |
| | | 350 | | // Linear probing to resolve collisions |
| | 1854 | 351 | | while (_buckets[bucketIndex] != -1) |
| | 1854 | 352 | | { |
| | 1854 | 353 | | int nodeIndex = _buckets[bucketIndex]; |
| | 1854 | 354 | | if (_nodePool[nodeIndex].IsLeaf && EqualityComparer<T>.Default.Equals(_nodePool[nodeIndex].Value, value) |
| | 1853 | 355 | | { |
| | 1853 | 356 | | _buckets[bucketIndex] = -1; |
| | 1853 | 357 | | RehashBucketCluster((bucketIndex + 1) & _bucketMask); |
| | 1853 | 358 | | return; |
| | | 359 | | } |
| | | 360 | | |
| | 1 | 361 | | bucketIndex = (bucketIndex + 1) & _bucketMask; |
| | 1 | 362 | | } |
| | 1853 | 363 | | } |
| | | 364 | | |
| | | 365 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 366 | | private void RehashBucketCluster(int startIndex) |
| | 1853 | 367 | | { |
| | 1853 | 368 | | int bucketIndex = startIndex; |
| | | 369 | | |
| | 115507 | 370 | | while (_buckets[bucketIndex] != -1) |
| | 113654 | 371 | | { |
| | 113654 | 372 | | int nodeIndex = _buckets[bucketIndex]; |
| | 113654 | 373 | | _buckets[bucketIndex] = -1; |
| | | 374 | | |
| | 113654 | 375 | | if (_nodePool[nodeIndex].IsLeaf && _nodePool[nodeIndex].IsAllocated) |
| | 113654 | 376 | | InsertIntoBuckets(_nodePool[nodeIndex].Value, nodeIndex); |
| | | 377 | | |
| | 113654 | 378 | | bucketIndex = (bucketIndex + 1) & _bucketMask; |
| | 113654 | 379 | | } |
| | 1853 | 380 | | } |
| | | 381 | | |
| | | 382 | | /// <summary> |
| | | 383 | | /// Recursively removes a node and updates the bounds and subtree sizes of parents. |
| | | 384 | | /// Ensures integrity of the BVH structure. |
| | | 385 | | /// </summary> |
| | | 386 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 387 | | private void RemoveFromTree(int nodeIndex) |
| | 1853 | 388 | | { |
| | 21035 | 389 | | while (nodeIndex != -1) |
| | 19186 | 390 | | { |
| | 19186 | 391 | | ref SwiftBVHNode<T> currentNode = ref _nodePool[nodeIndex]; |
| | | 392 | | |
| | 19186 | 393 | | int parentIndex = currentNode.ParentIndex; |
| | 19186 | 394 | | if (!currentNode.HasChildren) |
| | 3639 | 395 | | { |
| | | 396 | | // Clean up leaf or empty parent |
| | 3639 | 397 | | if (currentNode.HasParent) |
| | 3635 | 398 | | { |
| | | 399 | | // Remove the child reference from the parent root |
| | 3635 | 400 | | ref SwiftBVHNode<T> parentNode = ref _nodePool[parentIndex]; |
| | 3635 | 401 | | if (parentNode.LeftChildIndex == nodeIndex) |
| | 1839 | 402 | | parentNode.LeftChildIndex = -1; |
| | 1796 | 403 | | else if (parentNode.RightChildIndex == nodeIndex) |
| | 1796 | 404 | | parentNode.RightChildIndex = -1; |
| | 3635 | 405 | | } |
| | 4 | 406 | | else if (nodeIndex == RootNodeIndex) |
| | 4 | 407 | | { |
| | 4 | 408 | | Clear(); |
| | 4 | 409 | | return; |
| | | 410 | | } |
| | | 411 | | |
| | 3635 | 412 | | if (currentNode.IsLeaf) |
| | 1853 | 413 | | _leafCount--; |
| | | 414 | | |
| | 3635 | 415 | | currentNode.Reset(); |
| | 3635 | 416 | | _freeIndices.Push(nodeIndex); |
| | 3635 | 417 | | nodeIndex = parentIndex; |
| | 3635 | 418 | | continue; |
| | | 419 | | } |
| | | 420 | | |
| | | 421 | | // Resize parent that still has children... |
| | 15547 | 422 | | SwiftBVHNode<T> leftChild = currentNode.HasLeftChild |
| | 15547 | 423 | | ? _nodePool[currentNode.LeftChildIndex] |
| | 15547 | 424 | | : SwiftBVHNode<T>.Default; |
| | 15547 | 425 | | SwiftBVHNode<T> rightChild = currentNode.HasRightChild |
| | 15547 | 426 | | ? _nodePool[currentNode.RightChildIndex] |
| | 15547 | 427 | | : SwiftBVHNode<T>.Default; |
| | | 428 | | |
| | 15547 | 429 | | currentNode.Bounds = GetCombinedBounds(leftChild, rightChild); |
| | 15547 | 430 | | currentNode.SubtreeSize = 1 |
| | 15547 | 431 | | + (leftChild.IsAllocated ? leftChild.SubtreeSize : 0) |
| | 15547 | 432 | | + (rightChild.IsAllocated ? rightChild.SubtreeSize : 0); |
| | | 433 | | |
| | 15547 | 434 | | nodeIndex = parentIndex; |
| | 15547 | 435 | | } |
| | 1853 | 436 | | } |
| | | 437 | | |
| | | 438 | | #endregion |
| | | 439 | | |
| | | 440 | | #region Capacity Management |
| | | 441 | | |
| | | 442 | | /// <summary> |
| | | 443 | | /// Ensures the BVH has sufficient capacity, resizing the node pool and buckets if needed. |
| | | 444 | | /// </summary> |
| | | 445 | | public void EnsureCapacity(int capacity) |
| | 2 | 446 | | { |
| | 2 | 447 | | capacity = SwiftHashTools.NextPowerOfTwo(capacity); |
| | 2 | 448 | | if (capacity > _nodePool.Length) |
| | 1 | 449 | | Resize(capacity); |
| | 2 | 450 | | } |
| | | 451 | | |
| | | 452 | | /// <summary> |
| | | 453 | | /// Resizes the internal node pool to accommodate additional nodes. |
| | | 454 | | /// Preserves existing nodes and reinitializes the expanded capacity. |
| | | 455 | | /// </summary> |
| | | 456 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 457 | | private void Resize(int newSize) |
| | 58 | 458 | | { |
| | 58 | 459 | | SwiftBVHNode<T>[] newArray = new SwiftBVHNode<T>[newSize]; |
| | 58 | 460 | | Array.Copy(_nodePool, 0, newArray, 0, _peakIndex); |
| | | 461 | | |
| | 149058 | 462 | | for (int i = _peakIndex; i < newSize; i++) |
| | 74471 | 463 | | newArray[i].Reset(); // set default index lookup values |
| | | 464 | | |
| | 58 | 465 | | _nodePool = newArray; |
| | | 466 | | |
| | 58 | 467 | | ResizeBuckets(newSize); |
| | 58 | 468 | | } |
| | | 469 | | |
| | | 470 | | /// <summary> |
| | | 471 | | /// Resizes and rehashes the hash buckets to maintain lookup efficiency. |
| | | 472 | | /// Rehashes existing nodes after resizing. |
| | | 473 | | /// </summary> |
| | | 474 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 475 | | private void ResizeBuckets(int newSize) |
| | 58 | 476 | | { |
| | 148878 | 477 | | _buckets = new int[newSize].Populate(() => -1); |
| | 58 | 478 | | _bucketMask = newSize - 1; |
| | | 479 | | |
| | | 480 | | // Rehash existing leaf nodes |
| | 148814 | 481 | | for (int i = 0; i < _peakIndex; i++) |
| | 74349 | 482 | | { |
| | 74349 | 483 | | if (!_nodePool[i].IsLeaf) |
| | 37147 | 484 | | continue; |
| | | 485 | | |
| | 37202 | 486 | | int hash = _nodePool[i].Value.GetHashCode() & 0x7FFFFFFF; |
| | 37202 | 487 | | int bucketIndex = hash & _bucketMask; |
| | | 488 | | |
| | | 489 | | // Linear probe to find an empty slot |
| | 37533 | 490 | | while (_buckets[bucketIndex] != -1) |
| | 331 | 491 | | bucketIndex = (bucketIndex + 1) & _bucketMask; |
| | | 492 | | |
| | 37202 | 493 | | _buckets[bucketIndex] = i; |
| | 37202 | 494 | | } |
| | 58 | 495 | | } |
| | | 496 | | |
| | | 497 | | #endregion |
| | | 498 | | |
| | | 499 | | #region Utility Methods |
| | | 500 | | |
| | | 501 | | /// <summary> |
| | | 502 | | /// Gets the combined bounding volume of two child nodes. |
| | | 503 | | /// Handles cases where one or both children are missing. |
| | | 504 | | /// </summary> |
| | | 505 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 506 | | private IBoundVolume GetCombinedBounds(SwiftBVHNode<T> leftChild, SwiftBVHNode<T> rightChild) |
| | 322576 | 507 | | { |
| | 322576 | 508 | | if (leftChild.IsAllocated && rightChild.IsAllocated) |
| | 319753 | 509 | | return leftChild.Bounds.Union(rightChild.Bounds); |
| | 2823 | 510 | | if (leftChild.IsAllocated) |
| | 939 | 511 | | return leftChild.Bounds; |
| | 1884 | 512 | | return rightChild.Bounds; |
| | 322576 | 513 | | } |
| | | 514 | | |
| | | 515 | | /// <summary> |
| | | 516 | | /// Queries the BVH for values whose bounding volumes intersect with the specified volume. |
| | | 517 | | /// Uses a stack-based approach for efficient traversal. |
| | | 518 | | /// </summary> |
| | | 519 | | public void Query(IBoundVolume queryBounds, ICollection<T> results) |
| | 44 | 520 | | { |
| | 44 | 521 | | SwiftThrowHelper.ThrowIfNull(queryBounds, nameof(queryBounds)); |
| | | 522 | | |
| | 52 | 523 | | if (RootNodeIndex == -1) return; |
| | | 524 | | |
| | 36 | 525 | | _bvhLock.EnterReadLock(); |
| | | 526 | | try |
| | 36 | 527 | | { |
| | 36 | 528 | | SwiftIntStack nodeStack = _threadLocalNodeStack.Value; |
| | 36 | 529 | | nodeStack.EnsureCapacity(_peakIndex + 1); |
| | 36 | 530 | | nodeStack.Clear(); |
| | | 531 | | |
| | 36 | 532 | | nodeStack.Push(RootNodeIndex); |
| | | 533 | | |
| | 44597 | 534 | | while (nodeStack.Count > 0) |
| | 44562 | 535 | | { |
| | 44562 | 536 | | int index = nodeStack.Pop(); |
| | 44562 | 537 | | SwiftBVHNode<T> node = _nodePool[index]; |
| | | 538 | | |
| | 44562 | 539 | | if (!node.IsAllocated) |
| | 1 | 540 | | throw new InvalidOperationException($"Encountered an unallocated node at index {index} during qu |
| | | 541 | | |
| | 44561 | 542 | | if (!queryBounds.Intersects(node.Bounds)) |
| | 1058 | 543 | | continue; |
| | | 544 | | |
| | 43503 | 545 | | if (node.IsLeaf) |
| | 21211 | 546 | | { |
| | 21211 | 547 | | results.Add(node.Value); |
| | 21211 | 548 | | continue; |
| | | 549 | | } |
| | | 550 | | |
| | 22292 | 551 | | if (node.HasLeftChild) |
| | 22242 | 552 | | nodeStack.Push(node.LeftChildIndex); |
| | 22292 | 553 | | if (node.HasRightChild) |
| | 22284 | 554 | | nodeStack.Push(node.RightChildIndex); |
| | 22292 | 555 | | } |
| | 35 | 556 | | } |
| | | 557 | | finally |
| | 36 | 558 | | { |
| | 36 | 559 | | _bvhLock.ExitReadLock(); |
| | 36 | 560 | | } |
| | 43 | 561 | | } |
| | | 562 | | |
| | | 563 | | /// <summary> |
| | | 564 | | /// Finds the index of a node by its value in the BVH using hash buckets. |
| | | 565 | | /// Returns -1 if the value is not found. |
| | | 566 | | /// </summary> |
| | | 567 | | public int FindEntry(T value) |
| | 3877 | 568 | | { |
| | 3877 | 569 | | SwiftThrowHelper.ThrowIfNull(value, nameof(value)); |
| | | 570 | | |
| | 3877 | 571 | | _bvhLock.EnterReadLock(); |
| | | 572 | | try |
| | 3877 | 573 | | { |
| | 3877 | 574 | | int hash = value.GetHashCode() & 0x7FFFFFFF; |
| | 3877 | 575 | | int bucketIndex = hash & _bucketMask; |
| | | 576 | | |
| | | 577 | | // Linear probing to resolve collisions |
| | 3884 | 578 | | while (_buckets[bucketIndex] != -1) |
| | 3876 | 579 | | { |
| | 3876 | 580 | | int nodeIndex = _buckets[bucketIndex]; |
| | 3876 | 581 | | if (_nodePool[nodeIndex].IsLeaf && EqualityComparer<T>.Default.Equals(_nodePool[nodeIndex].Value, va |
| | 3869 | 582 | | return nodeIndex; |
| | | 583 | | |
| | 7 | 584 | | bucketIndex = (bucketIndex + 1) & _bucketMask; |
| | 7 | 585 | | } |
| | | 586 | | |
| | 8 | 587 | | return -1; // Not found |
| | | 588 | | } |
| | | 589 | | finally |
| | 3877 | 590 | | { |
| | 3877 | 591 | | _bvhLock.ExitReadLock(); |
| | 3877 | 592 | | } |
| | 3877 | 593 | | } |
| | | 594 | | |
| | | 595 | | /// <summary> |
| | | 596 | | /// Clears the BVH, resetting all nodes, buckets, and metadata. |
| | | 597 | | /// </summary> |
| | | 598 | | public void Clear() |
| | 8 | 599 | | { |
| | 8 | 600 | | if (RootNodeIndex == -1) return; |
| | | 601 | | |
| | 6816 | 602 | | for (int i = 0; i < _peakIndex; i++) |
| | 3400 | 603 | | _nodePool[i].Reset(); |
| | | 604 | | |
| | 8848 | 605 | | for (int i = 0; i < _buckets.Length; i++) |
| | 4416 | 606 | | _buckets[i] = -1; |
| | | 607 | | |
| | 8 | 608 | | _freeIndices.Reset(); |
| | | 609 | | |
| | 8 | 610 | | _leafCount = 0; |
| | 8 | 611 | | _peakIndex = 0; |
| | 8 | 612 | | _rootNodeIndex = -1; |
| | 8 | 613 | | } |
| | | 614 | | |
| | | 615 | | #endregion |
| | | 616 | | } |
| | | 617 | | } |