Exemple #1
0
 public bool TestSegment(out float lambda, out Vec2 normal, Segment segment, float maxLambda)
 {
     lambda = 0f;
     normal = default(Vec2);
     Vec2 p = segment.P1;
     Vec2 a = segment.P2 - p;
     Vec2 a2 = this.P2 - this.P1;
     Vec2 vec = Vec2.Cross(a2, 1f);
     float num = 100f * Settings.FLT_EPSILON;
     float num2 = -Vec2.Dot(a, vec);
     bool result;
     if (num2 > num)
     {
         Vec2 a3 = p - this.P1;
         float num3 = Vec2.Dot(a3, vec);
         if (0f <= num3 && num3 <= maxLambda * num2)
         {
             float num4 = -a.X * a3.Y + a.Y * a3.X;
             if (-num * num2 <= num4 && num4 <= num2 * (1f + num))
             {
                 num3 /= num2;
                 vec.Normalize();
                 lambda = num3;
                 normal = vec;
                 result = true;
                 return result;
             }
         }
     }
     result = false;
     return result;
 }
Exemple #2
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 SegmentCollide TestSegment(XForm transform, out float lambda, out Vec2 normal, Segment segment, float maxLambda)
		{
			lambda = 0f;
			normal = Vec2.Zero;

			Vec2 position = transform.Position + Common.Math.Mul(transform.R, _localPosition);
			Vec2 s = segment.P1 - position;
			float b = Vec2.Dot(s, s) - _radius * _radius;

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

			// Solve quadratic equation.
			Vec2 r = segment.P2 - segment.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 < Common.Settings.FLT_EPSILON)
			{
				return SegmentCollide.MissCollide;
			}

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

			// Is the intersection point on the segment?
			if (0.0f <= a && a <= maxLambda * rr)
			{
				a /= rr;
				lambda = a;
				normal = s + a * r;
				normal.Normalize();
				return SegmentCollide.HitCollide;
			}

			return SegmentCollide.MissCollide;
		}
Exemple #3
0
 public override SegmentCollide TestSegment(XForm transform, out float lambda, out Vec2 normal, Segment segment, float maxLambda)
 {
     lambda = 0f;
     normal = Vec2.Zero;
     Vec2 v = transform.Position + Box2DX.Common.Math.Mul(transform.R, this._localPosition);
     Vec2 vec = segment.P1 - v;
     float num = Vec2.Dot(vec, vec) - this._radius * this._radius;
     SegmentCollide result;
     if (num < 0f)
     {
         lambda = 0f;
         result = SegmentCollide.StartInsideCollide;
     }
     else
     {
         Vec2 vec2 = segment.P2 - segment.P1;
         float num2 = Vec2.Dot(vec, vec2);
         float num3 = Vec2.Dot(vec2, vec2);
         float num4 = num2 * num2 - num3 * num;
         if (num4 < 0f || num3 < Settings.FLT_EPSILON)
         {
             result = SegmentCollide.MissCollide;
         }
         else
         {
             float num5 = -(num2 + Box2DX.Common.Math.Sqrt(num4));
             if (0f <= num5 && num5 <= maxLambda * num3)
             {
                 num5 /= num3;
                 lambda = num5;
                 normal = vec + num5 * vec2;
                 normal.Normalize();
                 result = SegmentCollide.HitCollide;
             }
             else
             {
                 result = SegmentCollide.MissCollide;
             }
         }
     }
     return result;
 }
Exemple #4
0
		public override SegmentCollide TestSegment(XForm transform, out float lambda, out Vec2 normal, Segment segment, float maxLambda)
		{
			Vec2 r = segment.P2 - segment.P1;
			Vec2 v1 = Common.Math.Mul(transform, _v1);
			Vec2 d = Common.Math.Mul(transform, _v2) - v1;
			Vec2 n = Vec2.Cross(d, 1.0f);

			float k_slop = 100.0f * Common.Settings.FLT_EPSILON;
			float denom = -Vec2.Dot(r, n);

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

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

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

			lambda = 0;
			normal = new Vec2();
			return SegmentCollide.MissCollide;
		}
Exemple #5
0
		/// <summary>
		/// Performs a raycast as with Raycast, finding the first intersecting shape.
		/// </summary>
		/// <param name="segment">Defines the begin and end point of the ray cast, from p1 to p2.
		/// Use Segment.Extend to create (semi-)infinite rays.</param>
		/// <param name="lambda">Returns the hit fraction. You can use this to compute the contact point
		/// p = (1 - lambda) * segment.p1 + lambda * segment.p2.</param>
		/// <param name="normal">Returns the normal at the contact point. If there is no intersection, the normal is not set.</param>
		/// <param name="solidShapes">Determines if shapes that the ray starts in are counted as hits.</param>
		/// <param name="userData"></param>
		/// <returns>Returns the colliding shape shape, or null if not found.</returns>
		public Shape RaycastOne(Segment segment, out float lambda, out Vec2 normal, bool solidShapes, object userData)
		{
			lambda = 0;
			normal = new Vec2(0,0);

			int maxCount = 1;
			Shape[] shape = new Shape[maxCount];

			int count = Raycast(segment, shape, maxCount, solidShapes, userData);

			if (count == 0)
				return null;

			Box2DXDebug.Assert(count == 1);

			//Redundantly do TestSegment a second time, as the previous one's results are inaccessible

			XForm xf = shape[0].GetBody().GetXForm();
			shape[0].TestSegment(xf, out lambda, out normal, segment, 1);
			//We already know it returns true
			return shape[0];
		}
Exemple #6
0
		/// <summary>
		/// Query the world for all shapes that intersect a given segment. You provide a shap
		/// pointer buffer of specified size. The number of shapes found is returned, and the buffer
		/// is filled in order of intersection.
		/// </summary>
		/// <param name="segment">Defines the begin and end point of the ray cast, from p1 to p2.
		/// Use Segment.Extend to create (semi-)infinite rays.</param>
		/// <param name="shapes">A user allocated shape pointer array of size maxCount (or greater).</param>
		/// <param name="maxCount">The capacity of the shapes array.</param>
		/// <param name="solidShapes">Determines if shapes that the ray starts in are counted as hits.</param>
		/// <param name="userData">Passed through the worlds contact filter, with method RayCollide. This can be used to filter valid shapes.</param>
		/// <returns>The number of shapes found</returns>
		public int Raycast(Segment segment, Shape[] shapes, int maxCount, bool solidShapes, object userData)
		{
#warning "PTR"
			_raycastSegment = segment;
			_raycastUserData = userData;
			_raycastSolidShape = solidShapes;

			object[] results = new object[maxCount];
			int count = _broadPhase.QuerySegment(segment, results, maxCount, RaycastSortKey);
			for (int i = 0; i < count; ++i)
			{
				shapes[i] = (Shape)results[i];
			}
			results = null;
			return count;
		}
Exemple #7
0
        public override SegmentCollide TestSegment(XForm transform, out float lambda, out Vec2 normal, Segment segment, float maxLambda)
        {
            lambda = 0f;
            normal = Vec2.Zero;
            Vec2           v   = transform.Position + Box2DX.Common.Math.Mul(transform.R, this._localPosition);
            Vec2           vec = segment.P1 - v;
            float          num = Vec2.Dot(vec, vec) - this._radius * this._radius;
            SegmentCollide result;

            if (num < 0f)
            {
                lambda = 0f;
                result = SegmentCollide.StartInsideCollide;
            }
            else
            {
                Vec2  vec2 = segment.P2 - segment.P1;
                float num2 = Vec2.Dot(vec, vec2);
                float num3 = Vec2.Dot(vec2, vec2);
                float num4 = num2 * num2 - num3 * num;
                if (num4 < 0f || num3 < Settings.FLT_EPSILON)
                {
                    result = SegmentCollide.MissCollide;
                }
                else
                {
                    float num5 = -(num2 + Box2DX.Common.Math.Sqrt(num4));
                    if (0f <= num5 && num5 <= maxLambda * num3)
                    {
                        num5  /= num3;
                        lambda = num5;
                        normal = vec + num5 * vec2;
                        normal.Normalize();
                        result = SegmentCollide.HitCollide;
                    }
                    else
                    {
                        result = SegmentCollide.MissCollide;
                    }
                }
            }
            return(result);
        }
        // Collision Detection in Interactive 3D Environments by Gino van den Bergen
        // From Section 3.4.1
        // x = mu1 * p1 + mu2 * p2
        // mu1 + mu2 = 1 && mu1 >= 0 && mu2 >= 0
        // mu1 = 1 - mu2;
        // x = (1 - mu2) * p1 + mu2 * p2
        //   = p1 + mu2 * (p2 - p1)
        // x = s + a * r (s := start, r := end - start)
        // s + a * r = p1 + mu2 * d (d := p2 - p1)
        // -a * r + mu2 * d = b (b := s - p1)
        // [-r d] * [a; mu2] = b
        // Cramer's rule:
        // denom = det[-r d]
        // a = det[b d] / denom
        // mu2 = det[-r b] / denom
        /// <summary>
        /// Ray cast against this segment with another segment.        
        /// </summary>
        /// <param name="lambda"></param>
        /// <param name="normal"></param>
        /// <param name="segment"></param>
        /// <param name="maxLambda"></param>
        /// <returns></returns>
        public bool TestSegment(out float lambda, out Vec2 normal, Segment segment, float maxLambda)
        {
            lambda = 0f;
            normal = new Vec2();

            Vec2 s = segment.P1;
            Vec2 r = segment.P2 - s;
            Vec2 d = P2 - P1;
            Vec2 n = Vec2.Cross(d, 1.0f);

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

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

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

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

            return false;
        }
Exemple #9
0
 public abstract SegmentCollide TestSegment(XForm xf, out float lambda, out Vec2 normal, Segment segment, float maxLambda);
Exemple #10
0
 public int Raycast(Segment segment, Shape[] shapes, int maxCount, bool solidShapes, object userData)
 {
     this._raycastSegment = segment;
     this._raycastUserData = userData;
     this._raycastSolidShape = solidShapes;
     object[] array = new object[maxCount];
     int num = this._broadPhase.QuerySegment(segment, array, maxCount, new SortKeyFunc(World.RaycastSortKey));
     for (int i = 0; i < num; i++)
     {
         shapes[i] = (Shape)array[i];
     }
     return num;
 }
Exemple #11
0
 public override SegmentCollide TestSegment(XForm xf, out float lambda, out Vec2 normal, Segment segment, float maxLambda)
 {
     lambda = 0f;
     normal = Vec2.Zero;
     float num = 0f;
     float num2 = maxLambda;
     Vec2 v = Box2DX.Common.Math.MulT(xf.R, segment.P1 - xf.Position);
     Vec2 v2 = Box2DX.Common.Math.MulT(xf.R, segment.P2 - xf.Position);
     Vec2 b = v2 - v;
     int num3 = -1;
     int i = 0;
     SegmentCollide result;
     while (i < this._vertexCount)
     {
         float num4 = Vec2.Dot(this._normals[i], this._vertices[i] - v);
         float num5 = Vec2.Dot(this._normals[i], b);
         if (num5 == 0f)
         {
             if (num4 < 0f)
             {
                 result = SegmentCollide.MissCollide;
                 return result;
             }
         }
         else
         {
             if (num5 < 0f && num4 < num * num5)
             {
                 num = num4 / num5;
                 num3 = i;
             }
             else
             {
                 if (num5 > 0f && num4 < num2 * num5)
                 {
                     num2 = num4 / num5;
                 }
             }
         }
         if (num2 >= num)
         {
             i++;
             continue;
         }
         result = SegmentCollide.MissCollide;
         return result;
     }
     Box2DXDebug.Assert(0f <= num && num <= maxLambda);
     if (num3 >= 0)
     {
         lambda = num;
         normal = Box2DX.Common.Math.Mul(xf.R, this._normals[num3]);
         result = SegmentCollide.HitCollide;
         return result;
     }
     lambda = 0f;
     result = SegmentCollide.StartInsideCollide;
     return result;
 }
Exemple #12
0
        public void Draw(GraphicsHelper gh)
        {
            var world = parent_car.body.GetWorld();
            var s = new Box2DX.Collision.Segment();
            var pen = new Pen(System.Drawing.Color.Gray, 1 / gh.Scale);
            for (int i = 0; i < p.num_rays; i++)
            {
                GetSegment(i, ref s);

                float lambda = data[i] / p.dist;

                float b0 = 0.3f, b = System.Math.Max(b0, 1 - lambda);
                byte b1 = (byte)((1 - b) * 255);
                Vec2 p1 = s.P1, p2 = (1 - lambda) * s.P1 + lambda * s.P2;

                pen.Color = System.Drawing.Color.FromArgb(b1, b1, b1);
                gh.G.DrawLine(pen, p1.X, p1.Y, p2.X, p2.Y);
            }
        }
Exemple #13
0
        public void Simulate()
        {
            var world = parent_car.body.GetWorld();
            var s = new Box2DX.Collision.Segment();
            for (int i = 0; i < p.num_rays; i++)
            {
                GetSegment(i, ref s);

                float lambda;
                Vec2 normal;
                var x = world.RaycastOne(s, out lambda, out normal, false, null);

                if (x == null) lambda = 1;
                data[i] = lambda * p.dist;
            }
        }
        public override SegmentCollide TestSegment(Transform xf, out float lambda, out Vector2 normal, Segment segment, float maxLambda)
        {
            Vector2 r = segment.P2 - segment.P1;
            Vector2 v1 = xf.TransformPoint(_v1);
            Vector2 d = ((Vector2)xf.TransformPoint(_v2)) - v1;
            Vector2 n = d.CrossScalarPostMultiply(1.0f);

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

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

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

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

            lambda = 0;
            normal = new Vector2();
            return SegmentCollide.MissCollide;
        }
        public void Update(float mX, float mY, Vector2 offset, int levelWidth)
        {
            if (amDrawing || amErasing)
            {
                //animation object
                myGameTime++;
                sourceRect = new Rectangle(xFrame * spriteWidth, yFrame * spriteHeight, spriteWidth, spriteHeight);

                animateTimer += myGameTime;

                if (animateTimer > animateInterval)
                {
                    xFrame++;

                    if (xFrame > numFrames - 1)
                    {
                        xFrame = 0;
                    }
                    myGameTime = 0;
                    animateTimer = 0;

                }

            }

            //Console.WriteLine("{0} {1}", mX, mY);
            //mx and my are screen coords but have been divided by scale so they are in "gamecoords"
            //therefore mouseX and mouseY are in screencoords
            mouseX = mX * SCALE;
            mouseY = mY * SCALE;

            int guyScreenPos = 0;
            if (dude.Position.X * CASSWorld.SCALE <= GameEngine.SCREEN_WIDTH / 2)
                guyScreenPos = (int)(dude.getWorld().getScreenCoords(dude.Position).X);
            else if (dude.Position.X * CASSWorld.SCALE >= levelWidth - GameEngine.SCREEN_WIDTH / 2)
            {
                guyScreenPos = (int)(dude.getWorld().getScreenCoords(dude.Position).X); ;
            }
            else
            {
                guyScreenPos = GameEngine.SCREEN_WIDTH / 2;
            }

            original = new Vector2(dude.Position.X * SCALE, dude.Position.Y * SCALE);
            end = new Vector2((mX + dude.Position.X) * SCALE - guyScreenPos, mY * SCALE);
            //end = new Vector2(mX*SCALE, mY * SCALE);

            //Vector2 gunpos = original; //+ adjustment;

            Box2DX.Collision.Segment myseg = new Segment();
            //myseg.P1 = new Box2DX.Common.Vec2(original.X / SCALE, original.Y / SCALE);
            myseg.P1 = new Box2DX.Common.Vec2(dude.Position.X, dude.Position.Y);
            myseg.P2 = new Box2DX.Common.Vec2(end.X / SCALE, end.Y / SCALE);

            lambda = 1;
            Box2DX.Common.Vec2 normal;

            //Console.WriteLine("{0}", interference[0]);
            for (int i = 0; i < 2; i++)
            {
                interference[i] = null;
            }

            //interference = world.RaycastOne(myseg, out lambda, out normal, false, null);
            //Console.WriteLine("hi");
            int numShapes = world.Raycast(myseg, interference, 3, false, null);
            //Console.WriteLine( "{0}", interference[0]);
            if (interference[0] == null)
            {
                canIDraw = true;
                canIErase = true;

                endpoint = new Vector2(mouseX, mouseY);
            }
            else if (interference[0].IsSensor)
            {

                if (interference[1] == null)
                {
                    canIDraw = true;
                    canIErase = true;

                    endpoint = new Vector2(mouseX, mouseY);
                }
                else if (interference[1].IsSensor)
                {
                    if (interference[2] == null)
                    {
                        canIDraw = true;
                        canIErase = true;
                        endpoint = new Vector2(mouseX, mouseY);
                    }
                    else
                    {
                        AABB aabb = new AABB();
                        aabb.LowerBound = Common.Utils.Convert(dude.getWorld().getGameCoords(new Vector2(mouseX, mouseY)) - new Vector2(0.1f));
                        aabb.UpperBound = Common.Utils.Convert(dude.getWorld().getGameCoords(new Vector2(mouseX, mouseY)) + new Vector2(0.1f));

                        Shape[] shapes = new Shape[1];
                        int nHit = world.Query(aabb, shapes, 1);

                        if (nHit > 0)
                        {
                            Body body1 = shapes[0].GetBody();
                            PhysicsObject po1 = (PhysicsObject)body1.GetUserData();
                            Body body2 = interference[2].GetBody();
                            PhysicsObject po2 = (PhysicsObject)body2.GetUserData();
                            if ((po1 is PaintedObject) && body1.Equals(body2))
                            {
                                canIErase = true;
                                canIDraw = false;
                            }
                            else
                            {
                                canIErase = false;
                                canIDraw = false;
                            }
                        }
                        else
                        {
                            canIErase = false;
                            canIDraw = false;
                        }

                        myseg.P1 = new Box2DX.Common.Vec2(dude.Position.X, dude.Position.Y + .7f);
                        Box2DX.Collision.Shape intersect = world.RaycastOne(myseg, out lambda, out normal, false, null);
                        Box2DX.Common.Vec2 p = (((1 - lambda) * (myseg.P1)) + (lambda * (myseg.P2)));
                        endpoint = dude.getWorld().getScreenCoords(Common.Utils.Convert(p));
                    }

                }
                else
                {
                    AABB aabb = new AABB();
                    aabb.LowerBound = Common.Utils.Convert(dude.getWorld().getGameCoords(new Vector2(mouseX, mouseY)) - new Vector2(0.1f));
                    aabb.UpperBound = Common.Utils.Convert(dude.getWorld().getGameCoords(new Vector2(mouseX, mouseY)) + new Vector2(0.1f));

                    Shape[] shapes = new Shape[1];
                    int nHit = world.Query(aabb, shapes, 1);

                    if (nHit > 0)
                    {
                        Body body1 = shapes[0].GetBody();
                        PhysicsObject po1 = (PhysicsObject)body1.GetUserData();
                        Body body2 = interference[1].GetBody();
                        PhysicsObject po2 = (PhysicsObject)body2.GetUserData();
                        if ((po1 is PaintedObject) && body1.Equals(body2))
                        {
                            canIErase = true;
                            canIDraw = false;
                        }
                        else
                        {
                            canIErase = false;
                            canIDraw = false;
                        }
                    }
                    else
                    {
                        canIErase = false;
                        canIDraw = false;
                    }

                    myseg.P1 = new Box2DX.Common.Vec2(dude.Position.X, dude.Position.Y + .6f);
                    Box2DX.Collision.Shape intersect = world.RaycastOne(myseg, out lambda, out normal, false, null);
                    Box2DX.Common.Vec2 p = (((1 - lambda) * (myseg.P1)) + (lambda * (myseg.P2)));
                    endpoint = dude.getWorld().getScreenCoords(Common.Utils.Convert(p));
                }

            }
            else
            {
                AABB aabb = new AABB();
                aabb.LowerBound = Common.Utils.Convert(dude.getWorld().getGameCoords(new Vector2(mouseX, mouseY)) - new Vector2(0.1f));
                aabb.UpperBound = Common.Utils.Convert(dude.getWorld().getGameCoords(new Vector2(mouseX, mouseY)) + new Vector2(0.1f));

                Shape[] shapes = new Shape[1];
                int nHit = world.Query(aabb, shapes, 1);

                if (nHit > 0)
                {
                    Body body1 = shapes[0].GetBody();
                    PhysicsObject po1 = (PhysicsObject)body1.GetUserData();
                    Body body2 = interference[0].GetBody();
                    PhysicsObject po2 = (PhysicsObject)body2.GetUserData();

                    if ((po1 is PaintedObject) && body1.Equals(body2))
                    {
                        canIErase = true;
                        canIDraw = false;
                    }
                    else
                    {
                        canIErase = false;
                        canIDraw = false;
                    }
                }
                else
                {
                    canIDraw = false;
                    canIErase = false;
                }

                Box2DX.Collision.Shape intersect = world.RaycastOne(myseg, out lambda, out normal, false, null);
                Box2DX.Common.Vec2 p = (((1 - lambda) * (myseg.P1)) + (lambda * (myseg.P2)));
                endpoint = dude.getWorld().getScreenCoords(Common.Utils.Convert(p));

            }

            //original animation stuff
            //startpoint = original + adjustment + offset;
            startpoint = original + offset;
            //endpoint = new Vector2(mouseX, mouseY);

            // move the start point to the end of his gun
            Vector2 cursorDirection = (endpoint - startpoint);
            cursorDirection.Normalize();
            startpoint = startpoint + (Constants.HALF_GUN * cursorDirection);
        }
Exemple #16
0
		int QuerySegment(Segment segment, object[] userData, int maxCount, SortKeyFunc sortKey)
		{
			float maxLambda = 1;

			float dx = (segment.P2.X - segment.P1.X) * _quantizationFactor.X;
			float dy = (segment.P2.Y - segment.P1.Y) * _quantizationFactor.Y;

			int sx = dx < -Settings.FLT_EPSILON ? -1 : (dx > Settings.FLT_EPSILON ? 1 : 0);
			int sy = dy < -Settings.FLT_EPSILON ? -1 : (dy > Settings.FLT_EPSILON ? 1 : 0);

			Box2DXDebug.Assert(sx != 0 || sy != 0);

			float p1x = (segment.P1.X - _worldAABB.LowerBound.X) * _quantizationFactor.X;
			float p1y = (segment.P1.Y - _worldAABB.LowerBound.Y) * _quantizationFactor.Y;
#if ALLOWUNSAFE
			ushort* startValues = stackalloc ushort[2];
			ushort* startValues2 = stackalloc ushort[2];
#else
			ushort[] startValues = new ushort[2];
			ushort[] startValues2 = new ushort[2];
#endif

			int xIndex;
			int yIndex;

			ushort proxyId;
			Proxy proxy;

			// TODO_ERIN implement fast float to ushort conversion.
			startValues[0] = (ushort)((ushort)(p1x) & (BROADPHASE_MAX - 1));
			startValues2[0] = (ushort)((ushort)(p1x) | 1);

			startValues[1] = (ushort)((ushort)(p1y) & (BROADPHASE_MAX - 1));
			startValues2[1] = (ushort)((ushort)(p1y) | 1);

			//First deal with all the proxies that contain segment.p1
			int lowerIndex;
			int upperIndex;
			Query(out lowerIndex, out upperIndex, startValues[0], startValues2[0], _bounds[0], 2 * _proxyCount, 0);
			if (sx >= 0) xIndex = upperIndex - 1;
			else xIndex = lowerIndex;
			Query(out lowerIndex, out upperIndex, startValues[1], startValues2[1], _bounds[1], 2 * _proxyCount, 1);
			if (sy >= 0) yIndex = upperIndex - 1;
			else yIndex = lowerIndex;

			//If we are using sortKey, then sort what we have so far, filtering negative keys
			if (sortKey != null)
			{
				//Fill keys
				for (int j = 0; j < _queryResultCount; j++)
				{
					_querySortKeys[j] = sortKey(_proxyPool[_queryResults[j]].UserData);
				}
				//Bubble sort keys
				//Sorting negative values to the top, so we can easily remove them
				int i = 0;
				while (i < _queryResultCount - 1)
				{
					float a = _querySortKeys[i];
					float b = _querySortKeys[i + 1];
					if ((a < 0) ? (b >= 0) : (a > b && b >= 0))
					{
						_querySortKeys[i + 1] = a;
						_querySortKeys[i] = b;
						ushort tempValue = _queryResults[i + 1];
						_queryResults[i + 1] = _queryResults[i];
						_queryResults[i] = tempValue;
						i--;
						if (i == -1) i = 1;
					}
					else
					{
						i++;
					}
				}
				//Skim off negative values
				while (_queryResultCount > 0 && _querySortKeys[_queryResultCount - 1] < 0)
					_queryResultCount--;
			}

			//Now work through the rest of the segment
			for (; ; )
			{
				float xProgress = 0;
				float yProgress = 0;
				if (xIndex < 0 || xIndex >= _proxyCount * 2)
					break;
				if (yIndex < 0 || yIndex >= _proxyCount * 2)
					break;
				if (sx != 0)
				{
					//Move on to the next bound
					if (sx > 0)
					{
						xIndex++;
						if (xIndex == _proxyCount * 2)
							break;
					}
					else
					{
						xIndex--;
						if (xIndex < 0)
							break;
					}
					xProgress = (_bounds[0][xIndex].Value - p1x) / dx;
				}
				if (sy != 0)
				{
					//Move on to the next bound
					if (sy > 0)
					{
						yIndex++;
						if (yIndex == _proxyCount * 2)
							break;
					}
					else
					{
						yIndex--;
						if (yIndex < 0)
							break;
					}
					yProgress = (_bounds[1][yIndex].Value - p1y) / dy;
				}
				for (; ; )
				{
					if (sy == 0 || (sx != 0 && xProgress < yProgress))
					{
						if (xProgress > maxLambda)
							break;

						//Check that we are entering a proxy, not leaving
						if (sx > 0 ? _bounds[0][xIndex].IsLower : _bounds[0][xIndex].IsUpper)
						{
							//Check the other axis of the proxy
							proxyId = _bounds[0][xIndex].ProxyId;
							proxy = _proxyPool[proxyId];
							if (sy >= 0)
							{
								if (proxy.LowerBounds[1] <= yIndex - 1 && proxy.UpperBounds[1] >= yIndex)
								{
									//Add the proxy
									if (sortKey != null)
									{
										AddProxyResult(proxyId, proxy, maxCount, sortKey);
									}
									else
									{
										_queryResults[_queryResultCount] = proxyId;
										++_queryResultCount;
									}
								}
							}
							else
							{
								if (proxy.LowerBounds[1] <= yIndex && proxy.UpperBounds[1] >= yIndex + 1)
								{
									//Add the proxy
									if (sortKey != null)
									{
										AddProxyResult(proxyId, proxy, maxCount, sortKey);
									}
									else
									{
										_queryResults[_queryResultCount] = proxyId;
										++_queryResultCount;
									}
								}
							}
						}

						//Early out
						if (sortKey != null && _queryResultCount == maxCount && _queryResultCount > 0 && xProgress > _querySortKeys[_queryResultCount - 1])
							break;

						//Move on to the next bound
						if (sx > 0)
						{
							xIndex++;
							if (xIndex == _proxyCount * 2)
								break;
						}
						else
						{
							xIndex--;
							if (xIndex < 0)
								break;
						}
						xProgress = (_bounds[0][xIndex].Value - p1x) / dx;
					}
					else
					{
						if (yProgress > maxLambda)
							break;

						//Check that we are entering a proxy, not leaving
						if (sy > 0 ? _bounds[1][yIndex].IsLower : _bounds[1][yIndex].IsUpper)
						{
							//Check the other axis of the proxy
							proxyId = _bounds[1][yIndex].ProxyId;
							proxy = _proxyPool[proxyId];
							if (sx >= 0)
							{
								if (proxy.LowerBounds[0] <= xIndex - 1 && proxy.UpperBounds[0] >= xIndex)
								{
									//Add the proxy
									if (sortKey != null)
									{
										AddProxyResult(proxyId, proxy, maxCount, sortKey);
									}
									else
									{
										_queryResults[_queryResultCount] = proxyId;
										++_queryResultCount;
									}
								}
							}
							else
							{
								if (proxy.LowerBounds[0] <= xIndex && proxy.UpperBounds[0] >= xIndex + 1)
								{
									//Add the proxy
									if (sortKey != null)
									{
										AddProxyResult(proxyId, proxy, maxCount, sortKey);
									}
									else
									{
										_queryResults[_queryResultCount] = proxyId;
										++_queryResultCount;
									}
								}
							}
						}

						//Early out
						if (sortKey != null && _queryResultCount == maxCount && _queryResultCount > 0 && yProgress > _querySortKeys[_queryResultCount - 1])
							break;

						//Move on to the next bound
						if (sy > 0)
						{
							yIndex++;
							if (yIndex == _proxyCount * 2)
								break;
						}
						else
						{
							yIndex--;
							if (yIndex < 0)
								break;
						}
						yProgress = (_bounds[1][yIndex].Value - p1y) / dy;
					}
				}

				break;
			}

			int count = 0;
			for (int i = 0; i < _queryResultCount && count < maxCount; ++i, ++count)
			{
				Box2DXDebug.Assert(_queryResults[i] < Settings.MaxProxies);
				Proxy proxy_ = _proxyPool[_queryResults[i]];
				Box2DXDebug.Assert(proxy_.IsValid);
				userData[i] = proxy_.UserData;
			}

			// Prepare for next query.
			_queryResultCount = 0;
			IncrementTimeStamp();

			return count;
		}
Exemple #17
0
 public Shape RaycastOne(Segment segment, out float lambda, out Vec2 normal, bool solidShapes, object userData)
 {
     lambda = 0f;
     normal = new Vec2(0f, 0f);
     int num = 1;
     Shape[] array = new Shape[num];
     int num2 = this.Raycast(segment, array, num, solidShapes, userData);
     Shape result;
     if (num2 == 0)
     {
         result = null;
     }
     else
     {
         Box2DXDebug.Assert(num2 == 1);
         XForm xForm = array[0].GetBody().GetXForm();
         array[0].TestSegment(xForm, out lambda, out normal, segment, 1f);
         result = array[0];
     }
     return result;
 }
Exemple #18
0
        /// <summary>
        /// Performs a raycast as with Raycast, finding the first intersecting shape.
        /// </summary>
        /// <param name="segment">Defines the begin and end point of the ray cast, from p1 to p2.
        /// Use Segment.Extend to create (semi-)infinite rays.</param>
        /// <param name="lambda">Returns the hit fraction. You can use this to compute the contact point
        /// p = (1 - lambda) * segment.p1 + lambda * segment.p2.</param>
        /// <param name="normal">Returns the normal at the contact point. If there is no intersection, the normal is not set.</param>
        /// <param name="solidShapes">Determines if shapes that the ray starts in are counted as hits.</param>
        /// <param name="userData"></param>
        /// <returns>Returns the colliding shape shape, or null if not found.</returns>
        public Fixture RaycastOne(Segment segment, out float lambda, out Vector2 normal, bool solidShapes, object userData)
        {
            int maxCount = 1;
            Fixture[] fixture;
            lambda = 0.0f;
            normal = new Vector2();

            int count = Raycast(segment, out fixture, maxCount, solidShapes, userData);

            if (count == 0)
                return null;

            Box2DXDebug.Assert(count == 1);

            //Redundantly do TestSegment a second time, as the previous one's results are inaccessible

            fixture[0].TestSegment(out lambda, out normal, segment, 1);
            //We already know it returns true
            return fixture[0];
        }
Exemple #19
0
 public unsafe int QuerySegment(Segment segment, object[] userData, int maxCount, SortKeyFunc sortKey)
 {
     float num = 1f;
     float num2 = (segment.P2.X - segment.P1.X) * this._quantizationFactor.X;
     float num3 = (segment.P2.Y - segment.P1.Y) * this._quantizationFactor.Y;
     int num4 = (num2 < -Settings.FLT_EPSILON) ? -1 : ((num2 > Settings.FLT_EPSILON) ? 1 : 0);
     int num5 = (num3 < -Settings.FLT_EPSILON) ? -1 : ((num3 > Settings.FLT_EPSILON) ? 1 : 0);
     Box2DXDebug.Assert(num4 != 0 || num5 != 0);
     float num6 = (segment.P1.X - this._worldAABB.LowerBound.X) * this._quantizationFactor.X;
     float num7 = (segment.P1.Y - this._worldAABB.LowerBound.Y) * this._quantizationFactor.Y;
     ushort* ptr = stackalloc ushort[2];
     ushort* ptr2 = stackalloc ushort[2];
     *ptr = (ushort)((ushort)num6 & BroadPhase.BROADPHASE_MAX - 1);
     *ptr2 = (ushort)((ushort)num6 | 1);
     ptr[2 / 2] = (ushort)((ushort)num7 & BroadPhase.BROADPHASE_MAX - 1);
     ptr2[2 / 2] = (ushort)((ushort)num7 | 1);
     int num8;
     int num9;
     this.Query(out num8, out num9, *ptr, *ptr2, this._bounds[0], 2 * this._proxyCount, 0);
     int num10;
     if (num4 >= 0)
     {
         num10 = num9 - 1;
     }
     else
     {
         num10 = num8;
     }
     this.Query(out num8, out num9, ptr[2 / 2], *(ptr2 + 2 / 2), this._bounds[1], 2 * this._proxyCount, 1);
     int num11;
     if (num5 >= 0)
     {
         num11 = num9 - 1;
     }
     else
     {
         num11 = num8;
     }
     int j;
     if (sortKey != null)
     {
         for (int i = 0; i < this._queryResultCount; i++)
         {
             this._querySortKeys[i] = sortKey(this._proxyPool[(int)this._queryResults[i]].UserData);
         }
         j = 0;
         while (j < this._queryResultCount - 1)
         {
             float num12 = this._querySortKeys[j];
             float num13 = this._querySortKeys[j + 1];
             if (((num12 < 0f) ? ((num13 >= 0f) ? 1 : 0) : ((num12 <= num13) ? 0 : ((num13 >= 0f) ? 1 : 0))) != 0)
             {
                 this._querySortKeys[j + 1] = num12;
                 this._querySortKeys[j] = num13;
                 ushort num14 = this._queryResults[j + 1];
                 this._queryResults[j + 1] = this._queryResults[j];
                 this._queryResults[j] = num14;
                 j--;
                 if (j == -1)
                 {
                     j = 1;
                 }
             }
             else
             {
                 j++;
             }
         }
         while (this._queryResultCount > 0 && this._querySortKeys[this._queryResultCount - 1] < 0f)
         {
             this._queryResultCount--;
         }
     }
     float num15 = 0f;
     float num16 = 0f;
     if (num10 >= 0 && num10 < this._proxyCount * 2)
     {
         if (num11 >= 0 && num11 < this._proxyCount * 2)
         {
             if (num4 != 0)
             {
                 if (num4 > 0)
                 {
                     num10++;
                     if (num10 == this._proxyCount * 2)
                     {
                         goto IL_829;
                     }
                 }
                 else
                 {
                     num10--;
                     if (num10 < 0)
                     {
                         goto IL_829;
                     }
                 }
                 num15 = ((float)this._bounds[0][num10].Value - num6) / num2;
             }
             if (num5 != 0)
             {
                 if (num5 > 0)
                 {
                     num11++;
                     if (num11 == this._proxyCount * 2)
                     {
                         goto IL_829;
                     }
                 }
                 else
                 {
                     num11--;
                     if (num11 < 0)
                     {
                         goto IL_829;
                     }
                 }
                 num16 = ((float)this._bounds[1][num11].Value - num7) / num3;
             }
             while (true)
             {
                 if (num5 == 0 || (num4 != 0 && num15 < num16))
                 {
                     if (num15 > num)
                     {
                         break;
                     }
                     if ((num4 > 0) ? this._bounds[0][num10].IsLower : this._bounds[0][num10].IsUpper)
                     {
                         ushort proxyId = this._bounds[0][num10].ProxyId;
                         Proxy proxy = this._proxyPool[(int)proxyId];
                         if (num5 >= 0)
                         {
                             if ((int)proxy.LowerBounds[1] <= num11 - 1 && (int)proxy.UpperBounds[1] >= num11)
                             {
                                 if (sortKey != null)
                                 {
                                     this.AddProxyResult(proxyId, proxy, maxCount, sortKey);
                                 }
                                 else
                                 {
                                     this._queryResults[this._queryResultCount] = proxyId;
                                     this._queryResultCount++;
                                 }
                             }
                         }
                         else
                         {
                             if ((int)proxy.LowerBounds[1] <= num11 && (int)proxy.UpperBounds[1] >= num11 + 1)
                             {
                                 if (sortKey != null)
                                 {
                                     this.AddProxyResult(proxyId, proxy, maxCount, sortKey);
                                 }
                                 else
                                 {
                                     this._queryResults[this._queryResultCount] = proxyId;
                                     this._queryResultCount++;
                                 }
                             }
                         }
                     }
                     if (sortKey != null && this._queryResultCount == maxCount && this._queryResultCount > 0 && num15 > this._querySortKeys[this._queryResultCount - 1])
                     {
                         break;
                     }
                     if (num4 > 0)
                     {
                         num10++;
                         if (num10 == this._proxyCount * 2)
                         {
                             break;
                         }
                     }
                     else
                     {
                         num10--;
                         if (num10 < 0)
                         {
                             break;
                         }
                     }
                     num15 = ((float)this._bounds[0][num10].Value - num6) / num2;
                 }
                 else
                 {
                     if (num16 > num)
                     {
                         break;
                     }
                     if ((num5 > 0) ? this._bounds[1][num11].IsLower : this._bounds[1][num11].IsUpper)
                     {
                         ushort proxyId = this._bounds[1][num11].ProxyId;
                         Proxy proxy = this._proxyPool[(int)proxyId];
                         if (num4 >= 0)
                         {
                             if ((int)proxy.LowerBounds[0] <= num10 - 1 && (int)proxy.UpperBounds[0] >= num10)
                             {
                                 if (sortKey != null)
                                 {
                                     this.AddProxyResult(proxyId, proxy, maxCount, sortKey);
                                 }
                                 else
                                 {
                                     this._queryResults[this._queryResultCount] = proxyId;
                                     this._queryResultCount++;
                                 }
                             }
                         }
                         else
                         {
                             if ((int)proxy.LowerBounds[0] <= num10 && (int)proxy.UpperBounds[0] >= num10 + 1)
                             {
                                 if (sortKey != null)
                                 {
                                     this.AddProxyResult(proxyId, proxy, maxCount, sortKey);
                                 }
                                 else
                                 {
                                     this._queryResults[this._queryResultCount] = proxyId;
                                     this._queryResultCount++;
                                 }
                             }
                         }
                     }
                     if (sortKey != null && this._queryResultCount == maxCount && this._queryResultCount > 0 && num16 > this._querySortKeys[this._queryResultCount - 1])
                     {
                         break;
                     }
                     if (num5 > 0)
                     {
                         num11++;
                         if (num11 == this._proxyCount * 2)
                         {
                             break;
                         }
                     }
                     else
                     {
                         num11--;
                         if (num11 < 0)
                         {
                             break;
                         }
                     }
                     num16 = ((float)this._bounds[1][num11].Value - num7) / num3;
                 }
             }
         }
     }
     IL_829:
     int num17 = 0;
     j = 0;
     while (j < this._queryResultCount && num17 < maxCount)
     {
         Box2DXDebug.Assert((int)this._queryResults[j] < Settings.MaxProxies);
         Proxy proxy2 = this._proxyPool[(int)this._queryResults[j]];
         Box2DXDebug.Assert(proxy2.IsValid);
         userData[j] = proxy2.UserData;
         j++;
         num17++;
     }
     this._queryResultCount = 0;
     this.IncrementTimeStamp();
     return num17;
 }
Exemple #20
0
 /// <summary>
 /// Perform a ray cast against this shape.
 /// </summary>
 /// <param name="lambda">Returns the hit fraction. You can use this to compute the contact point
 /// p = (1 - lambda) * segment.p1 + lambda * segment.p2.</param>
 /// <param name="normal">Returns the normal at the contact point. If there is no intersection, the normal
 /// is not set.</param>
 /// <param name="segment">Defines the begin and end point of the ray cast.</param>
 /// <param name="maxLambda">A number typically in the range [0,1].</param>
 public SegmentCollide TestSegment(out float lambda, out Vec2 normal, Segment segment, float maxLambda)
 {
     return _shape.TestSegment(_body.GetXForm(), out lambda, out normal, segment, maxLambda);
 }
Exemple #21
0
		public override SegmentCollide TestSegment(XForm xf, out float lambda, out Vec2 normal, Segment segment, float maxLambda)
		{
			lambda = 0f;
			normal = Vec2.Zero;

			float lower = 0.0f, upper = maxLambda;

			Vec2 p1 = Common.Math.MulT(xf.R, segment.P1 - xf.Position);
			Vec2 p2 = Common.Math.MulT(xf.R, segment.P2 - xf.Position);
			Vec2 d = p2 - p1;
			int index = -1;

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

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

				if (upper < lower)
				{
					return SegmentCollide.MissCollide;
				}
			}

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

			if (index >= 0)
			{
				lambda = lower;
				normal = Common.Math.Mul(xf.R, _normals[index]);
				return SegmentCollide.HitCollide;
			}

			lambda = 0f;
			return SegmentCollide.StartInsideCollide;
		}
Exemple #22
0
        int QuerySegment(Segment segment, object[] userData, int maxCount, SortKeyFunc sortKey)
        {
            float maxLambda = 1;

            float dx = (segment.P2.X - segment.P1.X) * _quantizationFactor.X;
            float dy = (segment.P2.Y - segment.P1.Y) * _quantizationFactor.Y;

            int sx = dx < -Settings.FLT_EPSILON ? -1 : (dx > Settings.FLT_EPSILON ? 1 : 0);
            int sy = dy < -Settings.FLT_EPSILON ? -1 : (dy > Settings.FLT_EPSILON ? 1 : 0);

            Box2DXDebug.Assert(sx != 0 || sy != 0);

            float p1x = (segment.P1.X - _worldAABB.LowerBound.X) * _quantizationFactor.X;
            float p1y = (segment.P1.Y - _worldAABB.LowerBound.Y) * _quantizationFactor.Y;

#if ALLOWUNSAFE
            ushort *startValues  = stackalloc ushort[2];
            ushort *startValues2 = stackalloc ushort[2];
#else
            ushort[] startValues  = new ushort[2];
            ushort[] startValues2 = new ushort[2];
#endif

            int xIndex;
            int yIndex;

            ushort proxyId;
            Proxy  proxy;

            // TODO_ERIN implement fast float to ushort conversion.
            startValues[0]  = (ushort)((ushort)(p1x) & (BROADPHASE_MAX - 1));
            startValues2[0] = (ushort)((ushort)(p1x) | 1);

            startValues[1]  = (ushort)((ushort)(p1y) & (BROADPHASE_MAX - 1));
            startValues2[1] = (ushort)((ushort)(p1y) | 1);

            //First deal with all the proxies that contain segment.p1
            int lowerIndex;
            int upperIndex;
            Query(out lowerIndex, out upperIndex, startValues[0], startValues2[0], _bounds[0], 2 * _proxyCount, 0);
            if (sx >= 0)
            {
                xIndex = upperIndex - 1;
            }
            else
            {
                xIndex = lowerIndex;
            }
            Query(out lowerIndex, out upperIndex, startValues[1], startValues2[1], _bounds[1], 2 * _proxyCount, 1);
            if (sy >= 0)
            {
                yIndex = upperIndex - 1;
            }
            else
            {
                yIndex = lowerIndex;
            }

            //If we are using sortKey, then sort what we have so far, filtering negative keys
            if (sortKey != null)
            {
                //Fill keys
                for (int j = 0; j < _queryResultCount; j++)
                {
                    _querySortKeys[j] = sortKey(_proxyPool[_queryResults[j]].UserData);
                }
                //Bubble sort keys
                //Sorting negative values to the top, so we can easily remove them
                int i = 0;
                while (i < _queryResultCount - 1)
                {
                    float a = _querySortKeys[i];
                    float b = _querySortKeys[i + 1];
                    if ((a < 0) ? (b >= 0) : (a > b && b >= 0))
                    {
                        _querySortKeys[i + 1] = a;
                        _querySortKeys[i]     = b;
                        ushort tempValue = _queryResults[i + 1];
                        _queryResults[i + 1] = _queryResults[i];
                        _queryResults[i]     = tempValue;
                        i--;
                        if (i == -1)
                        {
                            i = 1;
                        }
                    }
                    else
                    {
                        i++;
                    }
                }
                //Skim off negative values
                while (_queryResultCount > 0 && _querySortKeys[_queryResultCount - 1] < 0)
                {
                    _queryResultCount--;
                }
            }

            //Now work through the rest of the segment
            for (; ;)
            {
                float xProgress = 0;
                float yProgress = 0;
                if (xIndex < 0 || xIndex >= _proxyCount * 2)
                {
                    break;
                }
                if (yIndex < 0 || yIndex >= _proxyCount * 2)
                {
                    break;
                }
                if (sx != 0)
                {
                    //Move on to the next bound
                    if (sx > 0)
                    {
                        xIndex++;
                        if (xIndex == _proxyCount * 2)
                        {
                            break;
                        }
                    }
                    else
                    {
                        xIndex--;
                        if (xIndex < 0)
                        {
                            break;
                        }
                    }
                    xProgress = (_bounds[0][xIndex].Value - p1x) / dx;
                }
                if (sy != 0)
                {
                    //Move on to the next bound
                    if (sy > 0)
                    {
                        yIndex++;
                        if (yIndex == _proxyCount * 2)
                        {
                            break;
                        }
                    }
                    else
                    {
                        yIndex--;
                        if (yIndex < 0)
                        {
                            break;
                        }
                    }
                    yProgress = (_bounds[1][yIndex].Value - p1y) / dy;
                }
                for (; ;)
                {
                    if (sy == 0 || (sx != 0 && xProgress < yProgress))
                    {
                        if (xProgress > maxLambda)
                        {
                            break;
                        }

                        //Check that we are entering a proxy, not leaving
                        if (sx > 0 ? _bounds[0][xIndex].IsLower : _bounds[0][xIndex].IsUpper)
                        {
                            //Check the other axis of the proxy
                            proxyId = _bounds[0][xIndex].ProxyId;
                            proxy   = _proxyPool[proxyId];
                            if (sy >= 0)
                            {
                                if (proxy.LowerBounds[1] <= yIndex - 1 && proxy.UpperBounds[1] >= yIndex)
                                {
                                    //Add the proxy
                                    if (sortKey != null)
                                    {
                                        AddProxyResult(proxyId, proxy, maxCount, sortKey);
                                    }
                                    else
                                    {
                                        _queryResults[_queryResultCount] = proxyId;
                                        ++_queryResultCount;
                                    }
                                }
                            }
                            else
                            {
                                if (proxy.LowerBounds[1] <= yIndex && proxy.UpperBounds[1] >= yIndex + 1)
                                {
                                    //Add the proxy
                                    if (sortKey != null)
                                    {
                                        AddProxyResult(proxyId, proxy, maxCount, sortKey);
                                    }
                                    else
                                    {
                                        _queryResults[_queryResultCount] = proxyId;
                                        ++_queryResultCount;
                                    }
                                }
                            }
                        }

                        //Early out
                        if (sortKey != null && _queryResultCount == maxCount && _queryResultCount > 0 && xProgress > _querySortKeys[_queryResultCount - 1])
                        {
                            break;
                        }

                        //Move on to the next bound
                        if (sx > 0)
                        {
                            xIndex++;
                            if (xIndex == _proxyCount * 2)
                            {
                                break;
                            }
                        }
                        else
                        {
                            xIndex--;
                            if (xIndex < 0)
                            {
                                break;
                            }
                        }
                        xProgress = (_bounds[0][xIndex].Value - p1x) / dx;
                    }
                    else
                    {
                        if (yProgress > maxLambda)
                        {
                            break;
                        }

                        //Check that we are entering a proxy, not leaving
                        if (sy > 0 ? _bounds[1][yIndex].IsLower : _bounds[1][yIndex].IsUpper)
                        {
                            //Check the other axis of the proxy
                            proxyId = _bounds[1][yIndex].ProxyId;
                            proxy   = _proxyPool[proxyId];
                            if (sx >= 0)
                            {
                                if (proxy.LowerBounds[0] <= xIndex - 1 && proxy.UpperBounds[0] >= xIndex)
                                {
                                    //Add the proxy
                                    if (sortKey != null)
                                    {
                                        AddProxyResult(proxyId, proxy, maxCount, sortKey);
                                    }
                                    else
                                    {
                                        _queryResults[_queryResultCount] = proxyId;
                                        ++_queryResultCount;
                                    }
                                }
                            }
                            else
                            {
                                if (proxy.LowerBounds[0] <= xIndex && proxy.UpperBounds[0] >= xIndex + 1)
                                {
                                    //Add the proxy
                                    if (sortKey != null)
                                    {
                                        AddProxyResult(proxyId, proxy, maxCount, sortKey);
                                    }
                                    else
                                    {
                                        _queryResults[_queryResultCount] = proxyId;
                                        ++_queryResultCount;
                                    }
                                }
                            }
                        }

                        //Early out
                        if (sortKey != null && _queryResultCount == maxCount && _queryResultCount > 0 && yProgress > _querySortKeys[_queryResultCount - 1])
                        {
                            break;
                        }

                        //Move on to the next bound
                        if (sy > 0)
                        {
                            yIndex++;
                            if (yIndex == _proxyCount * 2)
                            {
                                break;
                            }
                        }
                        else
                        {
                            yIndex--;
                            if (yIndex < 0)
                            {
                                break;
                            }
                        }
                        yProgress = (_bounds[1][yIndex].Value - p1y) / dy;
                    }
                }

                break;
            }

            int count = 0;
            for (int i = 0; i < _queryResultCount && count < maxCount; ++i, ++count)
            {
                Box2DXDebug.Assert(_queryResults[i] < Settings.MaxProxies);
                Proxy proxy_ = _proxyPool[_queryResults[i]];
                Box2DXDebug.Assert(proxy_.IsValid);
                userData[i] = proxy_.UserData;
            }

            // Prepare for next query.
            _queryResultCount = 0;
            IncrementTimeStamp();

            return(count);
        }