public NativeBVHTree(int initialCapacity = 64, Allocator allocator = Allocator.Temp, Configuration config = default) : this() { nodesList = UnsafeNodesList.Create(initialCapacity, allocator, NativeArrayOptions.ClearMemory); rootIndex = new NativeArray <int>(1, allocator); // Create invalid node (at index 0) AllocInternalNode(); insertionHeap = new UnsafeMinHeap(initialCapacity, allocator); this.config = config; }
public int InsertLeaf(Leaf entry) { var leafIndex = AllocLeafNode(ref entry); var bounds = nodes[leafIndex]->box; if (rootIndex[0] == InvalidNode) { rootIndex[0] = leafIndex; return(leafIndex); } // Stage 1: find the best sibling for the new leaf float bestCost = float.MaxValue; int bestIndex = -1; var heap = stackalloc UnsafeMinHeap.HeapItem[TreeTraversalStackSize]; var insertionHeap = new UnsafeMinHeap.MinHeap { count = 0, heap = heap, }; UnsafeMinHeap.Push(ref insertionHeap, new UnsafeMinHeap.HeapItem { Id = rootIndex[0], Cost = 0 }); while (insertionHeap.count != 0) { var heapItem = UnsafeMinHeap.Pop(ref insertionHeap); var node = nodes[heapItem.Id]; var union = node->box.Union(bounds); var directCost = union.Area(); var cost = directCost + heapItem.Cost; if (cost < bestCost) { bestCost = cost; bestIndex = heapItem.Id; } var extraInheritedCost = union.Area() - node->box.Area(); var totalInheritedCost = heapItem.Cost + extraInheritedCost; var lowerBoundChildrenCost = bounds.Area() + totalInheritedCost; if (lowerBoundChildrenCost < bestCost) { if (node->child1 != InvalidNode) { UnsafeMinHeap.Push(ref insertionHeap, new UnsafeMinHeap.HeapItem { Id = node->child1, Cost = totalInheritedCost }); } if (node->child2 != InvalidNode) { UnsafeMinHeap.Push(ref insertionHeap, new UnsafeMinHeap.HeapItem { Id = node->child2, Cost = totalInheritedCost }); } } } #if ENABLE_UNITY_COLLECTIONS_CHECKS if (bestIndex <= InvalidNode) { throw new InvalidOperationException(); } #endif var sibling = bestIndex; // Stage 2: create a new parent int oldParent = nodes[sibling]->parentIndex; int newParent = AllocInternalNode(); nodes[newParent]->parentIndex = oldParent; nodes[newParent]->box = bounds.Union(nodes[sibling]->box); if (oldParent != InvalidNode) { // The sibling was not the root if (nodes[oldParent]->child1 == sibling) { nodes[oldParent]->child1 = newParent; } else { nodes[oldParent]->child2 = newParent; } } else { // The sibling was the root rootIndex[0] = newParent; } nodes[newParent]->child1 = sibling; nodes[newParent]->child2 = leafIndex; nodes[sibling]->parentIndex = newParent; nodes[leafIndex]->parentIndex = newParent; // Stage 3: walk back up the tree refitting AABBs RefitParents(nodes[leafIndex]->parentIndex); return(leafIndex); }