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