public void Insert(Vertex *v)
{
var p = v->Point;
var node = _root;
while (!node->IsLeaf)
{
node = node->GetChild(p);
}
while (node->Count == _bucketSize)
{
var hs = node->HalfSize / 2;
var o = node->Origin;
node->BL = _nodes.Set(new QuadTreeNode(o - hs, hs, node->Data, node));
node->TL = _nodes.Set(new QuadTreeNode(o + new float2(-hs, hs), hs, GetChunk(), node));
node->BR = _nodes.Set(new QuadTreeNode(o + new float2(hs, -hs), hs, GetChunk(), node));
node->TR = _nodes.Set(new QuadTreeNode(o + hs, hs, GetChunk(), node));
node->Count = 0;
for (int i = 0; i < _bucketSize; i++)
{
var pt = node->Data[i]->Point;
if (pt.x < o.x)
{
if (pt.y < o.y)
{
node->BL->Data[node->BL->Count++] = node->BL->Data[i];
}
else
{
node->TL->Data[node->TL->Count++] = node->BL->Data[i];
}
}
else
{
if (pt.y < o.y)
{
node->BR->Data[node->BR->Count++] = node->BL->Data[i];
}
else
{
node->TR->Data[node->TR->Count++] = node->BL->Data[i];
}
}
}
node = node->GetChild(p);
}
node->Data[node->Count++] = v;
}
public QuadTree(float size, int initialCapacity, int bucketSize, Allocator allocator) : this()
{
_bucketSize = bucketSize;
_allocator = allocator;
var buckets = (int)math.ceil((float)initialCapacity / bucketSize);
_chunks = new Stack <IntPtr>(buckets, allocator);
for (int i = 0; i < buckets; i++)
{
_chunks.Push((IntPtr)Malloc());
}
_nodes = new PersistentStore <QuadTreeNode>((int)(1.3334f * buckets), allocator);
_root = _nodes.Set(new QuadTreeNode(0, size / 2, GetChunk(), null));
}