public void RemoveProxy(DynamicTreeNode node) { Debug.Assert(node.IsLeaf); RemoveLeaf(node); FreeNode(node); }
void ValidateStructure(DynamicTreeNode node) { if (node == null) { return; } if (node == root) { Debug.Assert(node.Parent == null); } var leftChild = node.LeftChild; var rightChild = node.RightChild; if (node.IsLeaf) { Debug.Assert(leftChild == null); Debug.Assert(rightChild == null); Debug.Assert(node.Depth == 0); return; } Debug.Assert(leftChild.Parent == node); Debug.Assert(rightChild.Parent == node); ValidateStructure(leftChild); ValidateStructure(rightChild); }
void ValidateMetrics(DynamicTreeNode node) { if (node == null) { return; } var leftChild = node.LeftChild; var rightChild = node.RightChild; if (node.IsLeaf) { Debug.Assert(leftChild == null); Debug.Assert(rightChild == null); Debug.Assert(node.Depth == 0); return; } int depth1 = leftChild.Depth; int depth2 = rightChild.Depth; int depth = 1 + Math.Max(depth1, depth2); Debug.Assert(node.Depth == depth); var aabb = leftChild.AABB.Combine(rightChild.AABB); Debug.Assert(aabb.LowerBound == node.AABB.LowerBound); Debug.Assert(aabb.UpperBound == node.AABB.UpperBound); ValidateMetrics(leftChild); ValidateMetrics(rightChild); }
private DynamicTreeNode FindSibling(DynamicTreeNode leaf) { var sibling = root; while (!sibling.IsLeaf) { var area = sibling.AABB.Perimeter; var combinedAABB = leaf.AABB.Combine(sibling.AABB); var combinedArea = combinedAABB.Perimeter; // Cost of creating a new parent for this node and the new leaf var cost = 2.0f * combinedArea; // Minimum cost of pushing the leaf further down the tree var inheritanceCost = 2.0f * (combinedArea - area); // Cost of descending into child1 var cost1 = CalculateSiblingChildCost(leaf, sibling.LeftChild, inheritanceCost); var cost2 = CalculateSiblingChildCost(leaf, sibling.RightChild, inheritanceCost); // Descend according to the minimum cost. if (cost < cost1 && cost1 < cost2) { return(sibling); } // Descend sibling = cost1 < cost2 ? sibling.LeftChild : sibling.RightChild; } return(sibling); }
private DynamicTreeNode Balance(DynamicTreeNode node) { Debug.Assert(node != null); if (node.IsLeaf || node.Depth < 2) { return(node); } var nodeB = node.LeftChild; var nodeC = node.RightChild; int balance = nodeC.Depth - nodeB.Depth; // Rotate C up if (balance > 1) { return(Rotate(node, false)); } // Rotate B up if (balance < -1) { return(Rotate(node, true)); } return(node); }
private void FreeNode(DynamicTreeNode node) { Debug.Assert(0 < count); node.Free(); freeNodes.Enqueue(node); --count; }
internal void Free() { AABB = default(AABB); Context = null; Depth = -1; Parent = LeftChild = RightChild = null; }
public int ComputeDepth(DynamicTreeNode node) { if (node.IsLeaf) { return(0); } int height1 = ComputeDepth(node.LeftChild); int height2 = ComputeDepth(node.RightChild); return(1 + Math.Max(height1, height2)); }
private void InsertLeaf(DynamicTreeNode leaf) { if (root == null) { root = leaf; root.Parent = null; return; } // Find the best sibling for this node var sibling = FindSibling(leaf); // Create a new parent. var oldParent = sibling.Parent; var newParent = AllocateNode(); newParent.Initialize(leaf.AABB.Combine(sibling.AABB), null, sibling.Depth + 1); newParent.Parent = oldParent; if (oldParent != null) { // The sibling was not the root. if (oldParent.LeftChild == sibling) { oldParent.LeftChild = newParent; } else { oldParent.RightChild = newParent; } newParent.LeftChild = sibling; newParent.RightChild = leaf; sibling.Parent = newParent; leaf.Parent = newParent; } else { // The sibling was the root. newParent.LeftChild = sibling; newParent.RightChild = leaf; sibling.Parent = newParent; leaf.Parent = newParent; root = newParent; } // Walk back up the tree fixing depths and AABBs BalanceBranch(leaf.Parent); #if DEBUG Validate(); #endif }
private float CalculateSiblingChildCost(DynamicTreeNode leaf, DynamicTreeNode child, float inheritanceCost) { var aabb = leaf.AABB.Combine(child.AABB); if (child.IsLeaf) { return(aabb.Perimeter + inheritanceCost); } else { return((aabb.Perimeter - child.AABB.Perimeter) + inheritanceCost); } }
private void BalanceBranch(DynamicTreeNode node) { while (node != null) { node = Balance(node); Debug.Assert(node.LeftChild != null); Debug.Assert(node.RightChild != null); node.Depth = 1 + Math.Max(node.LeftChild.Depth, node.RightChild.Depth); node.AABB = node.LeftChild.AABB.Combine(node.RightChild.AABB); node = node.Parent; } }
private DynamicTreeNode AllocateNode() { DynamicTreeNode node; if (freeNodes.Count > 0) { node = freeNodes.Dequeue(); } else { node = new DynamicTreeNode(); nodes.Add(node); } ++count; return(node); }
private void RemoveLeaf(DynamicTreeNode leaf) { Debug.Assert(leaf.IsLeaf); if (leaf == root) { root = null; return; } var parent = leaf.Parent; var grandParent = parent.Parent; var sibling = parent.LeftChild == leaf ? parent.RightChild : parent.LeftChild; if (grandParent != null) { // Destroy parent and connect sibling to grandParent. if (grandParent.LeftChild == parent) { grandParent.LeftChild = sibling; } else { grandParent.RightChild = sibling; } sibling.Parent = grandParent; FreeNode(parent); // Adjust ancestor bounds. BalanceBranch(grandParent); } else { root = sibling; sibling.Parent = null; FreeNode(parent); } #if DEBUG Validate(); #endif }
public bool MoveProxy(DynamicTreeNode node, AABB aabb, Vector2D displacement) { Debug.Assert(node.IsLeaf); if (node.AABB.Contains(aabb)) { return(false); } RemoveLeaf(node); // Extend AABB. aabb = aabb.Enlarge(new Vector2D(Configuration.FatAABBExtension, Configuration.FatAABBExtension)); // Predict AABB displacement. displacement = Configuration.AABBDisplacementMultiplier * displacement; if (displacement.X < 0.0f) { aabb.LowerBound.X += displacement.X; } else { aabb.UpperBound.X += displacement.X; } if (displacement.Y < 0.0f) { aabb.LowerBound.Y += displacement.Y; } else { aabb.UpperBound.Y += displacement.Y; } node.AABB = aabb; InsertLeaf(node); return(true); }
private DynamicTreeNode Rotate(DynamicTreeNode parent, bool leftChild) { var child1 = leftChild ? parent.LeftChild : parent.RightChild; var child2 = leftChild ? parent.RightChild : parent.LeftChild; var grandChild1 = child1.LeftChild; var grandChild2 = child1.RightChild; // Swap A and C child1.LeftChild = parent; child1.Parent = parent.Parent; parent.Parent = child1; // A's old parent should point to C if (child1.Parent != null) { if (child1.Parent.LeftChild == parent) { child1.Parent.LeftChild = child1; } else { Debug.Assert(child1.Parent.RightChild == parent); child1.Parent.RightChild = child1; } } else { root = child1; } // Rotate if (grandChild1.Depth > grandChild2.Depth) { child1.RightChild = grandChild1; if (leftChild) { parent.LeftChild = grandChild2; } else { parent.RightChild = grandChild2; } grandChild2.Parent = parent; parent.AABB = child2.AABB.Combine(grandChild2.AABB); child1.AABB = parent.AABB.Combine(grandChild1.AABB); parent.Depth = 1 + Math.Max(child2.Depth, grandChild2.Depth); child1.Depth = 1 + Math.Max(parent.Depth, grandChild1.Depth); } else { child1.RightChild = grandChild2; if (leftChild) { parent.LeftChild = grandChild1; } else { parent.RightChild = grandChild1; } grandChild1.Parent = parent; parent.AABB = child2.AABB.Combine(grandChild1.AABB); child1.AABB = parent.AABB.Combine(grandChild2.AABB); parent.Depth = 1 + Math.Max(child2.Depth, grandChild1.Depth); child1.Depth = 1 + Math.Max(parent.Depth, grandChild2.Depth); } return(child1); }