private void RemoveLeaf(int leaf)
        {
            if (_root == NullNode)
            {
                return;
            }

            if (leaf == _root)
            {
                _root = NullNode;
                return;
            }

            int parent      = _nodes[leaf].ParentOrNext;
            int grandParent = _nodes[parent].ParentOrNext;
            int sibling;

            if (_nodes[parent].Child1 == leaf)
            {
                sibling = _nodes[parent].Child2;
            }
            else
            {
                sibling = _nodes[parent].Child1;
            }

            if (grandParent != NullNode)
            {
                // Destroy node2 and connect node1 to sibling.
                if (_nodes[grandParent].Child1 == parent)
                {
                    _nodes[grandParent].Child1 = sibling;
                }
                else
                {
                    _nodes[grandParent].Child2 = sibling;
                }
                _nodes[sibling].ParentOrNext = grandParent;
                FreeNode(parent);


                // Adjust ancestor bounds.
                int index = grandParent;
                while (index != NullNode)
                {
                    index = Balance(index);

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

                    _nodes[index].Aabb   = BoundingBoxD.CreateMerged(_nodes[child1].Aabb, _nodes[child2].Aabb);
                    _nodes[index].Height = 1 + Math.Max(_nodes[child1].Height, _nodes[child2].Height);

                    index = _nodes[index].ParentOrNext;
                }
            }
            else
            {
                _root = sibling;
                _nodes[sibling].ParentOrNext = NullNode;
                FreeNode(parent);
            }
        }
        // Perform a left or right rotation if node A is imbalanced.
        // Returns the new root index.
        public int Balance(int iA)
        {
            Debug.Assert(iA != NullNode);
            DynamicTreeNode A = _nodes[iA];

            if (A.IsLeaf() || A.Height < 2)
            {
                return(iA);
            }

            int iB = A.Child1;
            int iC = A.Child2;

            Debug.Assert(0 <= iB && iB < _nodeCapacity);
            Debug.Assert(0 <= iC && iC < _nodeCapacity);

            DynamicTreeNode B = _nodes[iB];
            DynamicTreeNode C = _nodes[iC];

            int balance = C.Height - B.Height;

            //Rotate C up
            if (balance > 1)
            {
                int             iF = C.Child1;
                int             iG = C.Child2;
                DynamicTreeNode F  = _nodes[iF];
                DynamicTreeNode G  = _nodes[iG];
                Debug.Assert(0 <= iF && iF < _nodeCapacity);
                Debug.Assert(0 <= iG && iG < _nodeCapacity);

                // Swap A and C
                C.Child1       = iA;
                C.ParentOrNext = A.ParentOrNext;
                A.ParentOrNext = iC;

                // A's old parent should point to C

                if (C.ParentOrNext != NullNode)
                {
                    if (_nodes[C.ParentOrNext].Child1 == iA)
                    {
                        _nodes[C.ParentOrNext].Child1 = iC;
                    }
                    else
                    {
                        Debug.Assert(_nodes[C.ParentOrNext].Child2 == iA);
                        _nodes[C.ParentOrNext].Child2 = iC;
                    }
                }
                else
                {
                    _root = iC;
                }

                // Rotate
                if (F.Height > G.Height)
                {
                    C.Child2       = iF;
                    A.Child2       = iG;
                    G.ParentOrNext = iA;

                    BoundingBoxD.CreateMerged(ref B.Aabb, ref G.Aabb, out A.Aabb);
                    BoundingBoxD.CreateMerged(ref A.Aabb, ref F.Aabb, out C.Aabb);

                    A.Height = 1 + Math.Max(B.Height, G.Height);
                    C.Height = 1 + Math.Max(A.Height, F.Height);
                }
                else
                {
                    C.Child2       = iG;
                    A.Child2       = iF;
                    F.ParentOrNext = iA;

                    BoundingBoxD.CreateMerged(ref B.Aabb, ref F.Aabb, out A.Aabb);
                    BoundingBoxD.CreateMerged(ref A.Aabb, ref G.Aabb, out C.Aabb);

                    A.Height = 1 + Math.Max(B.Height, F.Height);
                    C.Height = 1 + Math.Max(A.Height, G.Height);
                }
                return(iC);
            }

            // Rotate B up
            if (balance < -1)
            {
                int             iD = B.Child1;
                int             iE = B.Child2;
                DynamicTreeNode D  = _nodes[iD];
                DynamicTreeNode E  = _nodes[iE];
                Debug.Assert(0 <= iD && iD < _nodeCapacity);
                Debug.Assert(0 <= iE && iE < _nodeCapacity);

                // Swap A and B
                B.Child1       = iA;
                B.ParentOrNext = A.ParentOrNext;
                A.ParentOrNext = iB;

                // A's old parent should point to B
                if (B.ParentOrNext != NullNode)
                {
                    if (_nodes[B.ParentOrNext].Child1 == iA)
                    {
                        _nodes[B.ParentOrNext].Child1 = iB;
                    }
                    else
                    {
                        Debug.Assert(_nodes[B.ParentOrNext].Child2 == iA);
                        _nodes[B.ParentOrNext].Child2 = iB;
                    }
                }
                else
                {
                    _root = iB;
                }

                // Rotate
                if (D.Height > E.Height)
                {
                    B.Child2       = iD;
                    A.Child1       = iE;
                    E.ParentOrNext = iA;
                    BoundingBoxD.CreateMerged(ref C.Aabb, ref E.Aabb, out A.Aabb);
                    BoundingBoxD.CreateMerged(ref A.Aabb, ref D.Aabb, out B.Aabb);

                    A.Height = 1 + Math.Max(C.Height, E.Height);
                    B.Height = 1 + Math.Max(A.Height, D.Height);
                }
                else
                {
                    B.Child2       = iE;
                    A.Child1       = iD;
                    D.ParentOrNext = iA;

                    BoundingBoxD.CreateMerged(ref C.Aabb, ref D.Aabb, out A.Aabb);
                    BoundingBoxD.CreateMerged(ref A.Aabb, ref E.Aabb, out B.Aabb);

                    A.Height = 1 + Math.Max(C.Height, D.Height);
                    B.Height = 1 + Math.Max(A.Height, E.Height);
                }
                return(iB);
            }
            return(iA);
        }
        private void InsertLeaf(int leaf, bool rebalance)
        {
            ++_insertionCount;

            if (_root == NullNode)
            {
                _root = leaf;
                _nodes[_root].ParentOrNext = NullNode;
                return;
            }

            // Find the best sibling for this node
            BoundingBoxD leafAABB = _nodes[leaf].Aabb;
            //Vector3 leafCenter = leafAABB.Min + (leafAABB.Max - leafAABB.Min) * 0.5f;
            int index = _root;

            while (_nodes[index].IsLeaf() == false)
            {
                int child1 = _nodes[index].Child1;
                int child2 = _nodes[index].Child2;

                if (rebalance)
                {
                    double area = _nodes[index].Aabb.Perimeter;

                    // Combined area
                    BoundingBoxD combinedAABB = BoundingBoxD.CreateMerged(_nodes[index].Aabb, leafAABB);
                    double       combinedArea = combinedAABB.Perimeter;

                    //Cost of creating a new parent for this node and the new leaf
                    double cost = 2.0f * combinedArea;



                    //Minimum cost of pushing the leaf further down the tree
                    double inheritanceCost = 2.0f * (combinedArea - area);
                    //Cost of descending into child1
                    double cost1;
                    if (_nodes[child1].IsLeaf())
                    {
                        BoundingBoxD aabb;
                        BoundingBoxD.CreateMerged(ref leafAABB, ref _nodes[child1].Aabb, out aabb);

                        cost1 = aabb.Perimeter + inheritanceCost;
                    }
                    else
                    {
                        BoundingBoxD aabb;
                        BoundingBoxD.CreateMerged(ref leafAABB, ref _nodes[child1].Aabb, out aabb);
                        double oldArea = _nodes[child1].Aabb.Perimeter;
                        double newArea = aabb.Perimeter;
                        cost1 = (newArea - oldArea) + inheritanceCost;
                    }

                    //Cost of descending into child2
                    double cost2;
                    if (_nodes[child2].IsLeaf())
                    {
                        BoundingBoxD aabb;
                        BoundingBoxD.CreateMerged(ref leafAABB, ref _nodes[child2].Aabb, out aabb);
                        cost2 = aabb.Perimeter + inheritanceCost;
                    }
                    else
                    {
                        BoundingBoxD aabb;
                        BoundingBoxD.CreateMerged(ref leafAABB, ref _nodes[child2].Aabb, out aabb);
                        double oldArea = _nodes[child2].Aabb.Perimeter;
                        double newArea = aabb.Perimeter;
                        cost2 = newArea - oldArea + inheritanceCost;
                    }

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

                    //Descend
                    if (cost1 < cost2)
                    {
                        index = child1;
                    }
                    else
                    {
                        index = child2;
                    }
                }
                else
                {
                    // Surface area heuristic
                    BoundingBoxD aabb1;
                    BoundingBoxD aabb2;
                    BoundingBoxD.CreateMerged(ref leafAABB, ref _nodes[child1].Aabb, out aabb1);
                    BoundingBoxD.CreateMerged(ref leafAABB, ref _nodes[child2].Aabb, out aabb2);
                    double norm1 = (_nodes[child1].Height + 1) * aabb1.Perimeter;
                    double norm2 = (_nodes[child2].Height + 1) * aabb2.Perimeter;

                    if (norm1 < norm2)
                    {
                        index = child1;
                    }
                    else
                    {
                        index = child2;
                    }
                }
            }

            int sibling = index;

            // Create a new parent for the siblings.
            int oldParent = _nodes[index].ParentOrNext;
            int newParent = AllocateNode();

            _nodes[newParent].ParentOrNext = oldParent;
            _nodes[newParent].UserData     = null;
            _nodes[newParent].Aabb         = BoundingBoxD.CreateMerged(leafAABB, _nodes[sibling].Aabb);
            _nodes[newParent].Height       = _nodes[sibling].Height + 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[index].ParentOrNext = newParent;
                _nodes[leaf].ParentOrNext  = newParent;
            }
            else
            {
                // The sibling was the root.
                _nodes[newParent].Child1   = sibling;
                _nodes[newParent].Child2   = leaf;
                _nodes[index].ParentOrNext = newParent;
                _nodes[leaf].ParentOrNext  = newParent;
                _root = newParent;
            }


            // Walk back up the tree fixing heights and AABBs
            index = _nodes[leaf].ParentOrNext;
            while (index != NullNode)
            {
                if (rebalance)
                {
                    index = Balance(index);
                }

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

                Debug.Assert(child1 != NullNode);
                Debug.Assert(child2 != NullNode);

                _nodes[index].Height = 1 + Math.Max(_nodes[child1].Height, _nodes[child2].Height);
                BoundingBoxD.CreateMerged(ref _nodes[child1].Aabb, ref _nodes[child2].Aabb, out _nodes[index].Aabb);
                index = _nodes[index].ParentOrNext;
            }
        }