< Summary

Information
Class: SwiftCollections.Query.SwiftBVH<T>
Assembly: SwiftCollections
File(s): /home/runner/work/SwiftCollections/SwiftCollections/src/SwiftCollections/Query/BoundingVolume/SwiftBVH.cs
Line coverage
99%
Covered lines: 327
Uncovered lines: 1
Coverable lines: 328
Total lines: 617
Line coverage: 99.6%
Branch coverage
93%
Covered branches: 140
Total branches: 150
Branch coverage: 93.3%
Method coverage

Feature is only available for sponsors

Upgrade to PRO version

Metrics

MethodBranch coverage Crap Score Cyclomatic complexity Line coverage
.cctor()100%11100%
.ctor(...)100%44100%
get_NodePool()100%11100%
get_RootNode()100%44100%
get_RootNodeIndex()100%11100%
get_Count()100%11100%
AllocateNode(...)100%66100%
Insert(...)100%11100%
InsertIntoTree(...)88.23%3434100%
InsertIntoBuckets(...)100%22100%
UpdateEntryBounds(...)75%161697.05%
Remove(...)100%66100%
RemoveFromBuckets(...)83.33%66100%
RehashBucketCluster(...)83.33%66100%
RemoveFromTree(...)100%2222100%
EnsureCapacity(...)100%22100%
Resize(...)100%22100%
ResizeBuckets(...)100%88100%
GetCombinedBounds(...)100%66100%
Query(...)100%1414100%
FindEntry(...)100%66100%
Clear()100%66100%

File(s)

/home/runner/work/SwiftCollections/SwiftCollections/src/SwiftCollections/Query/BoundingVolume/SwiftBVH.cs

#LineLine coverage
 1using System;
 2using System.Collections.Generic;
 3using System.Runtime.CompilerServices;
 4using System.Threading;
 5
 6namespace 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
 116        private static readonly ThreadLocal<SwiftIntStack> _threadLocalNodeStack =
 317            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
 6132        private readonly SwiftIntStack _freeIndices = new SwiftIntStack();
 33
 34        private int _rootNodeIndex;
 35
 6136        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>
 6145        public SwiftBVH(int capacity)
 6146        {
 6147            capacity = SwiftHashTools.NextPowerOfTwo(capacity);
 6148            _nodePool = new SwiftBVHNode<T>[capacity].Populate(() =>
 3956949                new SwiftBVHNode<T>() { ParentIndex = -1, LeftChildIndex = -1, RightChildIndex = -1 });
 3956950            _buckets = new int[capacity].Populate(() => -1);
 51
 6152            _bucketMask = capacity - 1;
 6153            _rootNodeIndex = -1;
 54
 6155            _freeIndices = new SwiftIntStack(SwiftIntStack.DefaultCapacity);
 6156        }
 57
 58        #endregion
 59
 60        #region Properties
 61
 62        /// <summary>
 63        /// Gets the underlying pool of nodes used in the BVH.
 64        /// </summary>
 1608365        public SwiftBVHNode<T>[] NodePool => _nodePool;
 66
 67        /// <summary>
 68        /// Gets the root node of the BVH.
 69        /// </summary>
 670        public SwiftBVHNode<T> RootNode => _rootNodeIndex >= 0 && _nodePool[_rootNodeIndex].IsAllocated
 671            ? _nodePool[_rootNodeIndex]
 672            : SwiftBVHNode<T>.Default;
 73
 74        /// <summary>
 75        /// Gets the index of the root node in the BVH.
 76        /// </summary>
 195877        public int RootNodeIndex => _rootNodeIndex;
 78
 79        /// <summary>
 80        /// Gets the total number of leaf nodes in the BVH.
 81        /// </summary>
 582        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)
 5750494        {
 95            int index;
 96
 97            // Check if there are any reusable indices in the freelist
 5750498            if (_freeIndices.Count > 0)
 299                index = _freeIndices.Pop(); // Reuse an available index
 100            else
 57502101            {
 57502102                if (_peakIndex + 1 >= _nodePool.Length)
 57103                    Resize(_nodePool.Length * 2);
 104
 105                // Allocate a new index if freelist is empty
 57502106                index = _peakIndex++;
 57502107            }
 108
 57504109            ref SwiftBVHNode<T> node = ref _nodePool[index];
 57504110            node.Reset(); // Explicit reset
 57504111            node.MyIndex = index;
 57504112            node.Value = value;
 57504113            node.Bounds = bounds;
 114
 57504115            if (isLeaf)
 28782116            {
 28782117                node.IsLeaf = isLeaf;
 28782118                _leafCount++;
 28782119            }
 120
 57504121            node.IsAllocated = true;
 122
 57504123            return index;
 57504124        }
 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)
 28782131        {
 28782132            SwiftThrowHelper.ThrowIfNull(bounds, nameof(bounds));
 133
 28782134            _bvhLock.EnterWriteLock();
 135            try
 28782136            {
 28782137                int newNodeIndex = AllocateNode(value, bounds, true); // Allocate new node as a leaf
 28782138                _rootNodeIndex = InsertIntoTree(_rootNodeIndex, newNodeIndex);
 28782139                InsertIntoBuckets(value, newNodeIndex);
 28782140                return true;
 141            }
 142            finally
 28782143            {
 28782144                _bvhLock.ExitWriteLock();
 28782145            }
 28782146        }
 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)
 328562154        {
 328562155            if (parentNodeIndex < 0 || !_nodePool[parentNodeIndex].IsAllocated)
 60156                return newNodeIndex;
 157
 328502158            ref SwiftBVHNode<T> parentNode = ref _nodePool[parentNodeIndex];
 328502159            ref SwiftBVHNode<T> newNode = ref _nodePool[newNodeIndex];
 328502160            if (parentNode.IsLeaf)
 28722161            {
 162                // Create a new parent node
 28722163                int newParentIndex = AllocateNode(default, parentNode.Bounds.Union(newNode.Bounds), false);
 28722164                ref SwiftBVHNode<T> newParentNode = ref _nodePool[newParentIndex];
 28722165                newParentNode.ParentIndex = parentNode.ParentIndex;
 166
 28722167                newParentNode.LeftChildIndex = parentNodeIndex;
 28722168                newParentNode.RightChildIndex = newNodeIndex;
 169
 28722170                parentNode.ParentIndex = newParentIndex;
 28722171                newNode.ParentIndex = newParentIndex;
 172
 28722173                newParentNode.SubtreeSize = 1 + parentNode.SubtreeSize + newNode.SubtreeSize;
 28722174                return newParentIndex;
 175            }
 176
 177            // Determines the optimal child for insertion based on balance and cost metrics.
 299780178            SwiftBVHNode<T> leftChild = parentNode.HasLeftChild
 299780179               ? _nodePool[parentNode.LeftChildIndex]
 299780180               : SwiftBVHNode<T>.Default;
 299780181            SwiftBVHNode<T> rightChild = parentNode.HasRightChild
 299780182                ? _nodePool[parentNode.RightChildIndex]
 299780183                : SwiftBVHNode<T>.Default;
 184
 185            // Compute balance metrics
 299780186            int leftSize = leftChild.IsAllocated ? leftChild.SubtreeSize : 0;
 299780187            int rightSize = rightChild.IsAllocated ? rightChild.SubtreeSize : 0;
 188
 189            bool isInsertingLeft;
 299780190            if (Math.Abs(leftSize - rightSize) > _childBalanceThreshold)
 122980191            {
 192                // Insert into the smaller subtree to maintain balance
 122980193                if (leftSize < rightSize)
 115775194                    isInsertingLeft = true;
 195                else
 7205196                    isInsertingLeft = false;
 122980197            }
 198            else
 176800199            {
 200                // Compute cost metrics
 176800201                int leftCost = parentNode.HasLeftChild
 176800202                    ? leftChild.Bounds.GetCost(newNode.Bounds)
 176800203                    : 0;
 204
 176800205                int rightCost = parentNode.HasRightChild
 176800206                    ? rightChild.Bounds.GetCost(newNode.Bounds)
 176800207                    : 0;
 208
 209                // Insert into the child with the least volume increase
 176800210                if (leftCost < rightCost)
 19859211                    isInsertingLeft = true;
 212                else
 156941213                    isInsertingLeft = false;
 176800214            }
 215
 299780216            if (isInsertingLeft)
 135634217            {
 135634218                parentNode.LeftChildIndex = InsertIntoTree(parentNode.LeftChildIndex, newNodeIndex);
 135634219                leftChild = parentNode.HasLeftChild
 135634220                   ? _nodePool[parentNode.LeftChildIndex]
 135634221                   : SwiftBVHNode<T>.Default;
 135634222            }
 223            else
 164146224            {
 164146225                parentNode.RightChildIndex = InsertIntoTree(parentNode.RightChildIndex, newNodeIndex);
 164146226                rightChild = parentNode.HasRightChild
 164146227                    ? _nodePool[parentNode.RightChildIndex]
 164146228                    : SwiftBVHNode<T>.Default;
 164146229            }
 230
 299780231            parentNode.Bounds = GetCombinedBounds(leftChild, rightChild);
 299780232            parentNode.SubtreeSize = 1
 299780233                + (leftChild.IsAllocated ? leftChild.SubtreeSize : 0)
 299780234                + (rightChild.IsAllocated ? rightChild.SubtreeSize : 0);
 235
 299780236            return parentNodeIndex;
 328562237        }
 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)
 142436245        {
 142436246            int hash = value.GetHashCode() & 0x7FFFFFFF;
 142436247            int bucketIndex = hash & _bucketMask;
 248
 249            // Linear probe to find an empty slot
 151145250            while (_buckets[bucketIndex] != -1)
 8709251                bucketIndex = (bucketIndex + 1) & _bucketMask;
 252
 142436253            _buckets[bucketIndex] = nodeIndex;
 142436254        }
 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)
 2005261        {
 2005262            SwiftThrowHelper.ThrowIfNull(value, nameof(value));
 2005263            SwiftThrowHelper.ThrowIfNull(newBounds, nameof(newBounds));
 264
 2005265            int index = FindEntry(value);
 2005266            if (index == -1) return;
 267
 2005268            _bvhLock.EnterWriteLock();
 269            try
 2005270            {
 2005271                ref SwiftBVHNode<T> node = ref _nodePool[index];
 2006272                if (!node.IsAllocated) return; // Skip update if node has been removed
 273
 2004274                IBoundVolume oldBounds = node.Bounds;
 2004275                if (oldBounds != null && oldBounds.Equals(newBounds))
 0276                    return; // Skip unnecessary updates
 277
 2004278                node.Bounds = newBounds;
 279
 280                // Propagate changes up the tree
 2004281                int parentIndex = node.ParentIndex;
 7545282                while (parentIndex != -1)
 7249283                {
 7249284                    ref SwiftBVHNode<T> parent = ref _nodePool[parentIndex];
 7249285                    SwiftBVHNode<T> leftChild = parent.HasLeftChild
 7249286                        ? _nodePool[parent.LeftChildIndex]
 7249287                        : SwiftBVHNode<T>.Default;
 7249288                    SwiftBVHNode<T> rightChild = parent.HasRightChild
 7249289                        ? _nodePool[parent.RightChildIndex]
 7249290                        : SwiftBVHNode<T>.Default;
 291
 7249292                    IBoundVolume newParentBounds = GetCombinedBounds(leftChild, rightChild);
 7249293                    if (parent.Bounds.Equals(newParentBounds))
 1708294                        break; // No further updates needed
 295
 5541296                    parent.Bounds = newParentBounds;
 5541297                    parentIndex = parent.ParentIndex;
 5541298                }
 2004299            }
 300            finally
 2005301            {
 2005302                _bvhLock.ExitWriteLock();
 2005303            }
 2005304        }
 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)
 1859311        {
 1859312            SwiftThrowHelper.ThrowIfNull(value, nameof(value));
 313
 1859314            int nodeIndex = FindEntry(value);
 1863315            if (nodeIndex == -1) return false;
 316
 1855317            _bvhLock.EnterWriteLock();
 318            try
 1855319            {
 320                // If the node is the root and the only node, reset the BVH
 1855321                if (nodeIndex == RootNodeIndex && _leafCount == 1)
 2322                {
 2323                    Clear();
 2324                    return true;
 325                }
 326
 1853327                RemoveFromBuckets(value); // Ensure the bucket is cleared before further operations
 328
 329                // Remove node and update tree structure
 1853330                RemoveFromTree(nodeIndex);
 331
 1853332                return true;
 333            }
 334            finally
 1855335            {
 1855336                _bvhLock.ExitWriteLock();
 1855337            }
 1859338        }
 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)
 1853346        {
 1853347            int hash = value.GetHashCode() & 0x7FFFFFFF;
 1853348            int bucketIndex = hash & _bucketMask;
 349
 350            // Linear probing to resolve collisions
 1854351            while (_buckets[bucketIndex] != -1)
 1854352            {
 1854353                int nodeIndex = _buckets[bucketIndex];
 1854354                if (_nodePool[nodeIndex].IsLeaf && EqualityComparer<T>.Default.Equals(_nodePool[nodeIndex].Value, value)
 1853355                {
 1853356                    _buckets[bucketIndex] = -1;
 1853357                    RehashBucketCluster((bucketIndex + 1) & _bucketMask);
 1853358                    return;
 359                }
 360
 1361                bucketIndex = (bucketIndex + 1) & _bucketMask;
 1362            }
 1853363        }
 364
 365        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 366        private void RehashBucketCluster(int startIndex)
 1853367        {
 1853368            int bucketIndex = startIndex;
 369
 115507370            while (_buckets[bucketIndex] != -1)
 113654371            {
 113654372                int nodeIndex = _buckets[bucketIndex];
 113654373                _buckets[bucketIndex] = -1;
 374
 113654375                if (_nodePool[nodeIndex].IsLeaf && _nodePool[nodeIndex].IsAllocated)
 113654376                    InsertIntoBuckets(_nodePool[nodeIndex].Value, nodeIndex);
 377
 113654378                bucketIndex = (bucketIndex + 1) & _bucketMask;
 113654379            }
 1853380        }
 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)
 1853388        {
 21035389            while (nodeIndex != -1)
 19186390            {
 19186391                ref SwiftBVHNode<T> currentNode = ref _nodePool[nodeIndex];
 392
 19186393                int parentIndex = currentNode.ParentIndex;
 19186394                if (!currentNode.HasChildren)
 3639395                {
 396                    // Clean up leaf or empty parent
 3639397                    if (currentNode.HasParent)
 3635398                    {
 399                        // Remove the child reference from the parent root
 3635400                        ref SwiftBVHNode<T> parentNode = ref _nodePool[parentIndex];
 3635401                        if (parentNode.LeftChildIndex == nodeIndex)
 1839402                            parentNode.LeftChildIndex = -1;
 1796403                        else if (parentNode.RightChildIndex == nodeIndex)
 1796404                            parentNode.RightChildIndex = -1;
 3635405                    }
 4406                    else if (nodeIndex == RootNodeIndex)
 4407                    {
 4408                        Clear();
 4409                        return;
 410                    }
 411
 3635412                    if (currentNode.IsLeaf)
 1853413                        _leafCount--;
 414
 3635415                    currentNode.Reset();
 3635416                    _freeIndices.Push(nodeIndex);
 3635417                    nodeIndex = parentIndex;
 3635418                    continue;
 419                }
 420
 421                // Resize parent that still has children...
 15547422                SwiftBVHNode<T> leftChild = currentNode.HasLeftChild
 15547423                    ? _nodePool[currentNode.LeftChildIndex]
 15547424                    : SwiftBVHNode<T>.Default;
 15547425                SwiftBVHNode<T> rightChild = currentNode.HasRightChild
 15547426                    ? _nodePool[currentNode.RightChildIndex]
 15547427                    : SwiftBVHNode<T>.Default;
 428
 15547429                currentNode.Bounds = GetCombinedBounds(leftChild, rightChild);
 15547430                currentNode.SubtreeSize = 1
 15547431                    + (leftChild.IsAllocated ? leftChild.SubtreeSize : 0)
 15547432                    + (rightChild.IsAllocated ? rightChild.SubtreeSize : 0);
 433
 15547434                nodeIndex = parentIndex;
 15547435            }
 1853436        }
 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)
 2446        {
 2447            capacity = SwiftHashTools.NextPowerOfTwo(capacity);
 2448            if (capacity > _nodePool.Length)
 1449                Resize(capacity);
 2450        }
 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)
 58458        {
 58459            SwiftBVHNode<T>[] newArray = new SwiftBVHNode<T>[newSize];
 58460            Array.Copy(_nodePool, 0, newArray, 0, _peakIndex);
 461
 149058462            for (int i = _peakIndex; i < newSize; i++)
 74471463                newArray[i].Reset(); // set default index lookup values
 464
 58465            _nodePool = newArray;
 466
 58467            ResizeBuckets(newSize);
 58468        }
 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)
 58476        {
 148878477            _buckets = new int[newSize].Populate(() => -1);
 58478            _bucketMask = newSize - 1;
 479
 480            // Rehash existing leaf nodes
 148814481            for (int i = 0; i < _peakIndex; i++)
 74349482            {
 74349483                if (!_nodePool[i].IsLeaf)
 37147484                    continue;
 485
 37202486                int hash = _nodePool[i].Value.GetHashCode() & 0x7FFFFFFF;
 37202487                int bucketIndex = hash & _bucketMask;
 488
 489                // Linear probe to find an empty slot
 37533490                while (_buckets[bucketIndex] != -1)
 331491                    bucketIndex = (bucketIndex + 1) & _bucketMask;
 492
 37202493                _buckets[bucketIndex] = i;
 37202494            }
 58495        }
 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)
 322576507        {
 322576508            if (leftChild.IsAllocated && rightChild.IsAllocated)
 319753509                return leftChild.Bounds.Union(rightChild.Bounds);
 2823510            if (leftChild.IsAllocated)
 939511                return leftChild.Bounds;
 1884512            return rightChild.Bounds;
 322576513        }
 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)
 44520        {
 44521            SwiftThrowHelper.ThrowIfNull(queryBounds, nameof(queryBounds));
 522
 52523            if (RootNodeIndex == -1) return;
 524
 36525            _bvhLock.EnterReadLock();
 526            try
 36527            {
 36528                SwiftIntStack nodeStack = _threadLocalNodeStack.Value;
 36529                nodeStack.EnsureCapacity(_peakIndex + 1);
 36530                nodeStack.Clear();
 531
 36532                nodeStack.Push(RootNodeIndex);
 533
 44597534                while (nodeStack.Count > 0)
 44562535                {
 44562536                    int index = nodeStack.Pop();
 44562537                    SwiftBVHNode<T> node = _nodePool[index];
 538
 44562539                    if (!node.IsAllocated)
 1540                        throw new InvalidOperationException($"Encountered an unallocated node at index {index} during qu
 541
 44561542                    if (!queryBounds.Intersects(node.Bounds))
 1058543                        continue;
 544
 43503545                    if (node.IsLeaf)
 21211546                    {
 21211547                        results.Add(node.Value);
 21211548                        continue;
 549                    }
 550
 22292551                    if (node.HasLeftChild)
 22242552                        nodeStack.Push(node.LeftChildIndex);
 22292553                    if (node.HasRightChild)
 22284554                        nodeStack.Push(node.RightChildIndex);
 22292555                }
 35556            }
 557            finally
 36558            {
 36559                _bvhLock.ExitReadLock();
 36560            }
 43561        }
 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)
 3877568        {
 3877569            SwiftThrowHelper.ThrowIfNull(value, nameof(value));
 570
 3877571            _bvhLock.EnterReadLock();
 572            try
 3877573            {
 3877574                int hash = value.GetHashCode() & 0x7FFFFFFF;
 3877575                int bucketIndex = hash & _bucketMask;
 576
 577                // Linear probing to resolve collisions
 3884578                while (_buckets[bucketIndex] != -1)
 3876579                {
 3876580                    int nodeIndex = _buckets[bucketIndex];
 3876581                    if (_nodePool[nodeIndex].IsLeaf && EqualityComparer<T>.Default.Equals(_nodePool[nodeIndex].Value, va
 3869582                        return nodeIndex;
 583
 7584                    bucketIndex = (bucketIndex + 1) & _bucketMask;
 7585                }
 586
 8587                return -1; // Not found
 588            }
 589            finally
 3877590            {
 3877591                _bvhLock.ExitReadLock();
 3877592            }
 3877593        }
 594
 595        /// <summary>
 596        /// Clears the BVH, resetting all nodes, buckets, and metadata.
 597        /// </summary>
 598        public void Clear()
 8599        {
 8600            if (RootNodeIndex == -1) return;
 601
 6816602            for (int i = 0; i < _peakIndex; i++)
 3400603                _nodePool[i].Reset();
 604
 8848605            for (int i = 0; i < _buckets.Length; i++)
 4416606                _buckets[i] = -1;
 607
 8608            _freeIndices.Reset();
 609
 8610            _leafCount = 0;
 8611            _peakIndex = 0;
 8612            _rootNodeIndex = -1;
 8613        }
 614
 615        #endregion
 616    }
 617}