public bool moveProxy(int proxyId, AABB aabb, Vec2 displacement)
{
Debug.Assert(aabb.isValid());
Debug.Assert(0 <= proxyId && proxyId < m_nodeCapacity);
DynamicTreeNode node = m_nodes[proxyId];
Debug.Assert(node.child1 == null);
AABB nodeAABB = node.aabb;
// if (nodeAABB.contains(aabb)) {
if (nodeAABB.lowerBound.x <= aabb.lowerBound.x && nodeAABB.lowerBound.y <= aabb.lowerBound.y
&& aabb.upperBound.x <= nodeAABB.upperBound.x && aabb.upperBound.y <= nodeAABB.upperBound.y)
{
return false;
}
removeLeaf(node);
// Extend AABB
Vec2 lowerBound = nodeAABB.lowerBound;
Vec2 upperBound = nodeAABB.upperBound;
lowerBound.x = aabb.lowerBound.x - Settings.aabbExtension;
lowerBound.y = aabb.lowerBound.y - Settings.aabbExtension;
upperBound.x = aabb.upperBound.x + Settings.aabbExtension;
upperBound.y = aabb.upperBound.y + Settings.aabbExtension;
// Predict AABB displacement.
float dx = displacement.x*Settings.aabbMultiplier;
float dy = displacement.y*Settings.aabbMultiplier;
if (dx < 0.0f)
{
lowerBound.x += dx;
}
else
{
upperBound.x += dx;
}
if (dy < 0.0f)
{
lowerBound.y += dy;
}
else
{
upperBound.y += dy;
}
insertLeaf(proxyId);
return true;
}
public void query(TreeCallback callback, AABB aabb)
{
Debug.Assert(aabb.isValid());
nodeStackIndex = 0;
nodeStack[nodeStackIndex++] = m_root;
while (nodeStackIndex > 0)
{
DynamicTreeNode node = nodeStack[--nodeStackIndex];
if (node == null)
{
continue;
}
if (AABB.testOverlap(node.aabb, aabb))
{
if (node.child1 == null)
{
bool proceed = callback.treeCallback(node.id);
if (!proceed)
{
return;
}
}
else
{
if (nodeStack.Length - nodeStackIndex - 2 <= 0)
{
DynamicTreeNode[] newBuffer = new DynamicTreeNode[nodeStack.Length*2];
Array.Copy(nodeStack, 0, newBuffer, 0, nodeStack.Length);
nodeStack = newBuffer;
}
nodeStack[nodeStackIndex++] = node.child1;
nodeStack[nodeStackIndex++] = node.child2;
}
}
}
}
public int createProxy(AABB aabb, object userData)
{
Debug.Assert(aabb.isValid());
DynamicTreeNode node = allocateNode();
int proxyId = node.id;
// Fatten the aabb
AABB nodeAABB = node.aabb;
nodeAABB.lowerBound.x = aabb.lowerBound.x - Settings.aabbExtension;
nodeAABB.lowerBound.y = aabb.lowerBound.y - Settings.aabbExtension;
nodeAABB.upperBound.x = aabb.upperBound.x + Settings.aabbExtension;
nodeAABB.upperBound.y = aabb.upperBound.y + Settings.aabbExtension;
node.userData = userData;
insertLeaf(proxyId);
return proxyId;
}