void ForceInsert(int id, int nodeId, RotRect bounds, Point xy)
{
var node = _nodes[nodeId];
_items[id]._bounds = bounds;
_items[id]._xy = xy;
_items[id]._node = nodeId;
_items[id]._next = -1;
if (node._itemCount == 0)
{
_nodes[nodeId]._firstChild = id;
}
else
{
var itemId = node._firstChild;
var item = _items[itemId];
for (var i = 1; i < node._itemCount; i++)
{
itemId = item._next;
item = _items[itemId];
}
_items[itemId]._next = id;
}
_nodes[nodeId]._itemCount++;
}
public void Insert(int id, Vector2 xy, Vector2 size, float angle = 0, Vector2 origin = default)
{
var bounds = new RotRect(xy, size, angle, origin);
var aabb = bounds.AABB;
var center = aabb.Center;
int nodeId = Insert(id, 0, bounds, center);
TrySubdivide(nodeId);
_nodesToGrow.Add(nodeId);
}
int Insert(int id, int nodeId, RotRect bounds, Point xy)
{
do
{
var n = _nodes[nodeId];
if (n._itemCount < 0)
{
nodeId = xy.X < n._cX ? xy.Y < n._cY ? n._firstChild : n._firstChild + 3 : xy.Y < n._cY ? n._firstChild + 1 : n._firstChild + 2;
continue;
}
ForceInsert(id, nodeId, bounds, xy);
return(nodeId);
} while (true);
}
public IEnumerable <Rectangle> Nodes(RotRect area = default)
{
if (area.Size == Vector2.Zero)
{
area = new RotRect(_bounds);
}
_toProcess.Push(0);
do
{
var nId = _toProcess.Pop();
var n = _nodes[nId];
yield return(n._bounds);
if (n._itemCount < 0)
{
var node2 = _nodes[n._firstChild];
if (area.Intersects(node2._bounds))
{
_toProcess.Push(n._firstChild);
}
node2 = _nodes[n._firstChild + 1];
if (area.Intersects(node2._bounds))
{
_toProcess.Push(n._firstChild + 1);
}
node2 = _nodes[n._firstChild + 2];
if (area.Intersects(node2._bounds))
{
_toProcess.Push(n._firstChild + 2);
}
node2 = _nodes[n._firstChild + 3];
if (area.Intersects(node2._bounds))
{
_toProcess.Push(n._firstChild + 3);
}
continue;
}
} while (_toProcess.Count > 0);
yield break;
}
/// <summary>Query and return the items intersecting <paramref name="rect"/>.</summary>
public IEnumerable <int> Query(RotRect rect)
{
_toProcess.Push(0);
do
{
var nId = _toProcess.Pop();
var n = _nodes[nId];
if (n._itemCount < 0)
{
var node2 = _nodes[n._firstChild];
if (rect.Intersects(node2._bounds))
{
_toProcess.Push(n._firstChild);
}
node2 = _nodes[n._firstChild + 1];
if (rect.Intersects(node2._bounds))
{
_toProcess.Push(n._firstChild + 1);
}
node2 = _nodes[n._firstChild + 2];
if (rect.Intersects(node2._bounds))
{
_toProcess.Push(n._firstChild + 2);
}
node2 = _nodes[n._firstChild + 3];
if (rect.Intersects(node2._bounds))
{
_toProcess.Push(n._firstChild + 3);
}
continue;
}
if (n._itemCount > 0)
{
var item = _items[n._firstChild];
if (rect.Contains(n._bounds))
{
//do {
// yield return item._id;
// if (item._next < 0)
// break;
// item = _items[item._next];
//} while (true);
var itemId = n._firstChild;
for (var i = 0; i < n._itemCount; i++)
{
item = _items[itemId];
yield return(item._id);
itemId = item._next;
}
}
else
{
do
{
if (rect.Intersects(item._bounds))
{
yield return(item._id);
}
if (item._next < 0)
{
break;
}
item = _items[item._next];
} while (true);
}
}
} while (_toProcess.Count > 0);
yield break;
}
/// <summary>Updates position of <paramref name="id"/> in the tree.</summary>
/// <returns>True if <paramref name="id"/> has been updated, otherwise false.</returns>
public bool Update(int id, Vector2 xy, Vector2 size, float angle = 0, Vector2 origin = default)
{
var bounds = new RotRect(xy, size, angle, origin);
var aabb = bounds.AABB;
var center = aabb.Center;
var item = _items[id];
var node = _nodes[item._node];
int halfWidth = _bounds.Width >> node._depth,
halfHeight = _bounds.Height >> node._depth;
var bounds2 = new Rectangle(node._cX - halfWidth, node._cY - halfHeight, halfWidth << 1, halfHeight << 1);
if (bounds2.Contains(center) || node._parent == -1)
{
_items[id]._bounds = bounds;
_items[id]._xy = center;
_nodesToGrow.Add(item._node);
return(true);
}
if (node._firstChild == id)
{
_nodes[item._node]._firstChild = item._next;
}
else
{
var prevItem = node._firstChild;
var curItem = _items[prevItem]._next;
for (var i = 0; i < node._itemCount; i++)
{
if (curItem == id)
{
_items[prevItem]._next = _items[curItem]._next;
break;
}
prevItem = curItem;
curItem = _items[curItem]._next;
}
}
_nodes[item._node]._itemCount--;
_nodesToGrow.Add(item._node);
var nodeId = item._node;
do
{
if (node._parent < 0)
{
break;
}
var parentNode = _nodes[node._parent];
halfWidth = _bounds.Width >> parentNode._depth;
halfHeight = _bounds.Height >> parentNode._depth;
bounds2 = new Rectangle(parentNode._cX - halfWidth, parentNode._cY - halfHeight, halfWidth << 1, halfHeight << 1);
if (bounds2.Contains(center))
{
nodeId = node._parent;
break;
}
nodeId = node._parent;
node = _nodes[nodeId];
} while (true);
var nId2 = Insert(id, nodeId, bounds, center);
TrySubdivide(nId2);
_nodesToGrow.Add(nId2);
return(true);
}
