Esempio n. 1
0
        public DynamicTree()
        {
            m_root = null;
            m_nodeCount = 0;
            m_nodeCapacity = 16;
            m_nodes = new DynamicTreeNode[16];

            // Build a linked list for the free list.
            for (int i = m_nodeCapacity - 1; i >= 0; i--)
            {
                m_nodes[i] = new DynamicTreeNode(i);
                m_nodes[i].parent = (i == m_nodeCapacity - 1) ? null : m_nodes[i + 1];
                m_nodes[i].height = -1;
            }
            m_freeList = 0;

            for (int i = 0; i < drawVecs.Length; i++)
            {
                drawVecs[i] = new Vec2();
            }
        }
Esempio n. 2
0
        private void validateStructure(DynamicTreeNode node)
        {
            if (node == null)
            {
                return;
            }
            Debug.Assert(node == m_nodes[node.id]);

            if (node == m_root)
            {
                Debug.Assert(node.parent == null);
            }

            DynamicTreeNode child1 = node.child1;
            DynamicTreeNode child2 = node.child2;

            if (node.child1 == null)
            {
                Debug.Assert(child1 == null);
                Debug.Assert(child2 == null);
                Debug.Assert(node.height == 0);
                return;
            }

            Debug.Assert(child1 != null && 0 <= child1.id && child1.id < m_nodeCapacity);
            Debug.Assert(child2 != null && 0 <= child2.id && child2.id < m_nodeCapacity);

            Debug.Assert(child1.parent == node);
            Debug.Assert(child2.parent == node);

            validateStructure(child1);
            validateStructure(child2);
        }
Esempio n. 3
0
        private void validateMetrics(DynamicTreeNode node)
        {
            if (node == null)
            {
                return;
            }

            DynamicTreeNode child1 = node.child1;
            DynamicTreeNode child2 = node.child2;

            if (node.child1 == null)
            {
                Debug.Assert(child1 == null);
                Debug.Assert(child2 == null);
                Debug.Assert(node.height == 0);
                return;
            }

            Debug.Assert(child1 != null && 0 <= child1.id && child1.id < m_nodeCapacity);
            Debug.Assert(child2 != null && 0 <= child2.id && child2.id < m_nodeCapacity);

            int height1 = child1.height;
            int height2 = child2.height;
            int height;
            height = 1 + MathUtils.max(height1, height2);
            Debug.Assert(node.height == height);

            AABB aabb = new AABB();
            aabb.combine(child1.aabb, child2.aabb);

            Debug.Assert(aabb.lowerBound.Equals(node.aabb.lowerBound));
            Debug.Assert(aabb.upperBound.Equals(node.aabb.upperBound));

            validateMetrics(child1);
            validateMetrics(child2);
        }
Esempio n. 4
0
        private void removeLeaf(DynamicTreeNode leaf)
        {
            if (leaf == m_root)
            {
                m_root = null;
                return;
            }

            DynamicTreeNode parent = leaf.parent;
            DynamicTreeNode grandParent = parent.parent;
            DynamicTreeNode sibling;
            if (parent.child1 == leaf)
            {
                sibling = parent.child2;
            }
            else
            {
                sibling = parent.child1;
            }

            if (grandParent != null)
            {
                // Destroy parent and connect sibling to grandParent.
                if (grandParent.child1 == parent)
                {
                    grandParent.child1 = sibling;
                }
                else
                {
                    grandParent.child2 = sibling;
                }
                sibling.parent = grandParent;
                freeNode(parent);

                // Adjust ancestor bounds.
                DynamicTreeNode index = grandParent;
                while (index != null)
                {
                    index = balance(index);

                    DynamicTreeNode child1 = index.child1;
                    DynamicTreeNode child2 = index.child2;

                    index.aabb.combine(child1.aabb, child2.aabb);
                    index.height = 1 + MathUtils.max(child1.height, child2.height);

                    index = index.parent;
                }
            }
            else
            {
                m_root = sibling;
                sibling.parent = null;
                freeNode(parent);
            }

            // validate();
        }
Esempio n. 5
0
        private void insertLeaf(int leaf_index)
        {
            DynamicTreeNode leaf = m_nodes[leaf_index];
            if (m_root == null)
            {
                m_root = leaf;
                m_root.parent = null;
                return;
            }

            // find the best sibling
            AABB leafAABB = leaf.aabb;
            DynamicTreeNode index = m_root;
            while (index.child1 != null)
            {
                DynamicTreeNode node = index;
                DynamicTreeNode child1 = node.child1;
                DynamicTreeNode child2 = node.child2;

                float area = node.aabb.getPerimeter();

                combinedAABB.combine(node.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 (child1.child1 == null)
                {
                    combinedAABB.combine(leafAABB, child1.aabb);
                    cost1 = combinedAABB.getPerimeter() + inheritanceCost;
                }
                else
                {
                    combinedAABB.combine(leafAABB, child1.aabb);
                    float oldArea = child1.aabb.getPerimeter();
                    float newArea = combinedAABB.getPerimeter();
                    cost1 = (newArea - oldArea) + inheritanceCost;
                }

                // Cost of descending into child2
                float cost2;
                if (child2.child1 == null)
                {
                    combinedAABB.combine(leafAABB, child2.aabb);
                    cost2 = combinedAABB.getPerimeter() + inheritanceCost;
                }
                else
                {
                    combinedAABB.combine(leafAABB, child2.aabb);
                    float oldArea = child2.aabb.getPerimeter();
                    float newArea = combinedAABB.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;
                }
            }

            DynamicTreeNode sibling = index;
            DynamicTreeNode oldParent = m_nodes[sibling.id].parent;
            DynamicTreeNode newParent = allocateNode();
            newParent.parent = oldParent;
            newParent.userData = null;
            newParent.aabb.combine(leafAABB, sibling.aabb);
            newParent.height = sibling.height + 1;

            if (oldParent != null)
            {
                // The sibling was not the root.
                if (oldParent.child1 == sibling)
                {
                    oldParent.child1 = newParent;
                }
                else
                {
                    oldParent.child2 = newParent;
                }

                newParent.child1 = sibling;
                newParent.child2 = leaf;
                sibling.parent = newParent;
                leaf.parent = newParent;
            }
            else
            {
                // The sibling was the root.
                newParent.child1 = sibling;
                newParent.child2 = leaf;
                sibling.parent = newParent;
                leaf.parent = newParent;
                m_root = newParent;
            }

            // Walk back up the tree fixing heights and AABBs
            index = leaf.parent;
            while (index != null)
            {
                index = balance(index);

                DynamicTreeNode child1 = index.child1;
                DynamicTreeNode child2 = index.child2;

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

                index.height = 1 + MathUtils.max(child1.height, child2.height);
                index.aabb.combine(child1.aabb, child2.aabb);

                index = index.parent;
            }
            // validate();
        }
Esempio n. 6
0
 /**
    * returns a node to the pool
    */
 private void freeNode(DynamicTreeNode node)
 {
     Debug.Assert(node != null);
     Debug.Assert(0 < m_nodeCount);
     node.parent = m_freeList != NULL_NODE ? m_nodes[m_freeList] : null;
     node.height = -1;
     m_freeList = node.id;
     m_nodeCount--;
 }
Esempio n. 7
0
        private int computeHeight(DynamicTreeNode node)
        {
            Debug.Assert(0 <= node.id && node.id < m_nodeCapacity);

            if (node.child1 == null)
            {
                return 0;
            }
            int height1 = computeHeight(node.child1);
            int height2 = computeHeight(node.child2);
            return 1 + MathUtils.max(height1, height2);
        }
Esempio n. 8
0
        // Perform a left or right rotation if node A is imbalanced.
        // Returns the new root index.
        private DynamicTreeNode balance(DynamicTreeNode iA)
        {
            Debug.Assert(iA != null);

            DynamicTreeNode A = iA;
            if (A.child1 == null || A.height < 2)
            {
                return iA;
            }

            DynamicTreeNode iB = A.child1;
            DynamicTreeNode iC = A.child2;
            Debug.Assert(0 <= iB.id && iB.id < m_nodeCapacity);
            Debug.Assert(0 <= iC.id && iC.id < m_nodeCapacity);

            DynamicTreeNode B = iB;
            DynamicTreeNode C = iC;

            int balance = C.height - B.height;

            // Rotate C up
            if (balance > 1)
            {
                DynamicTreeNode iF = C.child1;
                DynamicTreeNode iG = C.child2;
                DynamicTreeNode F = iF;
                DynamicTreeNode G = iG;
                Debug.Assert(F != null);
                Debug.Assert(G != null);
                Debug.Assert(0 <= iF.id && iF.id < m_nodeCapacity);
                Debug.Assert(0 <= iG.id && iG.id < m_nodeCapacity);

                // Swap A and C
                C.child1 = iA;
                C.parent = A.parent;
                A.parent = iC;

                // A's old parent should point to C
                if (C.parent != null)
                {
                    if (C.parent.child1 == iA)
                    {
                        C.parent.child1 = iC;
                    }
                    else
                    {
                        Debug.Assert(C.parent.child2 == iA);
                        C.parent.child2 = iC;
                    }
                }
                else
                {
                    m_root = iC;
                }

                // Rotate
                if (F.height > G.height)
                {
                    C.child2 = iF;
                    A.child2 = iG;
                    G.parent = iA;
                    A.aabb.combine(B.aabb, G.aabb);
                    C.aabb.combine(A.aabb, F.aabb);

                    A.height = 1 + MathUtils.max(B.height, G.height);
                    C.height = 1 + MathUtils.max(A.height, F.height);
                }
                else
                {
                    C.child2 = iG;
                    A.child2 = iF;
                    F.parent = iA;
                    A.aabb.combine(B.aabb, F.aabb);
                    C.aabb.combine(A.aabb, G.aabb);

                    A.height = 1 + MathUtils.max(B.height, F.height);
                    C.height = 1 + MathUtils.max(A.height, G.height);
                }

                return iC;
            }

            // Rotate B up
            if (balance < -1)
            {
                DynamicTreeNode iD = B.child1;
                DynamicTreeNode iE = B.child2;
                DynamicTreeNode D = iD;
                DynamicTreeNode E = iE;
                Debug.Assert(0 <= iD.id && iD.id < m_nodeCapacity);
                Debug.Assert(0 <= iE.id && iE.id < m_nodeCapacity);

                // Swap A and B
                B.child1 = iA;
                B.parent = A.parent;
                A.parent = iB;

                // A's old parent should point to B
                if (B.parent != null)
                {
                    if (B.parent.child1 == iA)
                    {
                        B.parent.child1 = iB;
                    }
                    else
                    {
                        Debug.Assert(B.parent.child2 == iA);
                        B.parent.child2 = iB;
                    }
                }
                else
                {
                    m_root = iB;
                }

                // Rotate
                if (D.height > E.height)
                {
                    B.child2 = iD;
                    A.child1 = iE;
                    E.parent = iA;
                    A.aabb.combine(C.aabb, E.aabb);
                    B.aabb.combine(A.aabb, D.aabb);

                    A.height = 1 + MathUtils.max(C.height, E.height);
                    B.height = 1 + MathUtils.max(A.height, D.height);
                }
                else
                {
                    B.child2 = iE;
                    A.child1 = iD;
                    D.parent = iA;
                    A.aabb.combine(C.aabb, D.aabb);
                    B.aabb.combine(A.aabb, E.aabb);

                    A.height = 1 + MathUtils.max(C.height, D.height);
                    B.height = 1 + MathUtils.max(A.height, E.height);
                }

                return iB;
            }

            return iA;
        }
Esempio n. 9
0
        private DynamicTreeNode allocateNode()
        {
            if (m_freeList == NULL_NODE)
            {
                Debug.Assert(m_nodeCount == m_nodeCapacity);

                DynamicTreeNode[] old = m_nodes;
                m_nodeCapacity *= 2;
                m_nodes = new DynamicTreeNode[m_nodeCapacity];
                Array.Copy(old, 0, m_nodes, 0, old.Length);

                // Build a linked list for the free list.
                for (int i = m_nodeCapacity - 1; i >= m_nodeCount; i--)
                {
                    m_nodes[i] = new DynamicTreeNode(i);
                    m_nodes[i].parent = (i == m_nodeCapacity - 1) ? null : m_nodes[i + 1];
                    m_nodes[i].height = -1;
                }
                m_freeList = m_nodeCount;
            }
            int nodeId = m_freeList;
            DynamicTreeNode treeNode = m_nodes[nodeId];
            m_freeList = treeNode.parent != null ? treeNode.parent.id : NULL_NODE;

            treeNode.parent = null;
            treeNode.child1 = null;
            treeNode.child2 = null;
            treeNode.height = 0;
            treeNode.userData = null;
            ++m_nodeCount;
            return treeNode;
        }
Esempio n. 10
0
        /**
           * Build an optimal tree. Very expensive. For testing.
           */
        public void rebuildBottomUp()
        {
            int[] nodes = new int[m_nodeCount];
            int count = 0;

            // Build array of leaves. Free the rest.
            for (int i = 0; i < m_nodeCapacity; ++i)
            {
                if (m_nodes[i].height < 0)
                {
                    // free node in pool
                    continue;
                }

                DynamicTreeNode node = m_nodes[i];
                if (node.child1 == null)
                {
                    node.parent = null;
                    nodes[count] = i;
                    ++count;
                }
                else
                {
                    freeNode(node);
                }
            }

            AABB b = new AABB();
            while (count > 1)
            {
                float minCost = float.MaxValue;
                int iMin = -1, jMin = -1;
                for (int i = 0; i < count; ++i)
                {
                    AABB aabbi = m_nodes[nodes[i]].aabb;

                    for (int j = i + 1; j < count; ++j)
                    {
                        AABB aabbj = m_nodes[nodes[j]].aabb;
                        b.combine(aabbi, aabbj);
                        float cost = b.getPerimeter();
                        if (cost < minCost)
                        {
                            iMin = i;
                            jMin = j;
                            minCost = cost;
                        }
                    }
                }

                int index1 = nodes[iMin];
                int index2 = nodes[jMin];
                DynamicTreeNode child1 = m_nodes[index1];
                DynamicTreeNode child2 = m_nodes[index2];

                DynamicTreeNode parent = allocateNode();
                parent.child1 = child1;
                parent.child2 = child2;
                parent.height = 1 + MathUtils.max(child1.height, child2.height);
                parent.aabb.combine(child1.aabb, child2.aabb);
                parent.parent = null;

                child1.parent = parent;
                child2.parent = parent;

                nodes[jMin] = nodes[count - 1];
                nodes[iMin] = parent.id;
                --count;
            }

            m_root = m_nodes[nodes[0]];

            validate();
        }
Esempio n. 11
0
        public void raycast(TreeRayCastCallback callback, RayCastInput input)
        {
            Vec2 p1 = input.p1;
            Vec2 p2 = input.p2;
            float p1x = p1.x, p2x = p2.x, p1y = p1.y, p2y = p2.y;
            float vx, vy;
            float rx, ry;
            float absVx, absVy;
            float cx, cy;
            float hx, hy;
            float tempx, tempy;
            r.x = p2x - p1x;
            r.y = p2y - p1y;
            Debug.Assert((r.x*r.x + r.y*r.y) > 0f);
            r.normalize();
            rx = r.x;
            ry = r.y;

            // v is perpendicular to the segment.
            vx = -1f*ry;
            vy = 1f*rx;
            absVx = MathUtils.abs(vx);
            absVy = MathUtils.abs(vy);

            // Separating axis for segment (Gino, p80).
            // |dot(v, p1 - c)| > dot(|v|, h)

            float maxFraction = input.maxFraction;

            // Build a bounding box for the segment.
            AABB segAABB = aabb;
            // Vec2 t = p1 + maxFraction * (p2 - p1);
            // before inline
            // temp.set(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1);
            // Vec2.minToOut(p1, temp, segAABB.lowerBound);
            // Vec2.maxToOut(p1, temp, segAABB.upperBound);
            tempx = (p2x - p1x)*maxFraction + p1x;
            tempy = (p2y - p1y)*maxFraction + p1y;
            segAABB.lowerBound.x = p1x < tempx ? p1x : tempx;
            segAABB.lowerBound.y = p1y < tempy ? p1y : tempy;
            segAABB.upperBound.x = p1x > tempx ? p1x : tempx;
            segAABB.upperBound.y = p1y > tempy ? p1y : tempy;
            // end inline

            nodeStackIndex = 0;
            nodeStack[nodeStackIndex++] = m_root;
            while (nodeStackIndex > 0)
            {
                DynamicTreeNode node = nodeStack[--nodeStackIndex];
                if (node == null)
                {
                    continue;
                }

                AABB nodeAABB = node.aabb;
                if (!AABB.testOverlap(nodeAABB, segAABB))
                {
                    continue;
                }

                // Separating axis for segment (Gino, p80).
                // |dot(v, p1 - c)| > dot(|v|, h)
                // node.aabb.getCenterToOut(c);
                // node.aabb.getExtentsToOut(h);
                cx = (nodeAABB.lowerBound.x + nodeAABB.upperBound.x)*.5f;
                cy = (nodeAABB.lowerBound.y + nodeAABB.upperBound.y)*.5f;
                hx = (nodeAABB.upperBound.x - nodeAABB.lowerBound.x)*.5f;
                hy = (nodeAABB.upperBound.y - nodeAABB.lowerBound.y)*.5f;
                tempx = p1x - cx;
                tempy = p1y - cy;
                float separation = MathUtils.abs(vx*tempx + vy*tempy) - (absVx*hx + absVy*hy);
                if (separation > 0.0f)
                {
                    continue;
                }

                if (node.child1 == null)
                {
                    subInput.p1.x = p1x;
                    subInput.p1.y = p1y;
                    subInput.p2.x = p2x;
                    subInput.p2.y = p2y;
                    subInput.maxFraction = maxFraction;

                    float value = callback.raycastCallback(subInput, node.id);

                    if (value == 0.0f)
                    {
                        // The client has terminated the ray cast.
                        return;
                    }

                    if (value > 0.0f)
                    {
                        // Update segment bounding box.
                        maxFraction = value;
                        // temp.set(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1);
                        // Vec2.minToOut(p1, temp, segAABB.lowerBound);
                        // Vec2.maxToOut(p1, temp, segAABB.upperBound);
                        tempx = (p2x - p1x)*maxFraction + p1x;
                        tempy = (p2y - p1y)*maxFraction + p1y;
                        segAABB.lowerBound.x = p1x < tempx ? p1x : tempx;
                        segAABB.lowerBound.y = p1y < tempy ? p1y : tempy;
                        segAABB.upperBound.x = p1x > tempx ? p1x : tempx;
                        segAABB.upperBound.y = p1y > tempy ? p1y : tempy;
                    }
                }
                else
                {
                    if (nodeStack.Length - nodeStackIndex - 2 <= 0)
                    {
                        DynamicTreeNode[] newBuffer = new DynamicTreeNode[nodeStack.Length*2];
                        Array.Copy(nodeStack, 0, newBuffer, 0, nodeStack.Length);
                        nodeStack = newBuffer;
                    }
                    nodeStack[nodeStackIndex++] = node.child1;
                    nodeStack[nodeStackIndex++] = node.child2;
                }
            }
        }
Esempio n. 12
0
        public void query(TreeCallback callback, AABB aabb)
        {
            Debug.Assert(aabb.isValid());
            nodeStackIndex = 0;
            nodeStack[nodeStackIndex++] = m_root;

            while (nodeStackIndex > 0)
            {
                DynamicTreeNode node = nodeStack[--nodeStackIndex];
                if (node == null)
                {
                    continue;
                }

                if (AABB.testOverlap(node.aabb, aabb))
                {
                    if (node.child1 == null)
                    {
                        bool proceed = callback.treeCallback(node.id);
                        if (!proceed)
                        {
                            return;
                        }
                    }
                    else
                    {
                        if (nodeStack.Length - nodeStackIndex - 2 <= 0)
                        {
                            DynamicTreeNode[] newBuffer = new DynamicTreeNode[nodeStack.Length*2];
                            Array.Copy(nodeStack, 0, newBuffer, 0, nodeStack.Length);
                            nodeStack = newBuffer;
                        }
                        nodeStack[nodeStackIndex++] = node.child1;
                        nodeStack[nodeStackIndex++] = node.child2;
                    }
                }
            }
        }
Esempio n. 13
0
        public void drawTree(DebugDraw argDraw, DynamicTreeNode node, int spot, int height)
        {
            node.aabb.getVertices(drawVecs);

            color.set(1, (height - spot)*1f/height, (height - spot)*1f/height);
            argDraw.drawPolygon(drawVecs, 4, color);

            argDraw.getViewportTranform().getWorldToScreen(node.aabb.upperBound, textVec);
            argDraw.drawString(textVec.x, textVec.y, node.id + "-" + (spot + 1) + "/" + height, color);

            if (node.child1 != null)
            {
                drawTree(argDraw, node.child1, spot + 1, height);
            }
            if (node.child2 != null)
            {
                drawTree(argDraw, node.child2, spot + 1, height);
            }
        }