private void UpdateRotated(
            Node P,
            Node Q,
            Node R,
            int iP,
            int iX,
            int iY,
            bool left)
        {
            Node X = this.nodes[iX];
            Node Y = this.nodes[iY];

            R.right = iX;
            if (left)
            {
                P.left = iY;
            }
            else
            {
                P.right = iY;
            }

            Y.parentOrNext = iP;
            P.aabb         = VoltAABB.CreateMerged(Q.aabb, Y.aabb);
            R.aabb         = VoltAABB.CreateMerged(P.aabb, X.aabb);

            P.height = 1 + Math.Max(Q.height, Y.height);
            R.height = 1 + Math.Max(P.height, X.height);
        }
        private void InsertLeaf(int leafId)
        {
            if (this.rootId == NULL_NODE)
            {
                this.rootId = leafId;
                this.nodes[this.rootId].parentOrNext = NULL_NODE;
                return;
            }

            // Find the best sibling for this node
            Node     leafNode  = this.nodes[leafId];
            VoltAABB leafAABB  = leafNode.aabb;
            int      siblingId = this.FindBestSibling(ref leafAABB);
            Node     sibling   = this.nodes[siblingId];

            // Create a new parent
            int oldParentId = sibling.parentOrNext;
            int newParentId;

            Node newParent = this.AllocateNode(out newParentId);

            newParent.Initialize(oldParentId, sibling.height + 1);
            newParent.aabb = VoltAABB.CreateMerged(leafAABB, sibling.aabb);

            if (oldParentId != NULL_NODE)
            {
                Node oldParent = this.nodes[oldParentId];
                // The sibling was not the root
                if (oldParent.left == siblingId)
                {
                    oldParent.left = newParentId;
                }
                else
                {
                    oldParent.right = newParentId;
                }
            }
            else
            {
                // The sibling was the root
                this.rootId = newParentId;
            }

            newParent.left        = siblingId;
            newParent.right       = leafId;
            sibling.parentOrNext  = newParentId;
            leafNode.parentOrNext = newParentId;

            // Walk back up the tree fixing heights and AABBs
            this.FixAncestors(leafNode.parentOrNext);
        }
        private void FixAncestors(int index)
        {
            while (index != NULL_NODE)
            {
                index = this.Balance(index);

                Node indexNode = this.nodes[index];
                Node left      = this.nodes[indexNode.left];
                Node right     = this.nodes[indexNode.right];

                indexNode.aabb   = VoltAABB.CreateMerged(left.aabb, right.aabb);
                indexNode.height = 1 + Math.Max(left.height, right.height);
                index            = indexNode.parentOrNext;
            }
        }
 private Fix64 GetCost(int index, ref VoltAABB leafAABB)
 {
     if (this.nodes[index].IsLeaf)
     {
         VoltAABB aabb =
             VoltAABB.CreateMerged(leafAABB, this.nodes[index].aabb);
         return(aabb.Perimeter);
     }
     else
     {
         VoltAABB aabb =
             VoltAABB.CreateMerged(leafAABB, this.nodes[index].aabb);
         Fix64 oldArea = this.nodes[index].aabb.Perimeter;
         Fix64 newArea = aabb.Perimeter;
         return(newArea - oldArea);
     }
 }
        private int FindBestSibling(ref VoltAABB leafAABB)
        {
            int index = this.rootId;

            while (this.nodes[index].IsLeaf == false)
            {
                Node indexNode = this.nodes[index];

                int child1 = indexNode.left;
                int child2 = indexNode.right;

                Fix64 area = indexNode.aabb.Perimeter;

                VoltAABB combinedAABB = new VoltAABB();
                VoltAABB.CreateMerged(indexNode.aabb, leafAABB);
                Fix64 combinedArea = combinedAABB.Perimeter;

                // Cost of creating a new parent for this node and the new leaf
                Fix64 cost = (Fix64)2 * combinedArea;

                // Minimum cost of pushing the leaf further down the tree
                Fix64 inheritanceCost = (Fix64)2 * (combinedArea - area);
                Fix64 cost1           = this.GetCost(child1, ref leafAABB) + inheritanceCost;
                Fix64 cost2           = this.GetCost(child2, ref leafAABB) + inheritanceCost;

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

                // Descend
                if (cost1 < cost2)
                {
                    index = child1;
                }
                else
                {
                    index = child2;
                }
            }
            return(index);
        }