Esempio n. 1
0
        private float RayCastCallback(RayCastInput input, int proxyid)
        {
            Actor actor = _tree.GetUserData(proxyid);

            RayCastOutput output;
            bool          hit = actor.AABB.RayCast(out output, ref input);

            if (hit)
            {
                _rayCastOutput = output;
                _rayActor      = actor;
                actor.Fraction = output.Fraction;
                return(output.Fraction);
            }

            return(input.MaxFraction);
        }
Esempio n. 2
0
        public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex)
        {
            Debug.Assert(childIndex < Vertices.Count);

            int i1 = childIndex;
            int i2 = childIndex + 1;

            if (i2 == Vertices.Count)
            {
                i2 = 0;
            }

            _edgeShape.Vertex1 = Vertices[i1];
            _edgeShape.Vertex2 = Vertices[i2];

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

            output = new RayCastOutput();

            Vector2 position = transform.p + MathUtils.Mul(transform.q, Position);
            Vector2 s        = input.Point1 - position;
            float   b        = Vector2.Dot(s, s) - _2radius;

            // Solve quadratic equation.
            Vector2 r     = input.Point2 - input.Point1;
            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.Epsilon)
            {
                return(false);
            }

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

            // Is the intersection point on the segment?
            if (0.0f <= a && a <= input.MaxFraction * rr)
            {
                a /= rr;
                output.Fraction = a;

                //TODO: Check results here
                output.Normal = s + a * r;
                if (output.Normal != Vector2.Zero)
                {
                    output.Normal = Vector2.Normalize(output.Normal);
                }

                return(true);
            }

            return(false);
        }
Esempio n. 4
0
        public override bool Raycast(RayCastOutput output, RayCastInput input, Transform xf, int childIndex)
        {
            Debug.Assert(childIndex < Count);

            EdgeShape edgeShape = pool0;

            int i1 = childIndex;
            int i2 = childIndex + 1;

            if (i2 == Count)
            {
                i2 = 0;
            }

            edgeShape.Vertex1.Set(Vertices[i1]);
            edgeShape.Vertex2.Set(Vertices[i2]);

            return(edgeShape.Raycast(output, input, xf, 0));
        }
Esempio n. 5
0
        public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex)
        {
            // Collision Detection in Interactive 3D Environments by Gino van den Bergen
            // From Section 3.1.2
            // x = s + a * r
            // norm(x) = radius

            output = new RayCastOutput();

            Vector2 position = transform.p + MathUtils.Mul(transform.q, Position);
            Vector2 s = input.Point1 - position;
            float b = Vector2.Dot(s, s) - _2radius;

            // Solve quadratic equation.
            Vector2 r = input.Point2 - input.Point1;
            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.Epsilon)
            {
                return false;
            }

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

            // Is the intersection point on the segment?
            if (0.0f <= a && a <= input.MaxFraction * rr)
            {
                a /= rr;
                output.Fraction = a;

                //TODO: Check results here
                output.Normal = s + a * r;
                output.Normal.Normalize();
                return true;
            }

            return false;
        }
Esempio n. 6
0
        private void RayCast()
        {
            _rayActor = null;

            RayCastInput input = _rayCastInput;

            // Ray cast against the dynamic tree.
            _tree.RayCast(RayCastCallback, ref input);

            // Brute force ray cast.
            Actor bruteActor = null;

#if DEBUG
            RayCastOutput bruteOutput = new RayCastOutput();
#endif
            for (int i = 0; i < ActorCount; ++i)
            {
                if (_actors[i].ProxyId == -1)
                {
                    continue;
                }

                RayCastOutput output;
                bool          hit = _actors[i].AABB.RayCast(out output, ref input);
                if (hit)
                {
                    bruteActor = _actors[i];
#if DEBUG
                    bruteOutput = output;
#endif
                    input.MaxFraction = output.Fraction;
                }
            }

            if (bruteActor != null)
            {
#if DEBUG
                Debug.Assert(bruteOutput.Fraction == _rayCastOutput.Fraction);
#endif
            }
        }
Esempio n. 7
0
        /// <summary>
        ///     Describes whether ray cast circle
        /// </summary>
        /// <param name="pos">The pos</param>
        /// <param name="radius">The radius</param>
        /// <param name="input">The input</param>
        /// <param name="transform">The transform</param>
        /// <param name="output">The output</param>
        /// <returns>The bool</returns>
        public static bool RayCastCircle(ref Vector2 pos, float radius, ref RayCastInput input, ref Transform transform,
                                         out RayCastOutput output)
        {
            // Collision Detection in Interactive 3D Environments by Gino van den Bergen
            // From Section 3.1.2
            // x = s + a * r
            // norm(x) = radius

            output = new RayCastOutput();

            Vector2 position = transform.P + MathUtils.Mul(transform.Q, pos);
            Vector2 s        = input.Point1 - position;
            float   b        = Vector2.Dot(s, s) - radius * radius;

            // Solve quadratic equation.
            Vector2 r     = input.Point2 - input.Point1;
            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 < MathConstants.Epsilon)
            {
                return(false);
            }

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

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

            return(false);
        }
Esempio n. 8
0
        private void RayCast()
        {
            _rayActor = null;

            RayCastInput input = _rayCastInput;

            // Ray cast against the dynamic tree.
            _tree.RayCast(RayCastCallback, ref input);

            // Brute force ray cast.
            Actor         bruteActor  = null;
            RayCastOutput bruteOutput = new RayCastOutput();

            for (int i = 0; i < ActorCount; ++i)
            {
                if (_actors[i].ProxyId == -1)
                {
                    continue;
                }

                bool hit = _actors[i].AABB.RayCast(ref input, out RayCastOutput output);
Esempio n. 9
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(RayCastOutput output, RayCastInput input, Transform transform, int childIndex)
        {
            Vec2 position = pool1;
            Vec2 s        = pool2;
            Vec2 r        = pool3;

            Rot.MulToOutUnsafe(transform.Q, P, position);
            position.AddLocal(transform.P);
            s.Set(input.P1).SubLocal(position);
            float b = Vec2.Dot(s, s) - Radius * Radius;

            // Solve quadratic equation.
            r.Set(input.P2).SubLocal(input.P1);
            float c     = Vec2.Dot(s, r);
            float rr    = Vec2.Dot(r, r);
            float sigma = c * c - rr * b;

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

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

            // Is the intersection point on the segment?
            if (0.0f <= a && a <= input.MaxFraction * rr)
            {
                a /= rr;
                output.Fraction = a;
                output.Normal.Set(r).MulLocal(a);
                output.Normal.AddLocal(s);
                output.Normal.Normalize();
                return(true);
            }

            return(false);
        }
Esempio n. 10
0
        public override bool raycast(RayCastOutput output, RayCastInput input, Transform xf, int childIndex)
        {
            EdgeShape edgeShape = pool0;

            int i1 = childIndex;
            int i2 = childIndex + 1;

            if (i2 == m_count)
            {
                i2 = 0;
            }
            Vec2 v = m_vertices[i1];

            edgeShape.m_vertex1.x = v.x;
            edgeShape.m_vertex1.y = v.y;
            Vec2 v1 = m_vertices[i2];

            edgeShape.m_vertex2.x = v1.x;
            edgeShape.m_vertex2.y = v1.y;

            return(edgeShape.raycast(output, input, xf, 0));
        }
Esempio n. 11
0
        // for pooling


        public override bool raycast(RayCastOutput output, RayCastInput input, Transform xf, int childIndex)
        {
            double tempx, tempy;
            Vec2   v1  = m_vertex1;
            Vec2   v2  = m_vertex2;
            Rot    xfq = xf.q;
            Vec2   xfp = xf.p;

            // Put the ray into the edge's frame of reference.
            //b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p);
            //b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p);
            tempx = input.p1.x - xfp.x;
            tempy = input.p1.y - xfp.y;
            double p1x = xfq.c * tempx + xfq.s * tempy;
            double p1y = -xfq.s * tempx + xfq.c * tempy;

            tempx = input.p2.x - xfp.x;
            tempy = input.p2.y - xfp.y;
            double p2x = xfq.c * tempx + xfq.s * tempy;
            double p2y = -xfq.s * tempx + xfq.c * tempy;

            double dx = p2x - p1x;
            double dy = p2y - p1y;

            // Vec2 normal = pool2.set(v2).subLocal(v1);
            // normal.set(normal.y, -normal.x);
            normal.x = v2.y - v1.y;
            normal.y = v1.x - v2.x;
            normal.normalize();
            double normalx = normal.x;
            double normaly = normal.y;

            // q = p1 + t * d
            // dot(normal, q - v1) = 0
            // dot(normal, p1 - v1) + t * dot(normal, d) = 0
            tempx = v1.x - p1x;
            tempy = v1.y - p1y;
            double numerator   = normalx * tempx + normaly * tempy;
            double denominator = normalx * dx + normaly * dy;

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

            double t = numerator / denominator;

            if (t < 0.0d || 1.0d < t)
            {
                return(false);
            }

            // Vec2 q = p1 + t * d;
            double qx = p1x + t * dx;
            double qy = p1y + t * dy;

            // q = v1 + s * r
            // s = dot(q - v1, r) / dot(r, r)
            // Vec2 r = v2 - v1;
            double rx = v2.x - v1.x;
            double ry = v2.y - v1.y;
            double rr = rx * rx + ry * ry;

            if (rr == 0.0d)
            {
                return(false);
            }
            tempx = qx - v1.x;
            tempy = qy - v1.y;
            // double s = Vec2.dot(pool5, r) / rr;
            double s = (tempx * rx + tempy * ry) / rr;

            if (s < 0.0d || 1.0d < s)
            {
                return(false);
            }

            output.fraction = t;
            if (numerator > 0.0d)
            {
                // argOutput.normal = -normal;
                output.normal.x = -normalx;
                output.normal.y = -normaly;
            }
            else
            {
                // output.normal = normal;
                output.normal.x = normalx;
                output.normal.y = normaly;
            }
            return(true);
        }
Esempio n. 12
0
 public override bool RayCast(ref RayCastInput input, ref Transform transform, int childIndex, out RayCastOutput output)
 {
     return(RayCastHelper.RayCastCircle(ref _position, Radius, ref input, ref transform, out output));
 }
Esempio n. 13
0
        /// <summary>
        /// Raycast against this AABB using the specified points and maxfraction (found in input)
        /// </summary>
        /// <param name="output">The results of the raycast.</param>
        /// <param name="input">The parameters for the raycast.</param>
        /// <returns>True if the ray intersects the AABB</returns>
        public bool RayCast(out RayCastOutput output, ref RayCastInput input, bool doInteriorCheck = true)
        {
            // From Real-time Collision Detection, p179.

            output = new RayCastOutput();

            var tmin = -Settings.MaxFloat;
            var tmax = Settings.MaxFloat;

            var p    = input.Point1;
            var d    = input.Point2 - input.Point1;
            var absD = MathUtils.Abs(d);

            var normal = Vector2.zero;

            for (var i = 0; i < 2; ++i)
            {
                var absD_i       = i == 0 ? absD.x : absD.y;
                var lowerBound_i = i == 0 ? LowerBound.x : LowerBound.y;
                var upperBound_i = i == 0 ? UpperBound.x : UpperBound.y;
                var p_i          = i == 0 ? p.x : p.y;

                if (absD_i < Settings.Epsilon)
                {
                    // Parallel.
                    if (p_i < lowerBound_i || upperBound_i < p_i)
                    {
                        return(false);
                    }
                }
                else
                {
                    var d_i = i == 0 ? d.x : d.y;

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

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

                    if (t1 > t2)
                    {
                        MathUtils.Swap(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 = Mathf.Min(tmax, t2);

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

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

            // Intersection.
            output.Fraction = tmin;
            output.Normal   = normal;
            return(true);
        }
Esempio n. 14
0
        public override bool RayCast(ref RayCastInput input, ref Transform transform, int childIndex, out RayCastOutput output)
        {
            Debug.Assert(childIndex < Vertices.Count);

            int i1 = childIndex;
            int i2 = childIndex + 1;

            if (i2 == Vertices.Count)
            {
                i2 = 0;
            }

            Vector2 v1 = Vertices[i1];
            Vector2 v2 = Vertices[i2];

            return(RayCastHelper.RayCastEdge(ref v1, ref v2, ref input, ref transform, out output));
        }
Esempio n. 15
0
    /// <summary>
    /// Raycast against this AABB using the specified points and maxfraction (found in input)
    /// </summary>
    /// <param name="output">The results of the raycast.</param>
    /// <param name="input">The parameters for the raycast.</param>
    /// <returns>True if the ray intersects the AABB</returns>
    public bool RayCast(out RayCastOutput output, ref RayCastInput input, bool doInteriorCheck = true)
    {
        // From Real-time Collision Detection, p179.

        output = new RayCastOutput();

        var tmin = -long.MaxValue;
        var tmax = long.MaxValue;

        var p    = input.Point1;
        var d    = input.Point2 - input.Point1;
        var absD = Vector2d.Abs(d);

        var normal = new Vector2d(0, 0);

        for (int i = 0; i < 2; ++i)
        {
            long absD_i       = i == 0 ? absD.x : absD.y;
            long lowerBound_i = i == 0 ? LowerBound.x : LowerBound.y;
            long upperBound_i = i == 0 ? UpperBound.x : UpperBound.y;
            long p_i          = i == 0 ? p.x : p.y;

            if (absD_i < 1)
            {
                // Parallel.
                if (p_i < lowerBound_i || upperBound_i < p_i)
                {
                    return(false);
                }
            }
            else
            {
                long d_i = i == 0 ? d.x : d.y;

                long inv_d = FixedMath.One.Div(d_i);
                long t1    = (lowerBound_i - p_i).Mul(inv_d);
                long t2    = (upperBound_i - p_i).Mul(inv_d);

                // Sign of the normal vector.
                long s = -FixedMath.One;

                if (t1 > t2)
                {
                    Utility.Swap(ref t1, ref t2);
                    s = FixedMath.One;
                }

                // 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 (doInteriorCheck && (tmin < 0 || input.MaxFraction < tmin))
        {
            return(false);
        }

        // Intersection.
        output.Fraction = tmin;
        output.Normal   = normal;
        return(true);
    }
Esempio n. 16
0
		public override bool rayCast( out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex )
		{
			// p = p1 + t * d
			// v = v1 + s * e
			// p1 + t * d = v1 + s * e
			// s * e - t * d = p1 - v1

			output = new RayCastOutput();

			// Put the ray into the edge's frame of reference.
			var p1 = MathUtils.mulT( transform.q, input.Point1 - transform.p );
			var p2 = MathUtils.mulT( transform.q, input.Point2 - transform.p );
			var d = p2 - p1;

			var v1 = _vertex1;
			var v2 = _vertex2;
			var e = v2 - v1;
			var normal = new Vector2( e.Y, -e.X ); //TODO: Could possibly cache the normal.
			normal.Normalize();

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

			if( denominator == 0.0f )
				return false;

			float t = numerator / denominator;
			if( t < 0.0f || input.MaxFraction < t )
				return false;

			var q = p1 + t * d;

			// q = v1 + s * r
			// s = dot(q - v1, r) / dot(r, r)
			var r = v2 - v1;
			var 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;
		}
Esempio n. 17
0
        /// <summary>Raycast against this AABB using the specified points and maxfraction (found in input)</summary>
        /// <param name="input">The parameters for the raycast.</param>
        /// <param name="output">The results of the raycast.</param>
        /// <param name="doInteriorCheck"></param>
        /// <returns>True if the ray intersects the AABB</returns>
        public bool RayCast(ref RayCastInput input, out RayCastOutput output, bool doInteriorCheck = true)
        {
            // From Real-time Collision Detection, p179.

            output = new RayCastOutput();

            float tmin = -MathConstants.MaxFloat;
            float tmax = MathConstants.MaxFloat;

            Vector2 p    = input.Point1;
            Vector2 d    = input.Point2 - input.Point1;
            Vector2 absD = MathUtils.Abs(d);

            Vector2 normal = Vector2.Zero;

            for (int i = 0; i < 2; ++i)
            {
                float absDI       = i == 0 ? absD.X : absD.Y;
                float lowerBoundI = i == 0 ? LowerBound.X : LowerBound.Y;
                float upperBoundI = i == 0 ? UpperBound.X : UpperBound.Y;
                float pI          = i == 0 ? p.X : p.Y;

                if (absDI < MathConstants.Epsilon)
                {
                    // Parallel.
                    if (pI < lowerBoundI || upperBoundI < pI)
                    {
                        return(false);
                    }
                }
                else
                {
                    float dI = i == 0 ? d.X : d.Y;

                    float invD = 1.0f / dI;
                    float t1   = (lowerBoundI - pI) * invD;
                    float t2   = (upperBoundI - pI) * invD;

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

                    if (t1 > t2)
                    {
                        MathUtils.Swap(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 (doInteriorCheck && (tmin < 0.0f || input.MaxFraction < tmin))
            {
                return(false);
            }

            // Intersection.
            output.Fraction = tmin;
            output.Normal   = normal;
            return(true);
        }
Esempio n. 18
0
        public override bool Raycast(RayCastOutput output, RayCastInput input, Transform xf, int childIndex)
        {
            Vec2 p1 = pool1;
            Vec2 p2 = pool2;
            Vec2 d = pool3;
            Vec2 temp = pool4;

            p1.Set(input.P1).SubLocal(xf.P);
            Rot.MulTrans(xf.Q, p1, p1);
            p2.Set(input.P2).SubLocal(xf.P);
            Rot.MulTrans(xf.Q, p2, p2);
            d.Set(p2).SubLocal(p1);

            // if (count == 2) {

            // } else {

            float lower = 0, 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
                temp.Set(Vertices[i]).SubLocal(p1);
                float numerator = Vec2.Dot(Normals[i], temp);
                float denominator = Vec2.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)
                {
                    return false;
                }
            }

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

            if (index >= 0)
            {
                output.Fraction = lower;
                Rot.MulToOutUnsafe(xf.Q, Normals[index], output.Normal);
                // normal = Mul(xf.R, m_normals[index]);
                return true;
            }
            return false;
        }
Esempio n. 19
0
        public override bool raycast(RayCastOutput output, RayCastInput input, Transform xf, int childIndex)
        {
            float xfqc = xf.q.c;
            float xfqs = xf.q.s;
            Vec2 xfp = xf.p;
            float tempx, tempy;
            // b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p);
            // b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p);
            tempx = input.p1.x - xfp.x;
            tempy = input.p1.y - xfp.y;
            float p1x = xfqc*tempx + xfqs*tempy;
            float p1y = -xfqs*tempx + xfqc*tempy;

            tempx = input.p2.x - xfp.x;
            tempy = input.p2.y - xfp.y;
            float p2x = xfqc*tempx + xfqs*tempy;
            float p2y = -xfqs*tempx + xfqc*tempy;

            float dx = p2x - p1x;
            float dy = p2y - p1y;

            float lower = 0, upper = input.maxFraction;

            int index = -1;

            for (int i = 0; i < m_count; ++i)
            {
                Vec2 normal = m_normals[i];
                Vec2 vertex = m_vertices[i];
                // p = p1 + a * d
                // dot(normal, p - v) = 0
                // dot(normal, p1 - v) + a * dot(normal, d) = 0
                float tempxn = vertex.x - p1x;
                float tempyn = vertex.y - p1y;
                float numerator = normal.x*tempxn + normal.y*tempyn;
                float denominator = normal.x*dx + normal.y*dy;

                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)
                {
                    return false;
                }
            }

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

            if (index >= 0)
            {
                output.fraction = lower;
                // normal = Mul(xf.R, m_normals[index]);
                Vec2 normal = m_normals[index];
                Vec2 outputNormal = output.normal;
                outputNormal.x = xfqc*normal.x - xfqs*normal.y;
                outputNormal.y = xfqs*normal.x + xfqc*normal.y;

                output.normal = outputNormal;
                return true;
            }
            return false;
        }
Esempio n. 20
0
        public override bool Raycast(RayCastOutput output, RayCastInput input, Transform xf, int childIndex)
        {
            Vec2 p1   = pool1;
            Vec2 p2   = pool2;
            Vec2 d    = pool3;
            Vec2 temp = pool4;

            p1.Set(input.P1).SubLocal(xf.P);
            Rot.MulTrans(xf.Q, p1, p1);
            p2.Set(input.P2).SubLocal(xf.P);
            Rot.MulTrans(xf.Q, p2, p2);
            d.Set(p2).SubLocal(p1);

            // if (count == 2) {

            // } else {

            float lower = 0, 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
                temp.Set(Vertices[i]).SubLocal(p1);
                float numerator   = Vec2.Dot(Normals[i], temp);
                float denominator = Vec2.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)
                {
                    return(false);
                }
            }

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

            if (index >= 0)
            {
                output.Fraction = lower;
                Rot.MulToOutUnsafe(xf.Q, Normals[index], output.Normal);
                // normal = Mul(xf.R, m_normals[index]);
                return(true);
            }
            return(false);
        }
Esempio n. 21
0
        // p = p1 + t * d
        // v = v1 + s * e
        // p1 + t * d = v1 + s * e
        // s * e - t * d = p1 - v1
        public override bool raycast(RayCastOutput output, RayCastInput input, Transform xf, int childIndex)
        {
            float tempx, tempy;
            Vec2 v1 = m_vertex1;
            Vec2 v2 = m_vertex2;
            Rot xfq = xf.q;
            Vec2 xfp = xf.p;

            // Put the ray into the edge's frame of reference.
            // b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p);
            // b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p);
            tempx = input.p1.x - xfp.x;
            tempy = input.p1.y - xfp.y;
            float p1x = xfq.c*tempx + xfq.s*tempy;
            float p1y = -xfq.s*tempx + xfq.c*tempy;

            tempx = input.p2.x - xfp.x;
            tempy = input.p2.y - xfp.y;
            float p2x = xfq.c*tempx + xfq.s*tempy;
            float p2y = -xfq.s*tempx + xfq.c*tempy;

            float dx = p2x - p1x;
            float dy = p2y - p1y;

            //  Vec2 normal = pool2.set(v2).subLocal(v1);
            // normal.set(normal.y, -normal.x);
            normal.x = v2.y - v1.y;
            normal.y = v1.x - v2.x;
            normal.normalize();
            float normalx = normal.x;
            float normaly = normal.y;

            // q = p1 + t * d
            // dot(normal, q - v1) = 0
            // dot(normal, p1 - v1) + t * dot(normal, d) = 0
            tempx = v1.x - p1x;
            tempy = v1.y - p1y;
            float numerator = normalx*tempx + normaly*tempy;
            float denominator = normalx*dx + normaly*dy;

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

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

            // Vec2 q = p1 + t * d;
            float qx = p1x + t*dx;
            float qy = p1y + t*dy;

            // q = v1 + s * r
            // s = dot(q - v1, r) / dot(r, r)
            // Vec2 r = v2 - v1;
            float rx = v2.x - v1.x;
            float ry = v2.y - v1.y;
            float rr = rx*rx + ry*ry;
            if (rr == 0.0f)
            {
                return false;
            }
            tempx = qx - v1.x;
            tempy = qy - v1.y;
            // float s = Vec2.dot(pool5, r) / rr;
            float s = (tempx*rx + tempy*ry)/rr;
            if (s < 0.0f || 1.0f < s)
            {
                return false;
            }

            output.fraction = t;
            if (numerator > 0.0f)
            {
                // output.normal = -b2Mul(xf.q, normal);
                output.normal.x = -xfq.c*normal.x + xfq.s*normal.y;
                output.normal.y = -xfq.s*normal.x - xfq.c*normal.y;
            }
            else
            {
                // output->normal = b2Mul(xf.q, normal);
                output.normal.x = xfq.c*normal.x - xfq.s*normal.y;
                output.normal.y = xfq.s*normal.x + xfq.c*normal.y;
            }
            return true;
        }
Esempio n. 22
0
 /// <summary>
 /// Cast a ray against this shape.
 /// </summary>
 /// <param name="output">the ray-cast results.</param>
 /// <param name="input">the ray-cast input parameters.</param>
 /// <param name="childIndex"></param>
 public bool Raycast(RayCastOutput output, RayCastInput input, int childIndex)
 {
     return(Shape.Raycast(output, input, Body.Xf, childIndex));
 }
Esempio n. 23
0
 public override bool RayCast(ref RayCastInput input, ref Transform transform, int childIndex, out RayCastOutput output)
 {
     return(RayCastHelper.RayCastEdge(ref _vertex1, ref _vertex2, ref input, ref transform, out output));
 }
Esempio n. 24
0
 /// <summary>
 /// Cast a ray against a child shape.
 /// </summary>
 /// <param name="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>
 /// <param name="childIndex">The child shape index.</param>
 /// <returns>True if the ray-cast hits the shape</returns>
 public abstract bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex);
Esempio n. 25
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(RayCastOutput output, RayCastInput input, Transform transform, int childIndex)
        {
            Vec2 position = pool1;
            Vec2 s = pool2;
            Vec2 r = pool3;

            Rot.MulToOutUnsafe(transform.Q, P, position);
            position.AddLocal(transform.P);
            s.Set(input.P1).SubLocal(position);
            float b = Vec2.Dot(s, s) - Radius * Radius;

            // Solve quadratic equation.
            r.Set(input.P2).SubLocal(input.P1);
            float c = Vec2.Dot(s, r);
            float rr = Vec2.Dot(r, r);
            float sigma = c * c - rr * b;

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

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

            // Is the intersection point on the segment?
            if (0.0f <= a && a <= input.MaxFraction * rr)
            {
                a /= rr;
                output.Fraction = a;
                output.Normal.Set(r).MulLocal(a);
                output.Normal.AddLocal(s);
                output.Normal.Normalize();
                return true;
            }

            return false;
        }
Esempio n. 26
0
        public override bool Raycast(RayCastOutput output, RayCastInput input, Transform xf, int childIndex)
        {
            // Put the ray into the edge's frame of reference.
            Vec2 p1 = pool0.Set(input.P1).SubLocal(xf.P);

            Rot.MulTrans(xf.Q, p1, p1);
            Vec2 p2 = pool1.Set(input.P2).SubLocal(xf.P);

            Rot.MulTrans(xf.Q, p1, p1);
            Vec2 d = p2.SubLocal(p1); // we don't use p2 later

            Vec2 v1     = Vertex1;
            Vec2 v2     = Vertex2;
            Vec2 normal = pool2.Set(v2).SubLocal(v1);

            normal.Set(normal.Y, -normal.X);
            normal.Normalize();

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

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

            float t = numerator / denominator;

            if (t < 0.0f || 1.0f < t)
            {
                return(false);
            }

            Vec2 q = pool3;
            Vec2 r = pool4;

            // Vec2 q = p1 + t * d;
            q.Set(d).MulLocal(t).AddLocal(p1);

            // q = v1 + s * r
            // s = dot(q - v1, r) / dot(r, r)
            // Vec2 r = v2 - v1;
            r.Set(v2).SubLocal(v1);
            float rr = Vec2.Dot(r, r);

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

            pool5.Set(q).SubLocal(v1);
            float s = Vec2.Dot(pool5, r) / rr;

            if (s < 0.0f || 1.0f < s)
            {
                return(false);
            }

            output.Fraction = t;
            if (numerator > 0.0f)
            {
                // argOutput.normal = -normal;
                output.Normal.Set(normal).NegateLocal();
            }
            else
            {
                // output.normal = normal;
                output.Normal.Set(normal);
            }
            return(true);
        }
Esempio n. 27
0
        /**
         * Cast a ray against this shape.
         *
         * @param output the ray-cast results.
         * @param input the ray-cast input parameters.
         * @param output
         * @param input
         */

        public bool raycast(RayCastOutput output, RayCastInput input, int childIndex)
        {
            return(m_shape.raycast(output, input, m_body.m_xf, childIndex));
        }
 /// <summary>
 /// Cast a ray against a child shape.
 /// </summary>
 /// <param name="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>
 /// <param name="childIndex">The child shape index.</param>
 /// <returns>True if the ray-cast hits the shape</returns>
 public abstract bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform,
     int childIndex);
Esempio n. 29
0
        public static bool RayCastEdge(ref Vector2 start, ref Vector2 end, bool oneSided, ref RayCastInput input, ref Transform transform, out RayCastOutput output)
        {
            // p = p1 + t * d
            // v = v1 + s * e
            // p1 + t * d = v1 + s * e
            // s * e - t * d = p1 - v1

            output = new RayCastOutput();

            // Put the ray into the edge's frame of reference.
            Vector2 p1 = MathUtils.MulT(transform.q, input.Point1 - transform.p);
            Vector2 p2 = MathUtils.MulT(transform.q, input.Point2 - transform.p);
            Vector2 d  = p2 - p1;

            Vector2 v1 = start;
            Vector2 v2 = end;
            Vector2 e  = v2 - v1;

            // Normal points to the right, looking from v1 at v2
            Vector2 normal = new Vector2(e.Y, -e.X); //Velcro TODO: Could possibly cache the normal.

            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);

            if (oneSided && numerator > 0.0f)
            {
                return(false);
            }

            float denominator = Vector2.Dot(normal, d);

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

            float t = numerator / denominator;

            if (t < 0.0f || input.MaxFraction < 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 = -MathUtils.MulT(transform.q, normal);
            }
            else
            {
                output.Normal = MathUtils.MulT(transform.q, normal);
            }
            return(true);
        }
Esempio n. 30
0
        public override bool Raycast(RayCastOutput output, RayCastInput input, Transform xf, int childIndex)
        {
            Debug.Assert(childIndex < Count);

            EdgeShape edgeShape = pool0;

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

            edgeShape.Vertex1.Set(Vertices[i1]);
            edgeShape.Vertex2.Set(Vertices[i2]);

            return edgeShape.Raycast(output, input, xf, 0);
        }
Esempio n. 31
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(RayCastOutput output, RayCastInput input, Transform transform,
            int childIndex)
        {
            Vec2 inputp1 = input.p1;
            Vec2 inputp2 = input.p2;
            Rot tq = transform.q;
            Vec2 tp = transform.p;

            // Rot.mulToOutUnsafe(transform.q, m_p, position);
            // position.addLocal(transform.p);
            float positionx = tq.c*m_p.x - tq.s*m_p.y + tp.x;
            float positiony = tq.s*m_p.x + tq.c*m_p.y + tp.y;

            float sx = inputp1.x - positionx;
            float sy = inputp1.y - positiony;
            //  float b = Vec2.dot(s, s) - m_radius * m_radius;
            float b = sx*sx + sy*sy - m_radius*m_radius;

            // Solve quadratic equation.
            float rx = inputp2.x - inputp1.x;
            float ry = inputp2.y - inputp1.y;
            //  float c = Vec2.dot(s, r);
            //  float rr = Vec2.dot(r, r);
            float c = sx*rx + sy*ry;
            float rr = rx*rx + ry*ry;
            float sigma = c*c - rr*b;

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

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

            // Is the intersection point on the segment?
            if (0.0f <= a && a <= input.maxFraction*rr)
            {
                a /= rr;
                output.fraction = a;
                output.normal.x = rx*a + sx;
                output.normal.y = ry*a + sy;
                output.normal.normalize();
                return true;
            }

            return false;
        }
Esempio n. 32
0
        /// <summary>
        /// Raycast against this AABB using the specified points and maxfraction (found in input)
        /// </summary>
        /// <param name="output">The results of the raycast.</param>
        /// <param name="input">The parameters for the raycast.</param>
        /// <returns>True if the ray intersects the AABB</returns>
        public bool RayCast(out RayCastOutput output, ref RayCastInput input, bool doInteriorCheck = true)
        {
            // From Real-time Collision Detection, p179.

            output = new RayCastOutput();

            GGame.Math.Fix64 tmin = -Settings.MaxFloat;
            GGame.Math.Fix64 tmax = Settings.MaxFloat;

            Vector2 p    = input.Point1;
            Vector2 d    = input.Point2 - input.Point1;
            Vector2 absD = MathUtils.Abs(d);

            Vector2 normal = Vector2.Zero;

            for (int i = 0; i < 2; ++i)
            {
                GGame.Math.Fix64 absD_i       = i == 0 ? absD.X : absD.Y;
                GGame.Math.Fix64 lowerBound_i = i == 0 ? LowerBound.X : LowerBound.Y;
                GGame.Math.Fix64 upperBound_i = i == 0 ? UpperBound.X : UpperBound.Y;
                GGame.Math.Fix64 p_i          = i == 0 ? p.X : p.Y;

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

                    GGame.Math.Fix64 inv_d = 1.0f / d_i;
                    GGame.Math.Fix64 t1    = (lowerBound_i - p_i) * inv_d;
                    GGame.Math.Fix64 t2    = (upperBound_i - p_i) * inv_d;

                    // Sign of the normal vector.
                    GGame.Math.Fix64 s = -1.0f;

                    if (t1 > t2)
                    {
                        MathUtils.Swap(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((float)tmax, (float)t2);

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

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

            // Intersection.
            output.Fraction = tmin;
            output.Normal   = normal;
            return(true);
        }
Esempio n. 33
0
 /// <summary>
 /// Cast a ray against this Fixture by passing the call through to the Shape
 /// </summary>
 /// <param name="output">The ray-cast results.</param>
 /// <param name="input">The ray-cast input parameters.</param>
 /// <param name="childIndex">Index of the child.</param>
 /// <returns></returns>
 public bool rayCast(out RayCastOutput output, ref RayCastInput input, int childIndex)
 {
     return(shape.rayCast(out output, ref input, ref body._xf, childIndex));
 }
Esempio n. 34
0
        public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex)
        {
            // p = p1 + t * d
            // v = v1 + s * e
            // p1 + t * d = v1 + s * e
            // s * e - t * d = p1 - v1

            output = new RayCastOutput();

            // Put the ray into the edge's frame of reference.
            Vector2 p1 = MathUtils.MulT(transform.q, input.Point1 - transform.p);
            Vector2 p2 = MathUtils.MulT(transform.q, input.Point2 - transform.p);
            Vector2 d  = p2 - p1;

            Vector2 v1     = _vertex1;
            Vector2 v2     = _vertex2;
            Vector2 e      = v2 - v1;
            Vector2 normal = new Vector2(e.Y, -e.X); //TODO: Could possibly cache the normal.

            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 || input.MaxFraction < 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);
        }
Esempio n. 35
0
        /// <summary>
        /// Cast a ray against a child shape.
        /// </summary>
        /// <param name="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>
        /// <param name="childIndex">The child shape index.</param>
        /// <returns>True if the ray-cast hits the shape</returns>
        public override bool RayCast(out RayCastOutput output, ref RayCastInput input,
                                     ref Transform transform, int childIndex)
        {
            // p = p1 + t * d
            // v = v1 + s * e
            // p1 + t * d = v1 + s * e
            // s * e - t * d = p1 - v1

            output = new RayCastOutput();

            // Put the ray into the edge's frame of reference.
            Vector2 p1 = MathUtils.MultiplyT(ref transform.R, input.Point1 - transform.Position);
            Vector2 p2 = MathUtils.MultiplyT(ref transform.R, input.Point2 - 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;
        }
Esempio n. 36
0
        public override bool raycast(RayCastOutput output, RayCastInput input, Transform xf, int childIndex)
        {
            Debug.Assert(childIndex < m_count);

            EdgeShape edgeShape = pool0;

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

            edgeShape.m_vertex1.set_Renamed(m_vertices[i1]);
            edgeShape.m_vertex2.set_Renamed(m_vertices[i2]);

            return edgeShape.raycast(output, input, xf, 0);
        }
Esempio n. 37
0
 public override bool RayCast(ref RayCastInput input, ref Transform transform, int childIndex, out RayCastOutput output)
 {
     return(RayCastHelper.RayCastPolygon(_vertices, _normals, ref input, ref transform, out output));
 }
Esempio n. 38
0
        public override bool raycast(RayCastOutput output, RayCastInput input, Transform xf, int childIndex)
        {
            Debug.Assert(childIndex < m_count);

            EdgeShape edgeShape = pool0;

            int i1 = childIndex;
            int i2 = childIndex + 1;
            if (i2 == m_count)
            {
                i2 = 0;
            }
            Vec2 v = m_vertices[i1];
            edgeShape.m_vertex1.x = v.x;
            edgeShape.m_vertex1.y = v.y;
            Vec2 v1 = m_vertices[i2];
            edgeShape.m_vertex2.x = v1.x;
            edgeShape.m_vertex2.y = v1.y;

            return edgeShape.raycast(output, input, xf, 0);
        }
Esempio n. 39
0
        float RayCastCallback(ref RayCastInput input, int proxyid)
        {
            Actor actor = (Actor)_tree.GetUserData(proxyid);

            RayCastOutput output;
            bool hit = actor.aabb.RayCast(out output, ref input);

            if (hit)
            {
                _rayCastOutput = output;
                actor.fraction = output.fraction;
                _rayActor = actor;

                return output.fraction;
            }

            return input.maxFraction;
        }
Esempio n. 40
0
        public override bool raycast(RayCastOutput output, RayCastInput input, Transform xf, int childIndex)
        {
            // Put the ray into the edge's frame of reference.
            Vec2 p1 = pool0.set_Renamed(input.p1).subLocal(xf.p);
            Rot.mulTrans(xf.q, p1, p1);
            Vec2 p2 = pool1.set_Renamed(input.p2).subLocal(xf.p);
            Rot.mulTrans(xf.q, p1, p1);
            Vec2 d = p2.subLocal(p1); // we don't use p2 later

            Vec2 v1 = m_vertex1;
            Vec2 v2 = m_vertex2;
            Vec2 normal = pool2.set_Renamed(v2).subLocal(v1);
            normal.set_Renamed(normal.y, -normal.x);
            normal.normalize();

            // q = p1 + t * d
            // dot(normal, q - v1) = 0
            // dot(normal, p1 - v1) + t * dot(normal, d) = 0
            pool3.set_Renamed(v1).subLocal(p1);
            float numerator = Vec2.dot(normal, pool3);
            float denominator = Vec2.dot(normal, d);

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

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

            Vec2 q = pool3;
            Vec2 r = pool4;

            // Vec2 q = p1 + t * d;
            q.set_Renamed(d).mulLocal(t).addLocal(p1);

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

            pool5.set_Renamed(q).subLocal(v1);
            float s = Vec2.dot(pool5, r) / rr;
            if (s < 0.0f || 1.0f < s)
            {
                return false;
            }

            output.fraction = t;
            if (numerator > 0.0f)
            {
                // argOutput.normal = -normal;
                output.normal.set_Renamed(normal).negateLocal();
            }
            else
            {
                // output.normal = normal;
                output.normal.set_Renamed(normal);
            }
            return true;
        }
Esempio n. 41
0
        /// <summary>
        /// Cast a ray against a child shape.
        /// </summary>
        /// <param name="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>
        /// <param name="childIndex">The child shape index.</param>
        /// <returns>True if the ray-cast hits the shape</returns>
        public override bool RayCast(out RayCastOutput output, ref RayCastInput input,
                                     ref Transform transform, int childIndex)
        {
            //Debug.Assert(childIndex < Vertices.Count);

            int i1 = childIndex;
            int i2 = childIndex + 1;
            if (i2 == Vertices.Count)
            {
                i2 = 0;
            }

            _edgeShape.Vertex1 = Vertices[i1];
            _edgeShape.Vertex2 = Vertices[i2];

            return _edgeShape.RayCast(out output, ref input, ref transform, 0);
        }
Esempio n. 42
0
 public override bool RayCast(out FarseerPhysics.Collision.RayCastOutput output, ref FarseerPhysics.Collision.RayCastInput input, ref FarseerPhysics.Common.Transform transform, int childIndex)
 {
     output = new RayCastOutput();
     return false;
 }
Esempio n. 43
0
        public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex)
        {
            output = new RayCastOutput();

            // Put the ray into the polygon's frame of reference.
            Vector2 p1 = MathUtils.MulT(transform.q, input.Point1 - transform.p);
            Vector2 p2 = MathUtils.MulT(transform.q, input.Point2 - transform.p);
            Vector2 d = p2 - p1;

            float lower = 0.0f, upper = input.MaxFraction;

            int index = -1;

            for (int i = 0; i < Vertices.Count; ++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.Mul(transform.q, Normals[index]);
                return true;
            }

            return false;
        }
Esempio n. 44
0
        public override bool Raycast(RayCastOutput output, RayCastInput input, Transform xf, int childIndex)
        {
            // Put the ray into the edge's frame of reference.
            Vec2 p1 = pool0.Set(input.P1).SubLocal(xf.P);
            Rot.MulTrans(xf.Q, p1, p1);
            Vec2 p2 = pool1.Set(input.P2).SubLocal(xf.P);
            Rot.MulTrans(xf.Q, p1, p1);
            Vec2 d = p2.SubLocal(p1); // we don't use p2 later

            Vec2 v1 = Vertex1;
            Vec2 v2 = Vertex2;
            Vec2 normal = pool2.Set(v2).SubLocal(v1);
            normal.Set(normal.Y, -normal.X);
            normal.Normalize();

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

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

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

            Vec2 q = pool3;
            Vec2 r = pool4;

            // Vec2 q = p1 + t * d;
            q.Set(d).MulLocal(t).AddLocal(p1);

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

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

            output.Fraction = t;
            if (numerator > 0.0f)
            {
                // argOutput.normal = -normal;
                output.Normal.Set(normal).NegateLocal();
            }
            else
            {
                // output.normal = normal;
                output.Normal.Set(normal);
            }
            return true;
        }
Esempio n. 45
0
 /// <summary>
 /// Cast a ray against this Shape.
 /// </summary>
 /// <param name="output">The ray-cast results.</param>
 /// <param name="input">The ray-cast input parameters.</param>
 /// <param name="childIndex">Index of the child.</param>
 /// <returns></returns>
 public bool RayCast(out RayCastOutput output, ref RayCastInput input, int childIndex)
 {
     return(Shape.RayCast(out output, ref input, ref Body.Xf, childIndex));
 }
Esempio n. 46
0
 /// <summary>
 /// Cast a ray against a child shape.
 /// </summary>
 /// <param name="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>
 /// <param name="childIndex">the child shape index</param>
 /// <returns>if hit</returns>
 public abstract bool Raycast(RayCastOutput output, RayCastInput input, Transform transform, int childIndex);
Esempio n. 47
0
        public bool RayCast(out RayCastOutput output, RayCastInput input)
        {
            output = default;
            var tmin = -Settings.MaxFloat;
            var tmax = Settings.MaxFloat;

            var p    = input.P1;
            var d    = input.P2 - input.P1;
            var absD = Vector2.Abs(d);

            var normal = new Vector2();

            {
                if (absD.X < Settings.Epsilon)
                {
                    // Parallel.
                    if (p.X < LowerBound.X || UpperBound.X < p.X)
                    {
                        return(false);
                    }
                }
                else
                {
                    var invD = 1.0f / d.X;
                    var t1   = (LowerBound.X - p.X) * invD;
                    var t2   = (UpperBound.X - p.X) * invD;

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

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

                    // Push the min up
                    if (t1 > tmin)
                    {
                        normal.SetZero();
                        normal.X = s;
                        tmin     = t1;
                    }

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

                    if (tmin > tmax)
                    {
                        return(false);
                    }
                }
            }
            {
                if (absD.Y < Settings.Epsilon)
                {
                    // Parallel.
                    if (p.Y < LowerBound.Y || UpperBound.Y < p.Y)
                    {
                        return(false);
                    }
                }
                else
                {
                    var invD = 1.0f / d.Y;
                    var t1   = (LowerBound.Y - p.Y) * invD;
                    var t2   = (UpperBound.Y - p.Y) * invD;

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

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

                    // Push the min up
                    if (t1 > tmin)
                    {
                        normal.SetZero();
                        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 = new RayCastOutput {
                Fraction = tmin, Normal = normal
            };

            return(true);
        }
Esempio n. 48
0
 /// <summary>
 /// Cast a ray against a child shape.
 /// </summary>
 /// <param name="argOutput">the ray-cast results.</param>
 /// <param name="argInput">the ray-cast input parameters.</param>
 /// <param name="argTransform">the transform to be applied to the shape.</param>
 /// <param name="argChildIndex">the child shape index</param>
 /// <returns>if hit</returns>
 public abstract bool raycast(RayCastOutput output, RayCastInput input, Transform transform, int childIndex);
        public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex)
        {
            output = new RayCastOutput();

            // Put the ray into the polygon's frame of reference.
            Vector2 p1 = MathUtils.MulT(transform.q, input.Point1 - transform.p);
            Vector2 p2 = MathUtils.MulT(transform.q, input.Point2 - transform.p);
            Vector2 d  = p2 - p1;

            float lower = 0.0f, upper = input.MaxFraction;

            int index = -1;

            for (int i = 0; i < Vertices.Count; ++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.Mul(transform.q, Normals[index]);
                return(true);
            }

            return(false);
        }
Esempio n. 50
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(RayCastOutput output, RayCastInput input, Transform transform, int childIndex)
        {
            Vec2 position = pool1;
            Vec2 s = pool2;
            Vec2 r = pool3;

            Rot.mulToOutUnsafe(transform.q, m_p, position);
            position.addLocal(transform.p);
            s.set_Renamed(input.p1).subLocal(position);
            float b = Vec2.dot(s, s) - m_radius * m_radius;

            // Solve quadratic equation.
            r.set_Renamed(input.p2).subLocal(input.p1);
            float c = Vec2.dot(s, r);
            float rr = Vec2.dot(r, r);
            float sigma = c * c - rr * b;

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

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

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

            return false;
        }