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