public DynamicTree() { m_root = null; m_nodeCount = 0; m_nodeCapacity = 16; m_nodes = new DynamicTreeNode[16]; // Build a linked list for the free list. for (int i = m_nodeCapacity - 1; i >= 0; 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 = 0; for (int i = 0; i < drawVecs.Length; i++) { drawVecs[i] = new Vec2(); } }
private void validateStructure(DynamicTreeNode node) { if (node == null) { return; } Debug.Assert(node == m_nodes[node.id]); if (node == m_root) { Debug.Assert(node.parent == null); } DynamicTreeNode child1 = node.child1; DynamicTreeNode child2 = node.child2; if (node.child1 == null) { Debug.Assert(child1 == null); Debug.Assert(child2 == null); Debug.Assert(node.height == 0); return; } Debug.Assert(child1 != null && 0 <= child1.id && child1.id < m_nodeCapacity); Debug.Assert(child2 != null && 0 <= child2.id && child2.id < m_nodeCapacity); Debug.Assert(child1.parent == node); Debug.Assert(child2.parent == node); validateStructure(child1); validateStructure(child2); }
private void validateMetrics(DynamicTreeNode node) { if (node == null) { return; } DynamicTreeNode child1 = node.child1; DynamicTreeNode child2 = node.child2; if (node.child1 == null) { Debug.Assert(child1 == null); Debug.Assert(child2 == null); Debug.Assert(node.height == 0); return; } Debug.Assert(child1 != null && 0 <= child1.id && child1.id < m_nodeCapacity); Debug.Assert(child2 != null && 0 <= child2.id && child2.id < m_nodeCapacity); int height1 = child1.height; int height2 = child2.height; int height; height = 1 + MathUtils.max(height1, height2); Debug.Assert(node.height == height); AABB aabb = new AABB(); aabb.combine(child1.aabb, child2.aabb); Debug.Assert(aabb.lowerBound.Equals(node.aabb.lowerBound)); Debug.Assert(aabb.upperBound.Equals(node.aabb.upperBound)); validateMetrics(child1); validateMetrics(child2); }
private 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 + MathUtils.max(child1.height, child2.height); index = index.parent; } } else { m_root = sibling; sibling.parent = null; freeNode(parent); } // validate(); }
private 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 AABB leafAABB = leaf.aabb; DynamicTreeNode index = m_root; while (index.child1 != null) { 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; 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; Debug.Assert(child1 != null); Debug.Assert(child2 != null); index.height = 1 + MathUtils.max(child1.height, child2.height); index.aabb.combine(child1.aabb, child2.aabb); index = index.parent; } // validate(); }
/** * returns a node to the pool */ private void freeNode(DynamicTreeNode node) { Debug.Assert(node != null); Debug.Assert(0 < m_nodeCount); node.parent = m_freeList != NULL_NODE ? m_nodes[m_freeList] : null; node.height = -1; m_freeList = node.id; m_nodeCount--; }
private int computeHeight(DynamicTreeNode node) { Debug.Assert(0 <= node.id && node.id < m_nodeCapacity); if (node.child1 == null) { return 0; } int height1 = computeHeight(node.child1); int height2 = computeHeight(node.child2); return 1 + MathUtils.max(height1, height2); }
// Perform a left or right rotation if node A is imbalanced. // Returns the new root index. private DynamicTreeNode balance(DynamicTreeNode iA) { Debug.Assert(iA != null); DynamicTreeNode A = iA; if (A.child1 == null || A.height < 2) { return iA; } DynamicTreeNode iB = A.child1; DynamicTreeNode iC = A.child2; Debug.Assert(0 <= iB.id && iB.id < m_nodeCapacity); Debug.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; Debug.Assert(F != null); Debug.Assert(G != null); Debug.Assert(0 <= iF.id && iF.id < m_nodeCapacity); Debug.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 { Debug.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 + MathUtils.max(B.height, G.height); C.height = 1 + 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 + MathUtils.max(B.height, F.height); C.height = 1 + 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; Debug.Assert(0 <= iD.id && iD.id < m_nodeCapacity); Debug.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 { Debug.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 + MathUtils.max(C.height, E.height); B.height = 1 + 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 + MathUtils.max(C.height, D.height); B.height = 1 + MathUtils.max(A.height, E.height); } return iB; } return iA; }
private DynamicTreeNode allocateNode() { if (m_freeList == NULL_NODE) { Debug.Assert(m_nodeCount == m_nodeCapacity); DynamicTreeNode[] old = m_nodes; m_nodeCapacity *= 2; m_nodes = new DynamicTreeNode[m_nodeCapacity]; Array.Copy(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; 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; }
/** * 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); } } AABB b = new AABB(); while (count > 1) { float minCost = float.MaxValue; int iMin = -1, jMin = -1; for (int i = 0; i < count; ++i) { AABB aabbi = m_nodes[nodes[i]].aabb; for (int j = i + 1; j < count; ++j) { 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 + 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(); }
public void raycast(TreeRayCastCallback callback, RayCastInput input) { Vec2 p1 = input.p1; Vec2 p2 = input.p2; float p1x = p1.x, p2x = p2.x, p1y = p1.y, p2y = p2.y; float vx, vy; float rx, ry; float absVx, absVy; float cx, cy; float hx, hy; float tempx, tempy; r.x = p2x - p1x; r.y = p2y - p1y; Debug.Assert((r.x*r.x + r.y*r.y) > 0f); r.normalize(); rx = r.x; ry = r.y; // v is perpendicular to the segment. vx = -1f*ry; vy = 1f*rx; absVx = MathUtils.abs(vx); absVy = MathUtils.abs(vy); // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) float maxFraction = input.maxFraction; // Build a bounding box for the segment. AABB segAABB = aabb; // Vec2 t = p1 + maxFraction * (p2 - p1); // before inline // temp.set(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1); // Vec2.minToOut(p1, temp, segAABB.lowerBound); // Vec2.maxToOut(p1, temp, segAABB.upperBound); tempx = (p2x - p1x)*maxFraction + p1x; tempy = (p2y - p1y)*maxFraction + p1y; segAABB.lowerBound.x = p1x < tempx ? p1x : tempx; segAABB.lowerBound.y = p1y < tempy ? p1y : tempy; segAABB.upperBound.x = p1x > tempx ? p1x : tempx; segAABB.upperBound.y = p1y > tempy ? p1y : tempy; // end inline nodeStackIndex = 0; nodeStack[nodeStackIndex++] = m_root; while (nodeStackIndex > 0) { DynamicTreeNode node = nodeStack[--nodeStackIndex]; if (node == null) { continue; } AABB nodeAABB = node.aabb; if (!AABB.testOverlap(nodeAABB, segAABB)) { continue; } // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) // node.aabb.getCenterToOut(c); // node.aabb.getExtentsToOut(h); cx = (nodeAABB.lowerBound.x + nodeAABB.upperBound.x)*.5f; cy = (nodeAABB.lowerBound.y + nodeAABB.upperBound.y)*.5f; hx = (nodeAABB.upperBound.x - nodeAABB.lowerBound.x)*.5f; hy = (nodeAABB.upperBound.y - nodeAABB.lowerBound.y)*.5f; tempx = p1x - cx; tempy = p1y - cy; float separation = MathUtils.abs(vx*tempx + vy*tempy) - (absVx*hx + absVy*hy); if (separation > 0.0f) { continue; } if (node.child1 == null) { subInput.p1.x = p1x; subInput.p1.y = p1y; subInput.p2.x = p2x; subInput.p2.y = p2y; subInput.maxFraction = maxFraction; float value = callback.raycastCallback(subInput, node.id); if (value == 0.0f) { // The client has terminated the ray cast. return; } if (value > 0.0f) { // Update segment bounding box. maxFraction = value; // temp.set(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1); // Vec2.minToOut(p1, temp, segAABB.lowerBound); // Vec2.maxToOut(p1, temp, segAABB.upperBound); tempx = (p2x - p1x)*maxFraction + p1x; tempy = (p2y - p1y)*maxFraction + p1y; segAABB.lowerBound.x = p1x < tempx ? p1x : tempx; segAABB.lowerBound.y = p1y < tempy ? p1y : tempy; segAABB.upperBound.x = p1x > tempx ? p1x : tempx; segAABB.upperBound.y = p1y > tempy ? p1y : tempy; } } 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 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 void drawTree(DebugDraw argDraw, DynamicTreeNode node, int spot, int height) { node.aabb.getVertices(drawVecs); color.set(1, (height - spot)*1f/height, (height - spot)*1f/height); argDraw.drawPolygon(drawVecs, 4, color); argDraw.getViewportTranform().getWorldToScreen(node.aabb.upperBound, textVec); argDraw.drawString(textVec.x, textVec.y, node.id + "-" + (spot + 1) + "/" + height, color); if (node.child1 != null) { drawTree(argDraw, node.child1, spot + 1, height); } if (node.child2 != null) { drawTree(argDraw, node.child2, spot + 1, height); } }