private void RemoveLeaf(int leaf) { if (leaf == m_root) { m_root = Null; return; } int parent = m_nodes[leaf].Parent; int grandParent = m_nodes[parent].Parent; int sibling = m_nodes[parent].ChildA == leaf ? m_nodes[parent].ChildB : m_nodes[parent].ChildA; if (grandParent != Null) { // destroy parent and connect sibling to grand parent if (m_nodes[grandParent].ChildA == parent) { m_nodes[grandParent].ChildA = sibling; } else { m_nodes[grandParent].ChildB = sibling; } m_nodes[sibling].Parent = grandParent; FreeNode(parent); // adjust ancestor bounds int index = grandParent; while (index != Null) { index = Balance(index); int childA = m_nodes[index].ChildA; int childB = m_nodes[index].ChildB; m_nodes[index].Bounds = Aabb.Union(m_nodes[childA].Bounds, m_nodes[childB].Bounds); m_nodes[index].Height = 1 + Mathf.Max(m_nodes[childA].Height, m_nodes[childB].Height); index = m_nodes[index].Parent; } } else { m_root = sibling; m_nodes[sibling].Parent = Null; FreeNode(parent); } }
private int Balance(int a) { if (m_nodes[a].IsLeaf || m_nodes[a].Height < 2) { return(a); } int b = m_nodes[a].ChildA; int c = m_nodes[a].ChildB; int balance = m_nodes[c].Height - m_nodes[b].Height; // rotate C up if (balance > 1) { int f = m_nodes[c].ChildA; int g = m_nodes[c].ChildB; // swap A and C m_nodes[c].ChildA = a; m_nodes[c].Parent = m_nodes[a].Parent; m_nodes[a].Parent = c; // A's old parent should point to C if (m_nodes[c].Parent != Null) { if (m_nodes[m_nodes[c].Parent].ChildA == a) { m_nodes[m_nodes[c].Parent].ChildA = c; } else { m_nodes[m_nodes[c].Parent].ChildB = c; } } else { m_root = c; } // rotate if (m_nodes[f].Height > m_nodes[g].Height) { m_nodes[c].ChildB = f; m_nodes[a].ChildB = g; m_nodes[g].Parent = a; m_nodes[a].Bounds = Aabb.Union(m_nodes[b].Bounds, m_nodes[g].Bounds); m_nodes[c].Bounds = Aabb.Union(m_nodes[a].Bounds, m_nodes[f].Bounds); m_nodes[a].Height = 1 + Mathf.Max(m_nodes[b].Height, m_nodes[g].Height); m_nodes[c].Height = 1 + Mathf.Max(m_nodes[a].Height, m_nodes[f].Height); } else { m_nodes[c].ChildB = g; m_nodes[a].ChildB = f; m_nodes[f].Parent = a; m_nodes[a].Bounds = Aabb.Union(m_nodes[b].Bounds, m_nodes[f].Bounds); m_nodes[c].Bounds = Aabb.Union(m_nodes[a].Bounds, m_nodes[g].Bounds); m_nodes[a].Height = 1 + Mathf.Max(m_nodes[b].Height, m_nodes[f].Height); m_nodes[c].Height = 1 + Mathf.Max(m_nodes[a].Height, m_nodes[g].Height); } return(c); } // rotate B up if (balance < -1) { int d = m_nodes[b].ChildA; int e = m_nodes[b].ChildB; // swap A and B m_nodes[b].ChildA = a; m_nodes[b].Parent = m_nodes[a].Parent; m_nodes[a].Parent = b; // A's old parent should point to B if (m_nodes[b].Parent != Null) { if (m_nodes[m_nodes[b].Parent].ChildA == a) { m_nodes[m_nodes[b].Parent].ChildA = b; } else { m_nodes[m_nodes[b].Parent].ChildB = b; } } else { m_root = b; } // rotate if (m_nodes[d].Height > m_nodes[e].Height) { m_nodes[b].ChildB = d; m_nodes[a].ChildA = e; m_nodes[e].Parent = a; m_nodes[a].Bounds = Aabb.Union(m_nodes[c].Bounds, m_nodes[e].Bounds); m_nodes[b].Bounds = Aabb.Union(m_nodes[a].Bounds, m_nodes[d].Bounds); m_nodes[a].Height = 1 + Mathf.Max(m_nodes[c].Height, m_nodes[e].Height); m_nodes[b].Height = 1 + Mathf.Max(m_nodes[a].Height, m_nodes[d].Height); } else { m_nodes[b].ChildB = e; m_nodes[a].ChildA = d; m_nodes[d].Parent = a; m_nodes[a].Bounds = Aabb.Union(m_nodes[c].Bounds, m_nodes[d].Bounds); m_nodes[b].Bounds = Aabb.Union(m_nodes[a].Bounds, m_nodes[e].Bounds); m_nodes[a].Height = 1 + Mathf.Max(m_nodes[c].Height, m_nodes[d].Height); m_nodes[b].Height = 1 + Mathf.Max(m_nodes[a].Height, m_nodes[e].Height); } return(b); } return(a); }
private void InsertLeaf(int leaf) { if (m_root == Null) { m_root = leaf; m_nodes[m_root].Parent = Null; return; } // find best sibling Aabb leafBounds = m_nodes[leaf].Bounds; int index = m_root; while (!m_nodes[index].IsLeaf) { int childA = m_nodes[index].ChildA; int childB = m_nodes[index].ChildB; float area = m_nodes[index].Bounds.HalfArea; Aabb combinedBounds = Aabb.Union(m_nodes[index].Bounds, leafBounds); float combinedArea = combinedBounds.HalfArea; // 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 child A float costA; if (m_nodes[childA].IsLeaf) { Aabb bounds; bounds = Aabb.Union(leafBounds, m_nodes[childA].Bounds); costA = bounds.HalfArea + inheritanceCost; } else { Aabb bounds; bounds = Aabb.Union(leafBounds, m_nodes[childA].Bounds); float oldArea = m_nodes[childA].Bounds.HalfArea; float newArea = bounds.HalfArea; costA = (newArea - oldArea) + inheritanceCost; } // cost of descending into child B float costB; if (m_nodes[childB].IsLeaf) { Aabb bounds; bounds = Aabb.Union(leafBounds, m_nodes[childB].Bounds); costB = bounds.HalfArea + inheritanceCost; } else { Aabb bounds; bounds = Aabb.Union(leafBounds, m_nodes[childB].Bounds); float oldArea = m_nodes[childB].Bounds.HalfArea; float newArea = bounds.HalfArea; costB = (newArea - oldArea) + inheritanceCost; } // descend according to the minimum cost if (cost < costA && cost < costB) { break; } //descend index = (costA < costB) ? childA : childB; } int sibling = index; // create a new parent int oldParent = m_nodes[sibling].Parent; int newParent = AllocateNode(); m_nodes[newParent].Parent = oldParent; m_nodes[newParent].Bounds = Aabb.Union(leafBounds, m_nodes[sibling].Bounds); m_nodes[newParent].Height = m_nodes[sibling].Height + 1; if (oldParent != Null) { // sibling was not the root if (m_nodes[oldParent].ChildA == sibling) { m_nodes[oldParent].ChildA = newParent; } else { m_nodes[oldParent].ChildB = newParent; } m_nodes[newParent].ChildA = sibling; m_nodes[newParent].ChildB = leaf; m_nodes[sibling].Parent = newParent; m_nodes[leaf].Parent = newParent; } else { // sibling was the root m_nodes[newParent].ChildA = sibling; m_nodes[newParent].ChildB = leaf; m_nodes[sibling].Parent = newParent; m_nodes[leaf].Parent = newParent; m_root = newParent; } // walk back up to re-balance heights index = m_nodes[leaf].Parent; while (index != Null) { index = Balance(index); int childA = m_nodes[index].ChildA; int childB = m_nodes[index].ChildB; m_nodes[index].Height = 1 + Mathf.Max(m_nodes[childA].Height, m_nodes[childB].Height); m_nodes[index].Bounds = Aabb.Union(m_nodes[childA].Bounds, m_nodes[childB].Bounds); index = m_nodes[index].Parent; } }