Exemplo n.º 1
0
        /// <summary> Implement Shape.
        /// </summary>
        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));
        }
Exemplo n.º 2
0
        /// <summary>
        /// Collision Detection in Interactive 3D Environments by Gino van den Bergen
        /// From Section 3.1.2
        /// x = s + a * r
        /// norm(x) = radius
        /// </summary>
        /// <param name="output"></param>
        /// <param name="input"></param>
        /// <param name="transform"></param>
        /// <returns></returns>
        public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform)
        {
            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);
        }
Exemplo n.º 3
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;
        }
Exemplo n.º 5
0
        public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform xf, int childIndex)
        {
            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;
                        }
                    }

                    // The use of epsilon here causes the assert on lower to trip
                    // in some cases. Apparently the use of epsilon was to make edge
                    // shapes work, but now those are handled separately.
                    //if (upper < lower - b2_epsilon)
                    if (upper < lower)
                    {
                        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);
        }
        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;
        }
Exemplo n.º 7
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);
        }
Exemplo n.º 8
0
Arquivo: Shape.cs Projeto: Bitsits/web
 /// <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);
Exemplo n.º 9
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);
        }
Exemplo n.º 10
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);
        }
Exemplo n.º 11
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;
        }
Exemplo n.º 12
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);
Exemplo n.º 13
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);
Exemplo n.º 14
0
        /// <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);
        }
Exemplo n.º 15
0
 /// <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);