/** returns a node to the pool */
private final void freeNode(DynamicTreeNode node) {
assert (node != null);
assert (0 < m_nodeCount);
node.parent = m_freeList != NULL_NODE ? m_nodes[m_freeList] : null;
node.height = -1;
m_freeList = node.id;
m_nodeCount--;
}
@Override
public final int createProxy(final org.jbox2d.collision.AABB aabb, Object userData) {
assert (aabb.isValid());
final DynamicTreeNode node = allocateNode();
int proxyId = node.id;
// Fatten the aabb
final org.jbox2d.collision.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;
}
private final DynamicTreeNode allocateNode() {
if (m_freeList == NULL_NODE) {
assert (m_nodeCount == m_nodeCapacity);
DynamicTreeNode[] old = m_nodes;
m_nodeCapacity *= 2;
m_nodes = new DynamicTreeNode[m_nodeCapacity];
System.arraycopy(old, 0, m_nodes, 0, old.length);
// Build a linked list for the free list.
for (int i = m_nodeCapacity - 1; i >= m_nodeCount; i--) {
m_nodes[i] = new DynamicTreeNode(i);
m_nodes[i].parent = (i == m_nodeCapacity - 1) ? null : m_nodes[i + 1];
m_nodes[i].height = -1;
}
m_freeList = m_nodeCount;
}
int nodeId = m_freeList;
final DynamicTreeNode treeNode = m_nodes[nodeId];
m_freeList = treeNode.parent != null ? treeNode.parent.id : NULL_NODE;
treeNode.parent = null;
treeNode.child1 = null;
treeNode.child2 = null;
treeNode.height = 0;
treeNode.userData = null;
++m_nodeCount;
return treeNode;
}
private final void removeLeaf(DynamicTreeNode leaf) {
if (leaf == m_root) {
m_root = null;
return;
}
DynamicTreeNode parent = leaf.parent;
DynamicTreeNode grandParent = parent.parent;
DynamicTreeNode sibling;
if (parent.child1 == leaf) {
sibling = parent.child2;
} else {
sibling = parent.child1;
}
if (grandParent != null) {
// Destroy parent and connect sibling to grandParent.
if (grandParent.child1 == parent) {
grandParent.child1 = sibling;
} else {
grandParent.child2 = sibling;
}
sibling.parent = grandParent;
freeNode(parent);
// Adjust ancestor bounds.
DynamicTreeNode index = grandParent;
while (index != null) {
index = balance(index);
DynamicTreeNode child1 = index.child1;
DynamicTreeNode child2 = index.child2;
index.aabb.combine(child1.aabb, child2.aabb);
index.height = 1 + org.jbox2d.common.MathUtils.max(child1.height, child2.height);
index = index.parent;
}
} else {
m_root = sibling;
sibling.parent = null;
freeNode(parent);
}
// validate();
}
// Perform a left or right rotation if node A is imbalanced.
// Returns the new root index.
private DynamicTreeNode balance(DynamicTreeNode iA) {
assert (iA != null);
DynamicTreeNode A = iA;
if (A.child1 == null || A.height < 2) {
return iA;
}
DynamicTreeNode iB = A.child1;
DynamicTreeNode iC = A.child2;
assert (0 <= iB.id && iB.id < m_nodeCapacity);
assert (0 <= iC.id && iC.id < m_nodeCapacity);
DynamicTreeNode B = iB;
DynamicTreeNode C = iC;
int balance = C.height - B.height;
// Rotate C up
if (balance > 1) {
DynamicTreeNode iF = C.child1;
DynamicTreeNode iG = C.child2;
DynamicTreeNode F = iF;
DynamicTreeNode G = iG;
assert (F != null);
assert (G != null);
assert (0 <= iF.id && iF.id < m_nodeCapacity);
assert (0 <= iG.id && iG.id < m_nodeCapacity);
// Swap A and C
C.child1 = iA;
C.parent = A.parent;
A.parent = iC;
// A's old parent should point to C
if (C.parent != null) {
if (C.parent.child1 == iA) {
C.parent.child1 = iC;
} else {
assert (C.parent.child2 == iA);
C.parent.child2 = iC;
}
} else {
m_root = iC;
}
// Rotate
if (F.height > G.height) {
C.child2 = iF;
A.child2 = iG;
G.parent = iA;
A.aabb.combine(B.aabb, G.aabb);
C.aabb.combine(A.aabb, F.aabb);
A.height = 1 + org.jbox2d.common.MathUtils.max(B.height, G.height);
C.height = 1 + org.jbox2d.common.MathUtils.max(A.height, F.height);
} else {
C.child2 = iG;
A.child2 = iF;
F.parent = iA;
A.aabb.combine(B.aabb, F.aabb);
C.aabb.combine(A.aabb, G.aabb);
A.height = 1 + org.jbox2d.common.MathUtils.max(B.height, F.height);
C.height = 1 + org.jbox2d.common.MathUtils.max(A.height, G.height);
}
return iC;
}
// Rotate B up
if (balance < -1) {
DynamicTreeNode iD = B.child1;
DynamicTreeNode iE = B.child2;
DynamicTreeNode D = iD;
DynamicTreeNode E = iE;
assert (0 <= iD.id && iD.id < m_nodeCapacity);
assert (0 <= iE.id && iE.id < m_nodeCapacity);
// Swap A and B
B.child1 = iA;
B.parent = A.parent;
A.parent = iB;
// A's old parent should point to B
if (B.parent != null) {
if (B.parent.child1 == iA) {
B.parent.child1 = iB;
} else {
assert (B.parent.child2 == iA);
B.parent.child2 = iB;
}
} else {
m_root = iB;
}
// Rotate
if (D.height > E.height) {
B.child2 = iD;
A.child1 = iE;
E.parent = iA;
A.aabb.combine(C.aabb, E.aabb);
B.aabb.combine(A.aabb, D.aabb);
A.height = 1 + org.jbox2d.common.MathUtils.max(C.height, E.height);
B.height = 1 + org.jbox2d.common.MathUtils.max(A.height, D.height);
} else {
B.child2 = iE;
A.child1 = iD;
D.parent = iA;
A.aabb.combine(C.aabb, D.aabb);
B.aabb.combine(A.aabb, E.aabb);
A.height = 1 + org.jbox2d.common.MathUtils.max(C.height, D.height);
B.height = 1 + org.jbox2d.common.MathUtils.max(A.height, E.height);
}
return iB;
}
return iA;
}
private final void insertLeaf(int leaf_index) {
DynamicTreeNode leaf = m_nodes[leaf_index];
if (m_root == null) {
m_root = leaf;
m_root.parent = null;
return;
}
// find the best sibling
org.jbox2d.collision.AABB leafAABB = leaf.aabb;
DynamicTreeNode index = m_root;
while (index.child1 != null) {
final DynamicTreeNode node = index;
DynamicTreeNode child1 = node.child1;
DynamicTreeNode child2 = node.child2;
float area = node.aabb.getPerimeter();
combinedAABB.combine(node.aabb, leafAABB);
float combinedArea = combinedAABB.getPerimeter();
// Cost of creating a new parent for this node and the new leaf
float cost = 2.0f * combinedArea;
// Minimum cost of pushing the leaf further down the tree
float inheritanceCost = 2.0f * (combinedArea - area);
// Cost of descending into child1
float cost1;
if (child1.child1 == null) {
combinedAABB.combine(leafAABB, child1.aabb);
cost1 = combinedAABB.getPerimeter() + inheritanceCost;
} else {
combinedAABB.combine(leafAABB, child1.aabb);
float oldArea = child1.aabb.getPerimeter();
float newArea = combinedAABB.getPerimeter();
cost1 = (newArea - oldArea) + inheritanceCost;
}
// Cost of descending into child2
float cost2;
if (child2.child1 == null) {
combinedAABB.combine(leafAABB, child2.aabb);
cost2 = combinedAABB.getPerimeter() + inheritanceCost;
} else {
combinedAABB.combine(leafAABB, child2.aabb);
float oldArea = child2.aabb.getPerimeter();
float newArea = combinedAABB.getPerimeter();
cost2 = newArea - oldArea + inheritanceCost;
}
// Descend according to the minimum cost.
if (cost < cost1 && cost < cost2) {
break;
}
// Descend
if (cost1 < cost2) {
index = child1;
} else {
index = child2;
}
}
DynamicTreeNode sibling = index;
DynamicTreeNode oldParent = m_nodes[sibling.id].parent;
final DynamicTreeNode newParent = allocateNode();
newParent.parent = oldParent;
newParent.userData = null;
newParent.aabb.combine(leafAABB, sibling.aabb);
newParent.height = sibling.height + 1;
if (oldParent != null) {
// The sibling was not the root.
if (oldParent.child1 == sibling) {
oldParent.child1 = newParent;
} else {
oldParent.child2 = newParent;
}
newParent.child1 = sibling;
newParent.child2 = leaf;
sibling.parent = newParent;
leaf.parent = newParent;
} else {
// The sibling was the root.
newParent.child1 = sibling;
newParent.child2 = leaf;
sibling.parent = newParent;
leaf.parent = newParent;
m_root = newParent;
}
// Walk back up the tree fixing heights and AABBs
index = leaf.parent;
while (index != null) {
index = balance(index);
DynamicTreeNode child1 = index.child1;
DynamicTreeNode child2 = index.child2;
assert (child1 != null);
assert (child2 != null);
index.height = 1 + org.jbox2d.common.MathUtils.max(child1.height, child2.height);
index.aabb.combine(child1.aabb, child2.aabb);
index = index.parent;
}
// validate();
}
/** Build an optimal tree. Very expensive. For testing. */
public void rebuildBottomUp() {
int[] nodes = new int[m_nodeCount];
int count = 0;
// Build array of leaves. Free the rest.
for (int i = 0; i < m_nodeCapacity; ++i) {
if (m_nodes[i].height < 0) {
// free node in pool
continue;
}
DynamicTreeNode node = m_nodes[i];
if (node.child1 == null) {
node.parent = null;
nodes[count] = i;
++count;
} else {
freeNode(node);
}
}
org.jbox2d.collision.AABB b = new org.jbox2d.collision.AABB();
while (count > 1) {
float minCost = Float.MAX_VALUE;
int iMin = -1, jMin = -1;
for (int i = 0; i < count; ++i) {
org.jbox2d.collision.AABB aabbi = m_nodes[nodes[i]].aabb;
for (int j = i + 1; j < count; ++j) {
org.jbox2d.collision.AABB aabbj = m_nodes[nodes[j]].aabb;
b.combine(aabbi, aabbj);
float cost = b.getPerimeter();
if (cost < minCost) {
iMin = i;
jMin = j;
minCost = cost;
}
}
}
int index1 = nodes[iMin];
int index2 = nodes[jMin];
DynamicTreeNode child1 = m_nodes[index1];
DynamicTreeNode child2 = m_nodes[index2];
DynamicTreeNode parent = allocateNode();
parent.child1 = child1;
parent.child2 = child2;
parent.height = 1 + org.jbox2d.common.MathUtils.max(child1.height, child2.height);
parent.aabb.combine(child1.aabb, child2.aabb);
parent.parent = null;
child1.parent = parent;
child2.parent = parent;
nodes[jMin] = nodes[count - 1];
nodes[iMin] = parent.id;
--count;
}
m_root = m_nodes[nodes[0]];
validate();
}
