Exemplo n.º 1
0
        /// <summary>
        /// Ray-cast against the proxies in the tree. This relies on the callback to perform a exact
        /// ray-cast in the case were the proxy contains a shape. The callback also performs the any
        /// collision filtering. This has performance roughly equal to k * log(n), where k is the number of
        /// collisions and n is the number of proxies in the tree.
        /// </summary>
        /// <param name="input">the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param>
        /// <param name="callback">a callback class that is called for each proxy that is hit by the ray.</param>
        public virtual void raycast(TreeRayCastCallback callback, RayCastInput input)
        {
            Vec2 p1 = input.p1;
            Vec2 p2 = input.p2;
            r.set_Renamed(p2).subLocal(p1);
            Debug.Assert(r.lengthSquared() > 0f);
            r.normalize();

            // v is perpendicular to the segment.
            Vec2.crossToOutUnsafe(1f, r, v);
            absV.set_Renamed(v).absLocal();

            // 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);
            temp.set_Renamed(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1);
            Vec2.minToOut(p1, temp, segAABB.lowerBound);
            Vec2.maxToOut(p1, temp, segAABB.upperBound);

            intStack.push(m_root);
            while (intStack.Count > 0)
            {
                int nodeId = intStack.pop();
                if (nodeId == TreeNode.NULL_NODE)
                {
                    continue;
                }

                TreeNode node = m_nodes[nodeId];

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

                // Separating axis for segment (Gino, p80).
                // |dot(v, p1 - c)| > dot(|v|, h)
                node.aabb.getCenterToOut(c);
                node.aabb.getExtentsToOut(h);
                temp.set_Renamed(p1).subLocal(c);
                float separation = MathUtils.abs(Vec2.dot(v, temp)) - Vec2.dot(absV, h);
                if (separation > 0.0f)
                {
                    continue;
                }

                if (node.Leaf)
                {
                    subInput.p1.set_Renamed(input.p1);
                    subInput.p2.set_Renamed(input.p2);
                    subInput.maxFraction = maxFraction;

                    float value_Renamed = callback.raycastCallback(subInput, nodeId);

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

                    if (value_Renamed > 0.0f)
                    {
                        // Update segment bounding box.
                        maxFraction = value_Renamed;
                        t.set_Renamed(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1);
                        Vec2.minToOut(p1, t, segAABB.lowerBound);
                        Vec2.maxToOut(p1, t, segAABB.upperBound);
                    }
                }
                else
                {
                    intStack.push(node.child1);
                    intStack.push(node.child2);
                }
            }
        }
Exemplo n.º 2
0
 public void raycast(TreeRayCastCallback callback, RayCastInput input)
 {
     m_tree.raycast(callback, input);
 }
Exemplo n.º 3
0
        public void raycast(TreeRayCastCallback callback, RayCastInput input)
        {
            Vec2   p1 = input.p1;
            Vec2   p2 = input.p2;
            double p1x = p1.x, p2x = p2.x, p1y = p1.y, p2y = p2.y;
            double vx, vy;
            double rx, ry;
            double absVx, absVy;
            double cx, cy;
            double hx, hy;
            double tempx, tempy;

            r.x = p2x - p1x;
            r.y = p2y - p1y;
            r.normalize();
            rx = r.x;
            ry = r.y;

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

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

            double 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

            nodeStack.reset();
            nodeStack.push(m_root);
            while (nodeStack.getCount() > 0)
            {
                DynamicTreeNode node = nodeStack.pop();
                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) * .5d;
                cy    = (nodeAABB.lowerBound.y + nodeAABB.upperBound.y) * .5d;
                hx    = (nodeAABB.upperBound.x - nodeAABB.lowerBound.x) * .5d;
                hy    = (nodeAABB.upperBound.y - nodeAABB.lowerBound.y) * .5d;
                tempx = p1x - cx;
                tempy = p1y - cy;
                double separation = MathUtils.abs(vx * tempx + vy * tempy) - (absVx * hx + absVy * hy);
                if (separation > 0.0d)
                {
                    continue;
                }

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

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

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

                    if (value > 0.0d)
                    {
                        // 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
                {
                    nodeStack.push(node.child1);
                    nodeStack.push(node.child2);
                }
            }
        }
Exemplo n.º 4
0
        /**
         * Ray-cast against the proxies in the tree. This relies on the callback to perform a exact
         * ray-cast in the case were the proxy contains a shape. The callback also performs the any
         * collision filtering. This has performance roughly equal to k * log(n), where k is the number of
         * collisions and n is the number of proxies in the tree.
         *
         * @param input the ray-cast input data. The ray : from p1 to p1 + maxFraction * (p2 - p1).
         * @param callback a callback class that is called for each proxy that is hit by the ray.
         */

        public void raycast(TreeRayCastCallback callback, RayCastInput input)
        {
            m_tree.raycast(callback, input);
        }
Exemplo n.º 5
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;
                }
            }
        }