/// Constructing the tree initializes the node pool. public b2DynamicTree() { m_root = Settings.b2_nullNode; m_nodeCapacity = 16; m_nodeCount = 0; m_nodes = Arrays.InitializeWithDefaultInstances <b2TreeNode>(m_nodeCapacity); // Build a linked list for the free list. for (int i = 0; i < m_nodeCapacity - 1; ++i) { m_nodes[i] = new b2TreeNode(); m_nodes[i].parentOrNext = i + 1; m_nodes[i].height = -1; } m_nodes[m_nodeCapacity - 1] = new b2TreeNode(); m_nodes[m_nodeCapacity - 1].parentOrNext = Settings.b2_nullNode; m_nodes[m_nodeCapacity - 1].height = -1; m_freeList = 0; //m_path = 0; m_insertionCount = 0; }
private void ValidateStructure(int index) { if (index == Settings.b2_nullNode) { return; } if (index == m_root) { Debug.Assert(m_nodes[index].parentOrNext == Settings.b2_nullNode); } b2TreeNode node = m_nodes[index]; int child1 = node.child1; int child2 = node.child2; if (node.IsLeaf()) { Debug.Assert(child1 == Settings.b2_nullNode); Debug.Assert(child2 == Settings.b2_nullNode); Debug.Assert(node.height == 0); return; } Debug.Assert(0 <= child1 && child1 < m_nodeCapacity); Debug.Assert(0 <= child2 && child2 < m_nodeCapacity); Debug.Assert(m_nodes[child1].parentOrNext == index); Debug.Assert(m_nodes[child2].parentOrNext == index); ValidateStructure(child1); ValidateStructure(child2); }
/// Get the ratio of the sum of the node areas to the root area. // public float GetAreaRatio() { if (m_root == Settings.b2_nullNode) { return(0.0f); } b2TreeNode root = m_nodes[m_root]; float rootArea = root.aabb.GetPerimeter(); float totalArea = 0.0f; for (int i = 0; i < m_nodeCapacity; ++i) { b2TreeNode node = m_nodes[i]; if (node.height < 0) { // Free node in pool continue; } totalArea += node.aabb.GetPerimeter(); } return(totalArea / rootArea); }
/// Query an AABB for overlapping proxies. The callback class /// is called for each proxy that overlaps the supplied AABB. public void Query(b2BroadphaseQueryCallback callback, b2AABB aabb) { Stack <int> stack = new Stack <int>(256); stack.Push(m_root); while (stack.Count > 0) { int nodeId = stack.Pop(); if (nodeId == Settings.b2_nullNode) { continue; } b2TreeNode node = m_nodes[nodeId]; if (Utils.b2TestOverlap(ref node.aabb, ref aabb)) { if (node.IsLeaf()) { bool proceed = callback(nodeId); if (proceed == false) { return; } } else { stack.Push(node.child1); stack.Push(node.child2); } } } }
// Compute the height of a sub-tree. private int ComputeHeight(int nodeId) { Debug.Assert(0 <= nodeId && nodeId < m_nodeCapacity); b2TreeNode node = m_nodes[nodeId]; if (node.IsLeaf()) { return(0); } int height1 = ComputeHeight(node.child1); int height2 = ComputeHeight(node.child2); return(1 + Utils.b2Max(height1, height2)); }
private void ValidateMetrics(int index) { if (index == Settings.b2_nullNode) { return; } b2TreeNode node = m_nodes[index]; int child1 = node.child1; int child2 = node.child2; if (node.IsLeaf()) { Debug.Assert(child1 == Settings.b2_nullNode); Debug.Assert(child2 == Settings.b2_nullNode); Debug.Assert(node.height == 0); return; } Debug.Assert(0 <= child1 && child1 < m_nodeCapacity); Debug.Assert(0 <= child2 && child2 < m_nodeCapacity); int height1 = m_nodes[child1].height; int height2 = m_nodes[child2].height; int height; height = 1 + Utils.b2Max(height1, height2); Debug.Assert(node.height == height); b2AABB aabb = new b2AABB(); aabb.Combine(ref m_nodes[child1].aabb, ref m_nodes[child2].aabb); Debug.Assert(aabb.lowerBound == node.aabb.lowerBound); Debug.Assert(aabb.upperBound == node.aabb.upperBound); ValidateMetrics(child1); ValidateMetrics(child2); }
// Allocate a node from the pool. Grow the pool if necessary. private int AllocateNode() { // Expand the node pool as needed. if (m_freeList == Settings.b2_nullNode) { Debug.Assert(m_nodeCount == m_nodeCapacity); // The free list is empty. Rebuild a bigger pool. b2TreeNode[] oldNodes = m_nodes; m_nodeCapacity *= 2; m_nodes = Arrays.InitializeWithDefaultInstances <b2TreeNode>(m_nodeCapacity); Array.Copy(oldNodes, m_nodes, m_nodeCount); // Build a linked list for the free list. The parentOrNext // pointer becomes the "parentOrNext" pointer. for (int i = m_nodeCount; i < m_nodeCapacity - 1; ++i) { m_nodes[i] = new b2TreeNode(); m_nodes[i].parentOrNext = i + 1; m_nodes[i].height = -1; } m_nodes[m_nodeCapacity - 1] = new b2TreeNode(); m_nodes[m_nodeCapacity - 1].parentOrNext = Settings.b2_nullNode; m_nodes[m_nodeCapacity - 1].height = -1; m_freeList = m_nodeCount; } // Peel a node off the free list. int nodeId = m_freeList; m_freeList = m_nodes[nodeId].parentOrNext; m_nodes[nodeId].parentOrNext = Settings.b2_nullNode; m_nodes[nodeId].child1 = Settings.b2_nullNode; m_nodes[nodeId].child2 = Settings.b2_nullNode; m_nodes[nodeId].height = 0; m_nodes[nodeId].userData = null; ++m_nodeCount; return(nodeId); }
/// Get the maximum balance of an node in the tree. The balance is the difference /// in height of the two children of a node. public int GetMaxBalance() { int maxBalance = 0; for (int i = 0; i < m_nodeCapacity; ++i) { b2TreeNode node = m_nodes[i]; if (node.height <= 1) { continue; } Debug.Assert(node.IsLeaf() == false); int child1 = node.child1; int child2 = node.child2; int balance = Utils.b2Abs(m_nodes[child2].height - m_nodes[child1].height); maxBalance = Utils.b2Max(maxBalance, balance); } return(maxBalance); }
// Perform a left or right rotation if node A is imbalanced. // Returns the new root index. private int Balance(int iA) { Debug.Assert(iA != Settings.b2_nullNode); b2TreeNode A = m_nodes[iA]; if (A.IsLeaf() || A.height < 2) { return(iA); } int iB = A.child1; int iC = A.child2; Debug.Assert(0 <= iB && iB < m_nodeCapacity); Debug.Assert(0 <= iC && iC < m_nodeCapacity); b2TreeNode B = m_nodes[iB]; b2TreeNode C = m_nodes[iC]; int balance = C.height - B.height; // Rotate C up if (balance > 1) { int iF = C.child1; int iG = C.child2; b2TreeNode F = m_nodes[iF]; b2TreeNode G = m_nodes[iG]; Debug.Assert(0 <= iF && iF < m_nodeCapacity); Debug.Assert(0 <= iG && iG < m_nodeCapacity); // Swap A and C C.child1 = iA; C.parentOrNext = A.parentOrNext; A.parentOrNext = iC; // A's old parentOrNext should point to C if (C.parentOrNext != Settings.b2_nullNode) { if (m_nodes[C.parentOrNext].child1 == iA) { m_nodes[C.parentOrNext].child1 = iC; } else { Debug.Assert(m_nodes[C.parentOrNext].child2 == iA); m_nodes[C.parentOrNext].child2 = iC; } } else { m_root = iC; } // Rotate if (F.height > G.height) { C.child2 = iF; A.child2 = iG; G.parentOrNext = iA; A.aabb.Combine(ref B.aabb, ref G.aabb); C.aabb.Combine(ref A.aabb, ref F.aabb); A.height = 1 + Utils.b2Max(B.height, G.height); C.height = 1 + Utils.b2Max(A.height, F.height); } else { C.child2 = iG; A.child2 = iF; F.parentOrNext = iA; A.aabb.Combine(ref B.aabb, ref F.aabb); C.aabb.Combine(ref A.aabb, ref G.aabb); A.height = 1 + Utils.b2Max(B.height, F.height); C.height = 1 + Utils.b2Max(A.height, G.height); } return(iC); } // Rotate B up if (balance < -1) { int iD = B.child1; int iE = B.child2; b2TreeNode D = m_nodes[iD]; b2TreeNode E = m_nodes[iE]; Debug.Assert(0 <= iD && iD < m_nodeCapacity); Debug.Assert(0 <= iE && iE < m_nodeCapacity); // Swap A and B B.child1 = iA; B.parentOrNext = A.parentOrNext; A.parentOrNext = iB; // A's old parentOrNext should point to B if (B.parentOrNext != Settings.b2_nullNode) { if (m_nodes[B.parentOrNext].child1 == iA) { m_nodes[B.parentOrNext].child1 = iB; } else { Debug.Assert(m_nodes[B.parentOrNext].child2 == iA); m_nodes[B.parentOrNext].child2 = iB; } } else { m_root = iB; } // Rotate if (D.height > E.height) { B.child2 = iD; A.child1 = iE; E.parentOrNext = iA; A.aabb.Combine(ref C.aabb, ref E.aabb); B.aabb.Combine(ref A.aabb, ref D.aabb); A.height = 1 + Utils.b2Max(C.height, E.height); B.height = 1 + Utils.b2Max(A.height, D.height); } else { B.child2 = iE; A.child1 = iD; D.parentOrNext = iA; A.aabb.Combine(ref C.aabb, ref D.aabb); B.aabb.Combine(ref A.aabb, ref E.aabb); A.height = 1 + Utils.b2Max(C.height, D.height); B.height = 1 + Utils.b2Max(A.height, E.height); } return(iB); } return(iA); }
/// 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; } if (m_nodes[i].IsLeaf()) { m_nodes[i].parentOrNext = Settings.b2_nullNode; nodes[count] = i; ++count; } else { FreeNode(i); } } while (count > 1) { float minCost = float.MaxValue; int iMin = -1; int jMin = -1; for (int i = 0; i < count; ++i) { b2AABB aabbi = m_nodes[nodes[i]].aabb; for (int j = i + 1; j < count; ++j) { b2AABB aabbj = m_nodes[nodes[j]].aabb; b2AABB b = new b2AABB(); b.Combine(ref aabbi, ref aabbj); float cost = b.GetPerimeter(); if (cost < minCost) { iMin = i; jMin = j; minCost = cost; } } } int index1 = nodes[iMin]; int index2 = nodes[jMin]; b2TreeNode child1 = m_nodes[index1]; b2TreeNode child2 = m_nodes[index2]; int parentIndex = AllocateNode(); b2TreeNode parentOrNext = m_nodes[parentIndex]; parentOrNext.child1 = index1; parentOrNext.child2 = index2; parentOrNext.height = 1 + Utils.b2Max(child1.height, child2.height); parentOrNext.aabb.Combine(ref child1.aabb, ref child2.aabb); parentOrNext.parentOrNext = Settings.b2_nullNode; child1.parentOrNext = parentIndex; child2.parentOrNext = parentIndex; nodes[jMin] = nodes[count - 1]; nodes[iMin] = parentIndex; --count; } m_root = nodes[0]; nodes = null; Validate(); }
/// Ray-cast against the proxies in the tree. This relies on the callback /// to perform a exact ray-cast in the case were the proxy contains a shape. /// The callback also performs the any collision filtering. This has performance /// roughly equal to k * log(n), where k is the number of collisions and n is the /// number of proxies in the tree. /// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). /// @param callback a callback class that is called for each proxy that is hit by the ray. public void RayCast(b2BroadphaseRayCastCallback callback, b2RayCastInput input) { b2Vec2 p1 = new b2Vec2(input.p1); b2Vec2 p2 = new b2Vec2(input.p2); b2Vec2 r = p2 - p1; Debug.Assert(r.LengthSquared() > 0.0f); r.Normalize(); // v is perpendicular to the segment. b2Vec2 v = Utils.b2Cross(1.0f, r); b2Vec2 abs_v = Utils.b2Abs(v); // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) float maxFraction = input.maxFraction; // Build a bounding box for the segment. b2AABB segmentAABB = new b2AABB(); { b2Vec2 t = p1 + maxFraction * (p2 - p1); segmentAABB.lowerBound = Utils.b2Min(p1, t); segmentAABB.upperBound = Utils.b2Max(p1, t); } Stack <int> stack = new Stack <int>(256); stack.Push(m_root); while (stack.Count > 0) { int nodeId = stack.Pop(); if (nodeId == Settings.b2_nullNode) { continue; } b2TreeNode node = m_nodes[nodeId]; if (Utils.b2TestOverlap(ref node.aabb, ref segmentAABB) == false) { continue; } // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) b2Vec2 c = node.aabb.GetCenter(); b2Vec2 h = node.aabb.GetExtents(); float separation = Utils.b2Abs(Utils.b2Dot(v, p1 - c)) - Utils.b2Dot(abs_v, h); if (separation > 0.0f) { continue; } if (node.IsLeaf()) { b2RayCastInput subInput = new b2RayCastInput(); subInput.p1 = input.p1; subInput.p2 = input.p2; subInput.maxFraction = maxFraction; float value = callback(subInput, nodeId); if (value == 0.0f) { // The client has terminated the ray cast. return; } if (value > 0.0f) { // Update segment bounding box. maxFraction = value; b2Vec2 t = p1 + maxFraction * (p2 - p1); segmentAABB.lowerBound = Utils.b2Min(p1, t); segmentAABB.upperBound = Utils.b2Max(p1, t); } } else { stack.Push(node.child1); stack.Push(node.child2); } } }
internal static global::System.Runtime.InteropServices.HandleRef getCPtr(b2TreeNode obj) { return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr); }