private void InsertLeaf(int leaf) { ++m_insertionCount; if (m_root == TreeNode._nullNode) { m_root = leaf; m_nodes[m_root].parent = TreeNode._nullNode; return; } // Find the best sibling for this node AABB leafAABB = m_nodes[leaf].aabb; int index = m_root; while (m_nodes[index].IsLeaf() == false) { int child1 = m_nodes[index].child1; int child2 = m_nodes[index].child2; float area = m_nodes[index].aabb.GetPerimeter(); AABB combinedAABB = new AABB(); combinedAABB.Combine(m_nodes[index].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 (m_nodes[child1].IsLeaf()) { AABB aabb = new AABB(); aabb.Combine(leafAABB, m_nodes[child1].aabb); cost1 = aabb.GetPerimeter() + inheritanceCost; } else { AABB aabb = new AABB(); aabb.Combine(leafAABB, m_nodes[child1].aabb); float oldArea = m_nodes[child1].aabb.GetPerimeter(); float newArea = aabb.GetPerimeter(); cost1 = (newArea - oldArea) + inheritanceCost; } // Cost of descending into child2 float cost2; if (m_nodes[child2].IsLeaf()) { AABB aabb = new AABB(); aabb.Combine(leafAABB, m_nodes[child2].aabb); cost2 = aabb.GetPerimeter() + inheritanceCost; } else { AABB aabb = new AABB(); aabb.Combine(leafAABB, m_nodes[child2].aabb); float oldArea = m_nodes[child2].aabb.GetPerimeter(); float newArea = aabb.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; } } int sibling = index; // Create a new parent. int oldParent = m_nodes[sibling].parent; int newParent = AllocateNode(); m_nodes[newParent].parent = oldParent; m_nodes[newParent].userData = null; m_nodes[newParent].aabb.Combine(leafAABB, m_nodes[sibling].aabb); m_nodes[newParent].height = m_nodes[sibling].height + 1; if (oldParent != TreeNode._nullNode) { // The sibling was not the root. if (m_nodes[oldParent].child1 == sibling) { m_nodes[oldParent].child1 = newParent; } else { m_nodes[oldParent].child2 = newParent; } m_nodes[newParent].child1 = sibling; m_nodes[newParent].child2 = leaf; m_nodes[sibling].parent = newParent; m_nodes[leaf].parent = newParent; } else { // The sibling was the root. m_nodes[newParent].child1 = sibling; m_nodes[newParent].child2 = leaf; m_nodes[sibling].parent = newParent; m_nodes[leaf].parent = newParent; m_root = newParent; } // Walk back up the tree fixing heights and AABBs index = m_nodes[leaf].parent; while (index != TreeNode._nullNode) { index = Balance(index); int child1 = m_nodes[index].child1; int child2 = m_nodes[index].child2; Utilities.Assert(child1 != TreeNode._nullNode); Utilities.Assert(child2 != TreeNode._nullNode); m_nodes[index].height = 1 + Math.Max(m_nodes[child1].height, m_nodes[child2].height); m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); index = m_nodes[index].parent; } //Validate(); }
private void InsertLeaf(int leaf) { ++_insertionCount; if (_root == NullNode) { _root = leaf; _nodes[_root].parentOrNext = NullNode; return; } // Find the best sibling for this node AABB leafAABB = _nodes[leaf].aabb; Vector2 leafCenter = leafAABB.GetCenter(); int sibling = _root; while (_nodes[sibling].IsLeaf() == false) { // Expand the node's AABB. _nodes[sibling].aabb.Combine(ref leafAABB); _nodes[sibling].leafCount += 1; int child1 = _nodes[sibling].child1; int child2 = _nodes[sibling].child2; #if false // This seems to create imbalanced trees Vector2 delta1 = Math.Abs(_nodes[child1].aabb.GetCenter() - leafCenter); Vector2 delta2 = Math.Abs(_nodes[child2].aabb.GetCenter() - leafCenter); float norm1 = delta1.x + delta1.y; float norm2 = delta2.x + delta2.y; #else // Surface area heuristic AABB aabb1 = new AABB(); AABB aabb2 = new AABB(); aabb1.Combine(ref leafAABB, ref _nodes[child1].aabb); aabb2.Combine(ref leafAABB, ref _nodes[child2].aabb); float norm1 = (_nodes[child1].leafCount + 1) * aabb1.GetPerimeter(); float norm2 = (_nodes[child2].leafCount + 1) * aabb2.GetPerimeter(); #endif if (norm1 < norm2) { sibling = child1; } else { sibling = child2; } } // Create a new parent for the siblings. int oldParent = _nodes[sibling].parentOrNext; int newParent = AllocateNode(); _nodes[newParent].parentOrNext = oldParent; _nodes[newParent].userData = null; _nodes[newParent].aabb.Combine(ref leafAABB, ref _nodes[sibling].aabb); _nodes[newParent].leafCount = _nodes[sibling].leafCount + 1; if (oldParent != NullNode) { // The sibling was not the root. if (_nodes[oldParent].child1 == sibling) { _nodes[oldParent].child1 = newParent; } else { _nodes[oldParent].child2 = newParent; } _nodes[newParent].child1 = sibling; _nodes[newParent].child2 = leaf; _nodes[sibling].parentOrNext = newParent; _nodes[leaf].parentOrNext = newParent; } else { // The sibling was the root. _nodes[newParent].child1 = sibling; _nodes[newParent].child2 = leaf; _nodes[sibling].parentOrNext = newParent; _nodes[leaf].parentOrNext = newParent; _root = newParent; } }