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);
}
