Box2D.Collision.Broadphase.DynamicTree.InsertLeaf C# (CSharp) Метод

        private void InsertLeaf(int leaf)
        {
            InsertionCount++;

            if (m_root == TreeNode.NULL_NODE)
            {
                m_root = leaf;
                m_nodes[m_root].Parent = TreeNode.NULL_NODE;
                return;
            }

            // find the best sibling
            AABB leafAABB = m_nodes[leaf].AABB;
            int index = m_root;
            while (m_nodes[index].Leaf == false)
            {
                TreeNode node = m_nodes[index];
                int child1 = node.Child1;
                int child2 = node.Child2;

                float area = node.AABB.Perimeter;

                combinedAABB.Combine(node.AABB, leafAABB);
                float combinedArea = combinedAABB.Perimeter;

                // 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].Leaf)
                {
                    combinedAABB.Combine(leafAABB, m_nodes[child1].AABB);
                    cost1 = combinedAABB.Perimeter + inheritanceCost;
                }
                else
                {
                    combinedAABB.Combine(leafAABB, m_nodes[child1].AABB);
                    float oldArea = m_nodes[child1].AABB.Perimeter;
                    float newArea = combinedAABB.Perimeter;
                    cost1 = (newArea - oldArea) + inheritanceCost;
                }

                // Cost of descending into child2
                float cost2;
                if (m_nodes[child2].Leaf)
                {
                    combinedAABB.Combine(leafAABB, m_nodes[child2].AABB);
                    cost2 = combinedAABB.Perimeter + inheritanceCost;
                }
                else
                {
                    combinedAABB.Combine(leafAABB, m_nodes[child2].AABB);
                    float oldArea = m_nodes[child2].AABB.Perimeter;
                    float newArea = combinedAABB.Perimeter;
                    cost2 = newArea - oldArea + inheritanceCost;
                }

                // Descend according to the minimum cost.
                if (cost < cost1 && cost < cost2)
                {
                    break;
                }

                // Descend
                index = cost1 < cost2 ? child1 : child2;
            }

            int sibling = index;
            int oldParent = m_nodes[sibling].Parent;
            int newParentId = AllocateNode();
            TreeNode newParent = m_nodes[newParentId];
            newParent.Parent = oldParent;
            newParent.UserData = null;
            newParent.AABB.Combine(leafAABB, m_nodes[sibling].AABB);
            newParent.Height = m_nodes[sibling].Height + 1;

            if (oldParent != TreeNode.NULL_NODE)
            {
                // The sibling was not the root.
                if (m_nodes[oldParent].Child1 == sibling)
                {
                    m_nodes[oldParent].Child1 = newParentId;
                }
                else
                {
                    m_nodes[oldParent].Child2 = newParentId;
                }

                m_nodes[newParentId].Child1 = sibling;
                m_nodes[newParentId].Child2 = leaf;
                m_nodes[sibling].Parent = newParentId;
                m_nodes[leaf].Parent = newParentId;
            }
            else
            {
                // The sibling was the root.
                m_nodes[newParentId].Child1 = sibling;
                m_nodes[newParentId].Child2 = leaf;
                m_nodes[sibling].Parent = newParentId;
                m_nodes[leaf].Parent = newParentId;
                m_root = newParentId;
            }

            // Walk back up the tree fixing heights and AABBs
            index = m_nodes[leaf].Parent;
            while (index != TreeNode.NULL_NODE)
            {
                index = Balance(index);

                int child1 = m_nodes[index].Child1;
                int child2 = m_nodes[index].Child2;

                Debug.Assert(child1 != TreeNode.NULL_NODE);
                Debug.Assert(child2 != TreeNode.NULL_NODE);

                m_nodes[index].Height = 1 + MathUtils.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();
        }