示例#1
0
        // Collision Detection in Interactive 3D Environments by Gino van den Bergen
        // From Section 3.1.2
        // x = s + a * r
        // norm(x) = radius
        public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex)
        {
            output = new RayCastOutput();

	        Vector2 position = transform.Position + MathUtils.Multiply(ref transform.R, _p);
	        Vector2 s = input.p1 - position;
	        float b = Vector2.Dot(s, s) - _radius * _radius;

	        // Solve quadratic equation.
            Vector2 r = input.p2 - input.p1;
	        float c =  Vector2.Dot(s, r);
	        float rr = Vector2.Dot(r, r);
	        float sigma = c * c - rr * b;

	        // Check for negative discriminant and short segment.
	        if (sigma < 0.0f || rr < Settings.b2_epsilon)
	        {
                return false;
	        }

	        // Find the point of intersection of the line with the circle.
	        float a = -(c + (float)Math.Sqrt((double)sigma));

	        // Is the intersection point on the segment?
            if (0.0f <= a && a <= input.maxFraction * rr)
	        {
		        a /= rr;
                output.fraction = a;
                Vector2 norm = (s + a * r);
                norm.Normalize();
                output.normal = norm;
                return true;
            }

            return false;
        }
        /// <summary>
        /// From Real-time Collision Detection, p179.
        /// </summary>
        /// <param name="output"></param>
        /// <param name="input"></param>
        /// <returns></returns>
        public bool RayCast(out RayCastOutput output, ref RayCastInput input)
        {
            output = new RayCastOutput();

            float tmin = -Settings.b2_maxFloat;
            float tmax = Settings.b2_maxFloat;

            Vector2 p = input.p1;
            Vector2 d = input.p2 - input.p1;
            Vector2 absD = MathUtils.Abs(d);

            Vector2 normal = Vector2.Zero;

            for (int i = 0; i < 2; ++i)
            {
                float absD_i = i == 0 ? absD.X : absD.Y;
                float lowerBound_i = i == 0 ? lowerBound.X : lowerBound.Y;
                float upperBound_i = i == 0 ? upperBound.X : upperBound.Y;
                float p_i = i == 0 ? p.X : p.Y;

                if (absD_i < Settings.b2_epsilon)
                {
                    // Parallel.
                    if (p_i < lowerBound_i || upperBound_i < p_i)
                    {
                        return false;
                    }
                }
                else
                {
                    float d_i = i == 0 ? d.X : d.Y;

                    float inv_d = 1.0f / d_i;
                    float t1 = (lowerBound_i - p_i) * inv_d;
                    float t2 = (upperBound_i - p_i) * inv_d;

                    // Sign of the normal vector.
                    float s = -1.0f;

                    if (t1 > t2)
                    {
                        MathUtils.Swap<float>(ref t1, ref t2);
                        s = 1.0f;
                    }

                    // Push the min up
                    if (t1 > tmin)
                    {
                        if (i == 0)
                        {
                            normal.X = s;
                        }
                        else
                        {
                            normal.Y = s;
                        }

                        tmin = t1;
                    }

                    // Pull the max down
                    tmax = Math.Min(tmax, t2);

                    if (tmin > tmax)
                    {
                        return false;
                    }
                }
            }

            // Does the ray start inside the box?
            // Does the ray intersect beyond the max fraction?
            if (tmin < 0.0f || input.maxFraction < tmin)
            {
                return false;
            }

            // Intersection.
            output.fraction = tmin;
            output.normal = normal;
            return true;
        }
        /// <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.
        /// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
        /// @param callback a callback class that is called for each proxy that is hit by the ray.
        /// </summary>
        /// <param name="callback"></param>
        /// <param name="input"></param>
        internal void RayCast(RayCastCallbackInternal callback, ref RayCastInput input)
        {
            Vector2 p1 = input.p1;
            Vector2 p2 = input.p2;
            Vector2 r = p2 - p1;
            Debug.Assert(r.LengthSquared() > 0.0f);
            r.Normalize();

            // v is perpendicular to the segment.
            Vector2 v = MathUtils.Cross(1.0f, r);
            Vector2 abs_v = MathUtils.Abs(v);

            // 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 segmentAABB = new AABB();
            {
                Vector2 t = p1 + maxFraction * (p2 - p1);
                segmentAABB.lowerBound = Vector2.Min(p1, t);
                segmentAABB.upperBound = Vector2.Max(p1, t);
            }

            int count = 0;
            stack[count++] = _root;

            while (count > 0)
            {
                int nodeId = stack[--count];
                if (nodeId == NullNode)
                {
                    continue;
                }

                DynamicTreeNode node = _nodes[nodeId];

                if (AABB.TestOverlap(ref node.aabb, ref segmentAABB) == false)
                {
                    continue;
                }

                // Separating axis for segment (Gino, p80).
                // |dot(v, p1 - c)| > dot(|v|, h)
                Vector2 c = node.aabb.GetCenter();
                Vector2 h = node.aabb.GetExtents();
                float separation = Math.Abs(Vector2.Dot(v, p1 - c)) - Vector2.Dot(abs_v, h);
                if (separation > 0.0f)
                {
                    continue;
                }

                if (node.IsLeaf())
                {
                    RayCastInput subInput;
                    subInput.p1 = input.p1;
                    subInput.p2 = input.p2;
                    subInput.maxFraction = maxFraction;

                    float value = callback(ref subInput, nodeId);

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

                    if (value > 0.0f)
                    {
                        // Update segment bounding box.
                        maxFraction = value;
                        Vector2 t = p1 + maxFraction * (p2 - p1);
                        segmentAABB.lowerBound = Vector2.Min(p1, t);
                        segmentAABB.upperBound = Vector2.Max(p1, t);
                    }
                }
                else
                {
                    if (count < k_stackSize)
                    {
                        stack[count++] = node.child1;
                    }

                    if (count < k_stackSize)
                    {
                        stack[count++] = node.child2;
                    }
                }
            }
        }
        public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform xf)
        {
            output = new RayCastOutput();

            // Put the ray into the polygon's frame of reference.
            Vector2 p1 = MathUtils.MultiplyT(ref xf.R, input.p1 - xf.Position);
            Vector2 p2 = MathUtils.MultiplyT(ref xf.R, input.p2 - xf.Position);
            Vector2 d = p2 - p1;

            if (_vertexCount == 2)
            {
                Vector2 v1 = _vertices[0];
                Vector2 v2 = _vertices[1];
                Vector2 normal = _normals[0];

                // q = p1 + t * d
                // dot(normal, q - v1) = 0
                // dot(normal, p1 - v1) + t * dot(normal, d) = 0
                float numerator = Vector2.Dot(normal, v1 - p1);
                float denominator = Vector2.Dot(normal, d);

                if (denominator == 0.0f)
                {
                    return false;
                }

                float t = numerator / denominator;
                if (t < 0.0f || 1.0f < t)
                {
                    return false;
                }

                Vector2 q = p1 + t * d;

                // q = v1 + s * r
                // s = dot(q - v1, r) / dot(r, r)
                Vector2 r = v2 - v1;
                float rr = Vector2.Dot(r, r);
                if (rr == 0.0f)
                {
                    return false;
                }

                float s = Vector2.Dot(q - v1, r) / rr;
                if (s < 0.0f || 1.0f < s)
                {
                    return false;
                }

                output.fraction = t;
                if (numerator > 0.0f)
                {
                    output.normal = -normal;
                }
                else
                {
                    output.normal = normal;
                }
                return true;
            }
            else
            {
                float lower = 0.0f, upper = input.maxFraction;

                int index = -1;

                for (int i = 0; i < _vertexCount; ++i)
                {
                    // p = p1 + a * d
                    // dot(normal, p - v) = 0
                    // dot(normal, p1 - v) + a * dot(normal, d) = 0
                    float numerator = Vector2.Dot(_normals[i], _vertices[i] - p1);
                    float denominator = Vector2.Dot(_normals[i], d);

                    if (denominator == 0.0f)
                    {
                        if (numerator < 0.0f)
                        {
                            return false;
                        }
                    }
                    else
                    {
                        // Note: we want this predicate without division:
                        // lower < numerator / denominator, where denominator < 0
                        // Since denominator < 0, we have to flip the inequality:
                        // lower < numerator / denominator <==> denominator * lower > numerator.
                        if (denominator < 0.0f && numerator < lower * denominator)
                        {
                            // Increase lower.
                            // The segment enters this half-space.
                            lower = numerator / denominator;
                            index = i;
                        }
                        else if (denominator > 0.0f && numerator < upper * denominator)
                        {
                            // Decrease upper.
                            // The segment exits this half-space.
                            upper = numerator / denominator;
                        }
                    }

                    if (upper < lower - Settings.b2_epsilon)
                    {
                        return false;
                    }
                }

                Debug.Assert(0.0f <= lower && lower <= input.maxFraction);

                if (index >= 0)
                {
                    output.fraction = lower;
                    output.normal = MathUtils.Multiply(ref xf.R, _normals[index]);
                    return true;
                }
            }

            return false;
        }
        float RayCastCallbackWrapper(ref RayCastInput input, int proxyId)
        {
            object userData = _contactManager._broadPhase.GetUserData(proxyId);
            Fixture fixture = (Fixture)userData;
            RayCastOutput output;
            bool hit = fixture.RayCast(out output, ref input);

            if (hit)
            {
                float fraction = output.fraction;
                Vector2 point = (1.0f - fraction) * input.p1 + fraction * input.p2;
                return _rayCastCallback(fixture, point, output.normal, fraction);
            }

            return input.maxFraction;
        }
        /// <summary>
        /// Ray-cast the world for all fixtures in the path of the ray. Your callback
        /// controls whether you get the closest point, any point, or n-points.
        /// The ray-cast ignores shapes that contain the starting point.
        /// </summary>
        /// <param name="callback">a user implemented callback class.</param>
        /// <param name="point1">the ray starting point</param>
        /// <param name="point2">the ray ending point</param>
        public void RayCast(RayCastCallback callback, Vector2 point1, Vector2 point2)
        {
            RayCastInput input = new RayCastInput();
            input.maxFraction = 1.0f;
            input.p1 = point1;
            input.p2 = point2;

            _rayCastCallback = callback;
            _contactManager._broadPhase.RayCast(_rayCastCallbackWrapper, ref input);
            _rayCastCallback = null;
        }
示例#7
0
        /// Cast a ray against this Shape.
	    /// @param output the ray-cast results.
	    /// @param input the ray-cast input parameters.
        public bool RayCast(out RayCastOutput output, ref RayCastInput input, int childIndex)
        {
            Transform xf;
            _body.GetTransform(out xf);
            return _shape.RayCast(out output, ref input, ref xf, childIndex);
        }
示例#8
0
        /// Implement Shape.
        public override bool RayCast(out RayCastOutput output, ref RayCastInput input,
            ref Transform transform, int childIndex)
        {
            Debug.Assert(childIndex < _count);

            int i1 = childIndex;
            int i2 = childIndex + 1;
            if (i2 == _count)
            {
                i2 = 0;
            }

            s_edgeShape._vertex1 = _vertices[i1];
            s_edgeShape._vertex2 = _vertices[i2];

            return s_edgeShape.RayCast(out output, ref input, ref transform, 0);
        }
示例#9
0
        /// Implement Shape.
        /// 
        // p = p1 + t * d
        // v = v1 + s * e
        // p1 + t * d = v1 + s * e
        // s * e - t * d = p1 - v1
        //
        public override bool RayCast(out RayCastOutput output, ref RayCastInput input,
            ref Transform transform, int childIndex)
        {
            output = new RayCastOutput();

            // Put the ray into the edge's frame of reference.
            Vector2 p1 = MathUtils.MultiplyT(ref transform.R, input.p1 - transform.Position);
            Vector2 p2 = MathUtils.MultiplyT(ref transform.R, input.p2 - transform.Position);
            Vector2 d = p2 - p1;

            Vector2 v1 = _vertex1;
            Vector2 v2 = _vertex2;
            Vector2 e = v2 - v1;
            Vector2 normal = new Vector2(e.Y, -e.X);
            normal.Normalize();

            // q = p1 + t * d
            // dot(normal, q - v1) = 0
            // dot(normal, p1 - v1) + t * dot(normal, d) = 0
            float numerator = Vector2.Dot(normal, v1 - p1);
            float denominator = Vector2.Dot(normal, d);

            if (denominator == 0.0f)
            {
                return false;
            }

            float t = numerator / denominator;
            if (t < 0.0f || 1.0f < t)
            {
                return false;
            }

            Vector2 q = p1 + t * d;

            // q = v1 + s * r
            // s = dot(q - v1, r) / dot(r, r)
            Vector2 r = v2 - v1;
            float rr = Vector2.Dot(r, r);
            if (rr == 0.0f)
            {
                return false;
            }

            float s = Vector2.Dot(q - v1, r) / rr;
            if (s < 0.0f || 1.0f < s)
            {
                return false;
            }

            output.fraction = t;
            if (numerator > 0.0f)
            {
                output.normal = -normal;
            }
            else
            {
                output.normal = normal;
            }
            return true;
        }
示例#10
0
        /// Cast a ray against a child shape.
	    /// @param output the ray-cast results.
	    /// @param input the ray-cast input parameters.
	    /// @param transform the transform to be applied to the shape.
        /// @param childIndex the child shape index
        public abstract bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex);
 /// <summary>
 /// Cast a ray against this shape.
 /// </summary>
 /// <param name="output">output the ray-cast results.</param>
 /// <param name="input">the ray-cast input parameters.</param>
 /// <param name="transform">the transform to be applied to the shape.</param>
 /// <returns></returns>
 public abstract bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform);
示例#12
0
        public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform xf)
        {
            output = new RayCastOutput();

            // Put the ray into the polygon's frame of reference.
            Vector2 p1 = MathUtils.MultiplyT(ref xf.R, input.p1 - xf.Position);
            Vector2 p2 = MathUtils.MultiplyT(ref xf.R, input.p2 - xf.Position);
            Vector2 d  = p2 - p1;

            if (_vertexCount == 2)
            {
                Vector2 v1     = _vertices[0];
                Vector2 v2     = _vertices[1];
                Vector2 normal = _normals[0];

                // q = p1 + t * d
                // dot(normal, q - v1) = 0
                // dot(normal, p1 - v1) + t * dot(normal, d) = 0
                float numerator   = Vector2.Dot(normal, v1 - p1);
                float denominator = Vector2.Dot(normal, d);

                if (denominator == 0.0f)
                {
                    return(false);
                }

                float t = numerator / denominator;
                if (t < 0.0f || 1.0f < t)
                {
                    return(false);
                }

                Vector2 q = p1 + t * d;

                // q = v1 + s * r
                // s = dot(q - v1, r) / dot(r, r)
                Vector2 r  = v2 - v1;
                float   rr = Vector2.Dot(r, r);
                if (rr == 0.0f)
                {
                    return(false);
                }

                float s = Vector2.Dot(q - v1, r) / rr;
                if (s < 0.0f || 1.0f < s)
                {
                    return(false);
                }

                output.fraction = t;
                if (numerator > 0.0f)
                {
                    output.normal = -normal;
                }
                else
                {
                    output.normal = normal;
                }
                return(true);
            }
            else
            {
                float lower = 0.0f, upper = input.maxFraction;

                int index = -1;

                for (int i = 0; i < _vertexCount; ++i)
                {
                    // p = p1 + a * d
                    // dot(normal, p - v) = 0
                    // dot(normal, p1 - v) + a * dot(normal, d) = 0
                    float numerator   = Vector2.Dot(_normals[i], _vertices[i] - p1);
                    float denominator = Vector2.Dot(_normals[i], d);

                    if (denominator == 0.0f)
                    {
                        if (numerator < 0.0f)
                        {
                            return(false);
                        }
                    }
                    else
                    {
                        // Note: we want this predicate without division:
                        // lower < numerator / denominator, where denominator < 0
                        // Since denominator < 0, we have to flip the inequality:
                        // lower < numerator / denominator <==> denominator * lower > numerator.
                        if (denominator < 0.0f && numerator < lower * denominator)
                        {
                            // Increase lower.
                            // The segment enters this half-space.
                            lower = numerator / denominator;
                            index = i;
                        }
                        else if (denominator > 0.0f && numerator < upper * denominator)
                        {
                            // Decrease upper.
                            // The segment exits this half-space.
                            upper = numerator / denominator;
                        }
                    }

                    if (upper < lower - Settings.b2_epsilon)
                    {
                        return(false);
                    }
                }

                Debug.Assert(0.0f <= lower && lower <= input.maxFraction);

                if (index >= 0)
                {
                    output.fraction = lower;
                    output.normal   = MathUtils.Multiply(ref xf.R, _normals[index]);
                    return(true);
                }
            }

            return(false);
        }
示例#13
0
 internal void RayCast(RayCastCallbackInternal callback, ref RayCastInput input)
 {
     _tree.RayCast(callback, ref input);
 }