示例#1
0
        /// @see Shape.TestSegment
        public override SegmentCollide TestSegment(	ref XForm transform,
					        out float lambda,
                            out Vector2 normal,
					        ref Segment segment,
					        float maxLambda)
        {
            lambda = 0.0f;
            normal = Vector2.Zero;

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

            // Does the segment start inside the circle?
            if (b < 0.0f)
            {
                return SegmentCollide.StartsInside;
            }

            // Solve quadratic equation.
            Vector2 r = segment.p2 - segment.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_FLT_EPSILON)
            {
                return SegmentCollide.Miss;
            }

            // 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 <= maxLambda * rr)
            {
                a /= rr;
                lambda = a;
                normal = s + a * r;
                normal.Normalize();
                return SegmentCollide.Hit;
            }

            return SegmentCollide.Miss;
        }
示例#2
0
 /// Perform a ray cast against this shape.
 /// @param xf the shape world transform.
 /// @param lambda returns the hit fraction. You can use this to compute the contact point
 /// p = (1 - lambda) * segment.p1 + lambda * segment.p2.
 /// @param normal returns the normal at the contact point. If there is no intersection, the normal
 /// is not set.
 /// @param segment defines the begin and end point of the ray cast.
 /// @param maxLambda a number typically in the range [0,1].
 public SegmentCollide TestSegment(out float lambda, out Vector2 normal, ref Segment segment, float maxLambda)
 {
     XForm xf;
     _body.GetXForm(out xf);
     return _shape.TestSegment(ref xf, out lambda, out normal, ref segment, maxLambda);
 }
示例#3
0
        /// @see Shape.TestSegment
        public override SegmentCollide TestSegment(ref XForm xf,
		    out float lambda,
		    out Vector2 normal,
		    ref Segment segment,
		    float maxLambda)
        {
            lambda = 0;
            normal = Vector2.Zero;
            float lower = 0.0f, upper = maxLambda;

            Vector2 p1 = MathUtils.MultiplyT(ref xf.R, segment.p1 - xf.Position);
            Vector2 p2 = MathUtils.MultiplyT(ref xf.R, segment.p2 - xf.Position);
            Vector2 d = p2 - p1;
            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 SegmentCollide.Miss;
                    }
                }
                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)
                {
                    return SegmentCollide.Miss;
                }
            }

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

            if (index >= 0)
            {
                lambda = lower;
                normal = MathUtils.Multiply(ref xf.R, _normals[index]);
                return SegmentCollide.Hit;
            }

            lambda = 0;
            return SegmentCollide.StartsInside;
        }
示例#4
0
        public Vector2 p2; ///< the ending point

        #endregion Fields

        #region Methods

        /// Ray cast against this segment with another segment.
        // Collision Detection in Interactive 3D Environments by Gino van den Bergen
        // From Section 3.4.1
        // x = mu1 * p1 + mu2 * p2
        // mu1 + mu2 = 1 && mu1 >= 0 && mu2 >= 0
        // mu1 = 1 - mu2;
        // x = (1 - mu2) * p1 + mu2 * p2
        //   = p1 + mu2 * (p2 - p1)
        // x = s + a * r (s := start, r := end - start)
        // s + a * r = p1 + mu2 * d (d := p2 - p1)
        // -a * r + mu2 * d = b (b := s - p1)
        // [-r d] * [a; mu2] = b
        // Cramer's rule:
        // denom = det[-r d]
        // a = det[b d] / denom
        // mu2 = det[-r b] / denom
        public bool TestSegment(out float lambda, out Vector2 normal, ref Segment segment, float maxLambda)
        {
            lambda = 0;
            normal = Vector2.Zero;

            Vector2 s = segment.p1;
            Vector2 r = segment.p2 - s;
            Vector2 d = p2 - p1;
            Vector2 n = MathUtils.Cross(d, 1.0f);

            float k_slop = 100.0f * Settings.b2_FLT_EPSILON;
            float denom = -Vector2.Dot(r, n);

            // Cull back facing collision and ignore parallel segments.
            if (denom > k_slop)
            {
                // Does the segment intersect the infinite line associated with this segment?
                Vector2 b = s - p1;
                float a = Vector2.Dot(b, n);

                if (0.0f <= a && a <= maxLambda * denom)
                {
                    float mu2 = -r.X * b.Y + r.Y * b.X;

                    // Does the segment intersect this segment?
                    if (-k_slop * denom <= mu2 && mu2 <= denom * (1.0f + k_slop))
                    {
                        a /= denom;
                        n.Normalize();
                        lambda = a;
                        normal = n;
                        return true;
                    }
                }
            }

            return false;
        }
示例#5
0
        /// Perform a ray cast against this shape.
        /// @param xf the shape world transform.
        /// @param lambda returns the hit fraction. You can use this to compute the contact point
        /// p = (1 - lambda) * segment.p1 + lambda * segment.p2.
        /// @param normal returns the normal at the contact point. If there is no intersection, the normal
        /// is not set.
        /// @param segment defines the begin and end point of the ray cast.
        /// @param maxLambda a number typically in the range [0,1].
        public abstract SegmentCollide TestSegment(	ref XForm xf,
											    out float lambda,
											    out Vector2 normal,
											    ref Segment segment,
											    float maxLambda);