Add() public method

public Add ( Body body ) : void
body Body
return void
Esempio n. 1
0
		// Find TOI contacts and solve them.
		private void SolveTOI(TimeStep step)
		{
			// Reserve an island and a queue for TOI island solution.
			Island island = new Island(_bodyCount, Settings.MaxTOIContactsPerIsland, Settings.MaxTOIJointsPerIsland, _contactListener);

			//Simple one pass queue
			//Relies on the fact that we're only making one pass
			//through and each body can only be pushed/popped once.
			//To push: 
			//  queue[queueStart+queueSize++] = newElement;
			//To pop: 
			//	poppedElement = queue[queueStart++];
			//  --queueSize;
			int queueCapacity = _bodyCount;
			Body[] queue = new Body[queueCapacity];
			for (Body b = _bodyList; b != null; b = b._next)
			{
				b._flags &= ~Body.BodyFlags.Island;
				b._sweep.T0 = 0.0f;
			}

			for (Contact c = _contactList; c != null; c = c._next)
			{
				// Invalidate TOI
				c._flags &= ~(Contact.CollisionFlags.Toi | Contact.CollisionFlags.Island);
			}

#if B2_TOI_JOINTS
			for (Joint j = _jointList; j!=null; j = j._next)
			{
					j._islandFlag = false;
			}
#endif

			// Find TOI events and solve them.
			for (; ; )
			{
				// Find the first TOI.
				Contact minContact = null;
				float minTOI = 1.0f;

				for (Contact c = _contactList; c != null; c = c._next)
				{
					if ((c._flags & (Contact.CollisionFlags.Slow | Contact.CollisionFlags.NonSolid)) != 0)
					{
						continue;
					}

					// TODO_ERIN keep a counter on the contact, only respond to M TOIs per contact.

					float toi = 1.0f;
					if ((c._flags & Contact.CollisionFlags.Toi) != 0)
					{
						// This contact has a valid cached TOI.
						toi = c._toi;
					}
					else
					{
						// Compute the TOI for this contact.
						Shape s1_ = c.GetShape1();
						Shape s2_ = c.GetShape2();
						Body b1_ = s1_.GetBody();
						Body b2_ = s2_.GetBody();

						if ((b1_.IsStatic() || b1_.IsSleeping()) && (b2_.IsStatic() || b2_.IsSleeping()))
						{
							continue;
						}

						// Put the sweeps onto the same time interval.
						float t0 = b1_._sweep.T0;

						if (b1_._sweep.T0 < b2_._sweep.T0)
						{
							t0 = b2_._sweep.T0;
							b1_._sweep.Advance(t0);
						}
						else if (b2_._sweep.T0 < b1_._sweep.T0)
						{
							t0 = b1_._sweep.T0;
							b2_._sweep.Advance(t0);
						}

						Box2DXDebug.Assert(t0 < 1.0f);

						// Compute the time of impact.
						toi = Collision.Collision.TimeOfImpact(c._shape1, b1_._sweep, c._shape2, b2_._sweep);
						Box2DXDebug.Assert(0.0f <= toi && toi <= 1.0f);

						if (toi > 0.0f && toi < 1.0f)
						{
							toi = Common.Math.Min((1.0f - toi) * t0 + toi, 1.0f);
						}


						c._toi = toi;
						c._flags |= Contact.CollisionFlags.Toi;
					}

					if (Common.Settings.FLT_EPSILON < toi && toi < minTOI)
					{
						// This is the minimum TOI found so far.
						minContact = c;
						minTOI = toi;
					}
				}

				if (minContact == null || 1.0f - 100.0f * Common.Settings.FLT_EPSILON < minTOI)
				{
					// No more TOI events. Done!
					break;
				}

				// Advance the bodies to the TOI.
				Shape s1 = minContact.GetShape1();
				Shape s2 = minContact.GetShape2();
				Body b1 = s1.GetBody();
				Body b2 = s2.GetBody();
				b1.Advance(minTOI);
				b2.Advance(minTOI);

				// The TOI contact likely has some new contact points.
				minContact.Update(_contactListener);
				minContact._flags &= ~Contact.CollisionFlags.Toi;

				if (minContact.GetManifoldCount() == 0)
				{
					// This shouldn't happen. Numerical error?
					//b2Assert(false);
					continue;
				}

				// Build the TOI island. We need a dynamic seed.
				Body seed = b1;
				if (seed.IsStatic())
				{
					seed = b2;
				}

				// Reset island and queue.
				island.Clear();

				int queueStart = 0; //starting index for queue
				int queueSize = 0;  //elements in queue
				queue[queueStart + queueSize++] = seed;
				seed._flags |= Body.BodyFlags.Island;

				// Perform a breadth first search (BFS) on the contact/joint graph.
				while (queueSize > 0)
				{
					// Grab the next body off the stack and add it to the island.
					Body b = queue[queueStart++];
					--queueSize;
					island.Add(b);

					// Make sure the body is awake.
					b._flags &= ~Body.BodyFlags.Sleep;

					// To keep islands as small as possible, we don't
					// propagate islands across static bodies.
					if (b.IsStatic())
					{
						continue;
					}

					// Search all contacts connected to this body.
					for (ContactEdge cn = b._contactList; cn != null; cn = cn.Next)
					{
						// Does the TOI island still have space for contacts?
						if (island._contactCount == island._contactCapacity)
						{
							continue;
						}

						// Has this contact already been added to an island? Skip slow or non-solid contacts.
						if ((cn.Contact._flags & (Contact.CollisionFlags.Island | Contact.CollisionFlags.Slow | Contact.CollisionFlags.NonSolid)) != 0)
						{
							continue;
						}

						// Is this contact touching? For performance we are not updating this contact.
						if (cn.Contact.GetManifoldCount() == 0)
						{
							continue;
						}

						island.Add(cn.Contact);
						cn.Contact._flags |= Contact.CollisionFlags.Island;

						// Update other body.
						Body other = cn.Other;

						// Was the other body already added to this island?
						if ((other._flags & Body.BodyFlags.Island) != 0)
						{
							continue;
						}

						// March forward, this can do no harm since this is the min TOI.
						if (other.IsStatic() == false)
						{
							other.Advance(minTOI);
							other.WakeUp();
						}

						Box2DXDebug.Assert(queueStart + queueSize < queueCapacity);
						queue[queueStart + queueSize++] = other;
						other._flags |= Body.BodyFlags.Island;
					}

#if B2_TOI_JOINTS
					for (JointEdge jn = b._jointList; jn!=null; jn = jn.Next)
					{
						if (island._jointCount == island._jointCapacity)
						{
							continue;
						}
						
						if (jn.Joint._islandFlag == true)
						{
							continue;
						}
						
						island.Add(jn.Joint);
						
						jn.Joint._islandFlag = true;
						
						Body other = jn.Other;
						
						if (other._flags & Body.BodyFlags.Island)
						{
							continue;
						}
						
						if (!other.IsStatic())
						{
							other.Advance(minTOI);
							other.WakeUp();
						}
						
						Box2DXDebug.Assert(queueStart + queueSize < queueCapacity);
						queue[queueStart + queueSize++] = other;
						other._flags |= Body.BodyFlags.Island; 
					}
#endif
				}

				TimeStep subStep = new TimeStep();
				subStep.WarmStarting = false;
				subStep.Dt = (1.0f - minTOI) * step.Dt;
				Box2DXDebug.Assert(subStep.Dt > Common.Settings.FLT_EPSILON);
				subStep.Inv_Dt = 1.0f / subStep.Dt;
				subStep.VelocityIterations = step.VelocityIterations;
				subStep.PositionIterations = step.PositionIterations;

				island.SolveTOI(ref subStep);

				// Post solve cleanup.
				for (int i = 0; i < island._bodyCount; ++i)
				{
					// Allow bodies to participate in future TOI islands.
					Body b = island._bodies[i];
					b._flags &= ~Body.BodyFlags.Island;

					if ((b._flags & (Body.BodyFlags.Sleep | Body.BodyFlags.Frozen)) != 0)
					{
						continue;
					}

					if (b.IsStatic())
					{
						continue;
					}

					// Update shapes (for broad-phase). If the shapes go out of
					// the world AABB then shapes and contacts may be destroyed,
					// including contacts that are
					bool inRange = b.SynchronizeShapes();

					// Did the body's shapes leave the world?
					if (inRange == false && _boundaryListener != null)
					{
						_boundaryListener.Violation(b);
					}

					// Invalidate all contact TOIs associated with this body. Some of these
					// may not be in the island because they were not touching.
					for (ContactEdge cn = b._contactList; cn != null; cn = cn.Next)
					{
						cn.Contact._flags &= ~Contact.CollisionFlags.Toi;
					}
				}

				for (int i = 0; i < island._contactCount; ++i)
				{
					// Allow contacts to participate in future TOI islands.
					Contact c = island._contacts[i];
					c._flags &= ~(Contact.CollisionFlags.Toi | Contact.CollisionFlags.Island);
				}

				for (int i = 0; i < island._jointCount; ++i)
				{
					// Allow joints to participate in future TOI islands.
					Joint j = island._joints[i];
					j._islandFlag = false;
				}

				// Commit shape proxy movements to the broad-phase so that new contacts are created.
				// Also, some contacts can be destroyed.
				_broadPhase.Commit();
			}

			queue = null;
		}
Esempio n. 2
0
 private void Solve(TimeStep step)
 {
     Island island = new Island(this._bodyCount, this._contactCount, this._jointCount, this._contactListener);
     for (Body body = this._bodyList; body != null; body = body._next)
     {
         body._flags &= ~Body.BodyFlags.Island;
     }
     for (Contact contact = this._contactList; contact != null; contact = contact._next)
     {
         contact._flags &= ~Contact.CollisionFlags.Island;
     }
     for (Joint joint = this._jointList; joint != null; joint = joint._next)
     {
         joint._islandFlag = false;
     }
     int bodyCount = this._bodyCount;
     Body[] array = new Body[bodyCount];
     for (Body body2 = this._bodyList; body2 != null; body2 = body2._next)
     {
         if ((body2._flags & (Body.BodyFlags.Frozen | Body.BodyFlags.Island | Body.BodyFlags.Sleep)) == (Body.BodyFlags)0)
         {
             if (!body2.IsStatic())
             {
                 island.Clear();
                 int i = 0;
                 array[i++] = body2;
                 body2._flags |= Body.BodyFlags.Island;
                 while (i > 0)
                 {
                     Body body = array[--i];
                     island.Add(body);
                     body._flags &= ~Body.BodyFlags.Sleep;
                     if (!body.IsStatic())
                     {
                         for (ContactEdge contactEdge = body._contactList; contactEdge != null; contactEdge = contactEdge.Next)
                         {
                             if ((contactEdge.Contact._flags & (Contact.CollisionFlags.NonSolid | Contact.CollisionFlags.Island)) == (Contact.CollisionFlags)0)
                             {
                                 if (contactEdge.Contact.GetManifoldCount() != 0)
                                 {
                                     island.Add(contactEdge.Contact);
                                     contactEdge.Contact._flags |= Contact.CollisionFlags.Island;
                                     Body other = contactEdge.Other;
                                     if ((other._flags & Body.BodyFlags.Island) == (Body.BodyFlags)0)
                                     {
                                         Box2DXDebug.Assert(i < bodyCount);
                                         array[i++] = other;
                                         other._flags |= Body.BodyFlags.Island;
                                     }
                                 }
                             }
                         }
                         for (JointEdge jointEdge = body._jointList; jointEdge != null; jointEdge = jointEdge.Next)
                         {
                             if (!jointEdge.Joint._islandFlag)
                             {
                                 island.Add(jointEdge.Joint);
                                 jointEdge.Joint._islandFlag = true;
                                 Body other = jointEdge.Other;
                                 if ((other._flags & Body.BodyFlags.Island) == (Body.BodyFlags)0)
                                 {
                                     Box2DXDebug.Assert(i < bodyCount);
                                     array[i++] = other;
                                     other._flags |= Body.BodyFlags.Island;
                                 }
                             }
                         }
                     }
                 }
                 island.Solve(step, this._gravity, this._allowSleep);
                 for (int j = 0; j < island._bodyCount; j++)
                 {
                     Body body = island._bodies[j];
                     if (body.IsStatic())
                     {
                         body._flags &= ~Body.BodyFlags.Island;
                     }
                 }
             }
         }
     }
     for (Body body = this._bodyList; body != null; body = body.GetNext())
     {
         if ((body._flags & (Body.BodyFlags.Frozen | Body.BodyFlags.Sleep)) == (Body.BodyFlags)0)
         {
             if (!body.IsStatic())
             {
                 if (!body.SynchronizeShapes() && this._boundaryListener != null)
                 {
                     this._boundaryListener.Violation(body);
                 }
             }
         }
     }
     this._broadPhase.Commit();
 }
Esempio n. 3
0
		// Find islands, integrate and solve constraints, solve position constraints
		private void Solve(TimeStep step)
		{
			// Size the island for the worst case.
			Island island = new Island(_bodyCount, _contactCount, _jointCount, _contactListener);

			// Clear all the island flags.
			for (Body b = _bodyList; b != null; b = b._next)
			{
				b._flags &= ~Body.BodyFlags.Island;
			}
			for (Contact c = _contactList; c != null; c = c._next)
			{
				c._flags &= ~Contact.CollisionFlags.Island;
			}
			for (Joint j = _jointList; j != null; j = j._next)
			{
				j._islandFlag = false;
			}

			// Build and simulate all awake islands.
			int stackSize = _bodyCount;
			{
				Body[] stack = new Body[stackSize];

				for (Body seed = _bodyList; seed != null; seed = seed._next)
				{
					if ((seed._flags & (Body.BodyFlags.Island | Body.BodyFlags.Sleep | Body.BodyFlags.Frozen)) != 0)
					{
						continue;
					}

					if (seed.IsStatic())
					{
						continue;
					}

					// Reset island and stack.
					island.Clear();
					int stackCount = 0;
					stack[stackCount++] = seed;
					seed._flags |= Body.BodyFlags.Island;

					// Perform a depth first search (DFS) on the constraint graph.
					while (stackCount > 0)
					{
						// Grab the next body off the stack and add it to the island.
						Body b = stack[--stackCount];
						island.Add(b);

						// Make sure the body is awake.
						b._flags &= ~Body.BodyFlags.Sleep;

						// To keep islands as small as possible, we don't
						// propagate islands across static bodies.
						if (b.IsStatic())
						{
							continue;
						}

						// Search all contacts connected to this body.
						for (ContactEdge cn = b._contactList; cn != null; cn = cn.Next)
						{
							// Has this contact already been added to an island?
							if ((cn.Contact._flags & (Contact.CollisionFlags.Island | Contact.CollisionFlags.NonSolid)) != 0)
							{
								continue;
							}

							// Is this contact touching?
							if (cn.Contact.GetManifoldCount() == 0)
							{
								continue;
							}

							island.Add(cn.Contact);
							cn.Contact._flags |= Contact.CollisionFlags.Island;

							Body other = cn.Other;

							// Was the other body already added to this island?
							if ((other._flags & Body.BodyFlags.Island) != 0)
							{
								continue;
							}

							Box2DXDebug.Assert(stackCount < stackSize);
							stack[stackCount++] = other;
							other._flags |= Body.BodyFlags.Island;
						}

						// Search all joints connect to this body.
						for (JointEdge jn = b._jointList; jn != null; jn = jn.Next)
						{
							if (jn.Joint._islandFlag == true)
							{
								continue;
							}

							island.Add(jn.Joint);
							jn.Joint._islandFlag = true;

							Body other = jn.Other;
							if ((other._flags & Body.BodyFlags.Island) != 0)
							{
								continue;
							}

							Box2DXDebug.Assert(stackCount < stackSize);
							stack[stackCount++] = other;
							other._flags |= Body.BodyFlags.Island;
						}
					}

					island.Solve(step, _gravity, _allowSleep);

					// Post solve cleanup.
					for (int i = 0; i < island._bodyCount; ++i)
					{
						// Allow static bodies to participate in other islands.
						Body b = island._bodies[i];
						if (b.IsStatic())
						{
							b._flags &= ~Body.BodyFlags.Island;
						}
					}
				}

				stack = null;
			}

			// Synchronize shapes, check for out of range bodies.
			for (Body b = _bodyList; b != null; b = b.GetNext())
			{
				if ((b._flags & (Body.BodyFlags.Sleep | Body.BodyFlags.Frozen)) != 0)
				{
					continue;
				}

				if (b.IsStatic())
				{
					continue;
				}

				// Update shapes (for broad-phase). If the shapes go out of
				// the world AABB then shapes and contacts may be destroyed,
				// including contacts that are
				bool inRange = b.SynchronizeShapes();

				// Did the body's shapes leave the world?
				if (inRange == false && _boundaryListener != null)
				{
					_boundaryListener.Violation(b);
				}
			}

			// Commit shape proxy movements to the broad-phase so that new contacts are created.
			// Also, some contacts can be destroyed.
			_broadPhase.Commit();
		}
Esempio n. 4
0
 private void SolveTOI(TimeStep step)
 {
     Island island = new Island(this._bodyCount, Settings.MaxTOIContactsPerIsland, Settings.MaxTOIJointsPerIsland, this._contactListener);
     int bodyCount = this._bodyCount;
     Body[] array = new Body[bodyCount];
     for (Body body = this._bodyList; body != null; body = body._next)
     {
         body._flags &= ~Body.BodyFlags.Island;
         body._sweep.T0 = 0f;
     }
     for (Contact contact = this._contactList; contact != null; contact = contact._next)
     {
         contact._flags &= ~(Contact.CollisionFlags.Island | Contact.CollisionFlags.Toi);
     }
     while (true)
     {
         Contact contact2 = null;
         float num = 1f;
         for (Contact contact = this._contactList; contact != null; contact = contact._next)
         {
             if ((contact._flags & (Contact.CollisionFlags.NonSolid | Contact.CollisionFlags.Slow)) == (Contact.CollisionFlags)0)
             {
                 float num2 = 1f;
                 if ((contact._flags & Contact.CollisionFlags.Toi) != (Contact.CollisionFlags)0)
                 {
                     num2 = contact._toi;
                 }
                 else
                 {
                     Shape shape = contact.GetShape1();
                     Shape shape2 = contact.GetShape2();
                     Body body2 = shape.GetBody();
                     Body body3 = shape2.GetBody();
                     if ((body2.IsStatic() || body2.IsSleeping()) && (body3.IsStatic() || body3.IsSleeping()))
                     {
                         goto IL_2B4;
                     }
                     float t = body2._sweep.T0;
                     if (body2._sweep.T0 < body3._sweep.T0)
                     {
                         t = body3._sweep.T0;
                         body2._sweep.Advance(t);
                     }
                     else
                     {
                         if (body3._sweep.T0 < body2._sweep.T0)
                         {
                             t = body2._sweep.T0;
                             body3._sweep.Advance(t);
                         }
                     }
                     Box2DXDebug.Assert(t < 1f);
                     num2 = Collision.Collision.TimeOfImpact(contact._shape1, body2._sweep, contact._shape2, body3._sweep);
                     Box2DXDebug.Assert(0f <= num2 && num2 <= 1f);
                     if (num2 > 0f && num2 < 1f)
                     {
                         num2 = Box2DX.Common.Math.Min((1f - num2) * t + num2, 1f);
                     }
                     contact._toi = num2;
                     contact._flags |= Contact.CollisionFlags.Toi;
                 }
                 if (Settings.FLT_EPSILON < num2 && num2 < num)
                 {
                     contact2 = contact;
                     num = num2;
                 }
             }
             IL_2B4:;
         }
         if (contact2 == null || 1f - 100f * Settings.FLT_EPSILON < num)
         {
             break;
         }
         Shape shape3 = contact2.GetShape1();
         Shape shape4 = contact2.GetShape2();
         Body body4 = shape3.GetBody();
         Body body5 = shape4.GetBody();
         body4.Advance(num);
         body5.Advance(num);
         contact2.Update(this._contactListener);
         contact2._flags &= ~Contact.CollisionFlags.Toi;
         if (contact2.GetManifoldCount() != 0)
         {
             Body body6 = body4;
             if (body6.IsStatic())
             {
                 body6 = body5;
             }
             island.Clear();
             int num3 = 0;
             int i = 0;
             array[num3 + i++] = body6;
             body6._flags |= Body.BodyFlags.Island;
             while (i > 0)
             {
                 Body body = array[num3++];
                 i--;
                 island.Add(body);
                 body._flags &= ~Body.BodyFlags.Sleep;
                 if (!body.IsStatic())
                 {
                     for (ContactEdge contactEdge = body._contactList; contactEdge != null; contactEdge = contactEdge.Next)
                     {
                         if (island._contactCount != island._contactCapacity)
                         {
                             if ((contactEdge.Contact._flags & (Contact.CollisionFlags.NonSolid | Contact.CollisionFlags.Slow | Contact.CollisionFlags.Island)) == (Contact.CollisionFlags)0)
                             {
                                 if (contactEdge.Contact.GetManifoldCount() != 0)
                                 {
                                     island.Add(contactEdge.Contact);
                                     contactEdge.Contact._flags |= Contact.CollisionFlags.Island;
                                     Body other = contactEdge.Other;
                                     if ((other._flags & Body.BodyFlags.Island) == (Body.BodyFlags)0)
                                     {
                                         if (!other.IsStatic())
                                         {
                                             other.Advance(num);
                                             other.WakeUp();
                                         }
                                         Box2DXDebug.Assert(num3 + i < bodyCount);
                                         array[num3 + i++] = other;
                                         other._flags |= Body.BodyFlags.Island;
                                     }
                                 }
                             }
                         }
                     }
                 }
             }
             TimeStep timeStep = default(TimeStep);
             timeStep.WarmStarting = false;
             timeStep.Dt = (1f - num) * step.Dt;
             Box2DXDebug.Assert(timeStep.Dt > Settings.FLT_EPSILON);
             timeStep.Inv_Dt = 1f / timeStep.Dt;
             timeStep.VelocityIterations = step.VelocityIterations;
             timeStep.PositionIterations = step.PositionIterations;
             island.SolveTOI(ref timeStep);
             for (int j = 0; j < island._bodyCount; j++)
             {
                 Body body = island._bodies[j];
                 body._flags &= ~Body.BodyFlags.Island;
                 if ((body._flags & (Body.BodyFlags.Frozen | Body.BodyFlags.Sleep)) == (Body.BodyFlags)0)
                 {
                     if (!body.IsStatic())
                     {
                         if (!body.SynchronizeShapes() && this._boundaryListener != null)
                         {
                             this._boundaryListener.Violation(body);
                         }
                         for (ContactEdge contactEdge = body._contactList; contactEdge != null; contactEdge = contactEdge.Next)
                         {
                             contactEdge.Contact._flags &= ~Contact.CollisionFlags.Toi;
                         }
                     }
                 }
             }
             for (int j = 0; j < island._contactCount; j++)
             {
                 Contact contact = island._contacts[j];
                 contact._flags &= ~(Contact.CollisionFlags.Island | Contact.CollisionFlags.Toi);
             }
             for (int j = 0; j < island._jointCount; j++)
             {
                 Joint joint = island._joints[j];
                 joint._islandFlag = false;
             }
             this._broadPhase.Commit();
         }
     }
 }
Esempio n. 5
0
        // Find TOI contacts and solve them.
        private void SolveTOI(TimeStep step)
        {
            // Reserve an island and a queue for TOI island solution.
            Island island = new Island(_bodyCount,
                                       Settings.MaxTOIContactsPerIsland,
                                       Settings.MaxTOIJointsPerIsland,
                                       _contactManager._contactListener);

            //Simple one pass queue
            //Relies on the fact that we're only making one pass
            //through and each body can only be pushed/popped once.
            //To push:
            //  queue[queueStart+queueSize++] = newElement;
            //To pop:
            //	poppedElement = queue[queueStart++];
            //  --queueSize;
            int queueCapacity = _bodyCount;

            Body[] queue = new Body[queueCapacity];

            for (Body b = _bodyList; b != null; b = b._next)
            {
                b._flags   &= ~Body.BodyFlags.Island;
                b._sweep.T0 = 0.0f;
            }

            for (Contact c = _contactManager._contactList; c != null; c = c.Next)
            {
                // Invalidate TOI
                c.Flags &= ~(ContactFlag.ToiFlag | ContactFlag.IslandFlag);
            }

            for (Joint j = _jointList; j != null; j = j._next)
            {
                j._islandFlag = false;
            }

            // Find TOI events and solve them.
            for (; ;)
            {
                // Find the first TOI.
                Contact minContact = null;
                float   minTOI     = 1.0f;

                for (Contact c = _contactManager._contactList; c != null; c = c.Next)
                {
                    // Can this contact generate a solid TOI contact?
                    if (c.IsSolid() == false || c.IsContinuous() == false)
                    {
                        continue;
                    }

                    // TODO_ERIN keep a counter on the contact, only respond to M TOIs per contact.

                    float toi = 1.0f;
                    if ((c.Flags & ContactFlag.ToiFlag) != 0)
                    {
                        // This contact has a valid cached TOI.
                        toi = c.Toi;
                    }
                    else
                    {
                        // Compute the TOI for this contact.
                        Fixture s1 = c.GetFixtureA();
                        Fixture s2 = c.GetFixtureB();
                        Body    b1 = s1.GetBody();
                        Body    b2 = s2.GetBody();

                        if ((b1.IsStatic() || b1.IsSleeping()) && (b2.IsStatic() || b2.IsSleeping()))
                        {
                            continue;
                        }

                        // Put the sweeps onto the same time interval.
                        float t0 = b1._sweep.T0;

                        if (b1._sweep.T0 < b2._sweep.T0)
                        {
                            t0 = b2._sweep.T0;
                            b1._sweep.Advance(t0);
                        }
                        else if (b2._sweep.T0 < b1._sweep.T0)
                        {
                            t0 = b1._sweep.T0;
                            b2._sweep.Advance(t0);
                        }

                        Box2DXDebug.Assert(t0 < 1.0f);

                        // Compute the time of impact.
                        toi = c.ComputeTOI(b1._sweep, b2._sweep);

                        Box2DXDebug.Assert(0.0f <= toi && toi <= 1.0f);

                        // If the TOI is in range ...
                        if (0.0f < toi && toi < 1.0f)
                        {
                            // Interpolate on the actual range.
                            toi = Math.Min((1.0f - toi) * t0 + toi, 1.0f);
                        }


                        c.Toi    = toi;
                        c.Flags |= ContactFlag.ToiFlag;
                    }

                    if (Settings.FLT_EPSILON < toi && toi < minTOI)
                    {
                        // This is the minimum TOI found so far.
                        minContact = c;
                        minTOI     = toi;
                    }
                }

                if (minContact == null || 1.0f - 100.0f * Settings.FLT_EPSILON < minTOI)
                {
                    // No more TOI events. Done!
                    break;
                }

                // Advance the bodies to the TOI.
                Fixture f1 = minContact.GetFixtureA();
                Fixture f2 = minContact.GetFixtureB();
                Body    b3 = f1.GetBody();
                Body    b4 = f2.GetBody();

                Sweep backup1 = b3._sweep;
                Sweep backup2 = b4._sweep;

                b3.Advance(minTOI);
                b4.Advance(minTOI);

                // The TOI contact likely has some new contact points.
                minContact.Update(_contactManager._contactListener);
                minContact.Flags &= ~ContactFlag.ToiFlag;

                // Is the contact solid?
                if (minContact.IsSolid() == false)
                {
                    // Restore the sweeps.
                    b3._sweep = backup1;
                    b4._sweep = backup2;
                    b3.SynchronizeTransform();
                    b4.SynchronizeTransform();
                    continue;
                }

                // Did numerical issues prevent a contact point from being generated?
                if (minContact.IsTouching() == false)
                {
                    // Give up on this TOI.
                    continue;
                }

                // Build the TOI island. We need a dynamic seed.
                Body seed = b3;
                if (seed.IsStatic())
                {
                    seed = b4;
                }

                // Reset island and queue.
                island.Clear();

                int queueStart = 0; // starting index for queue
                int queueSize  = 0; // elements in queue
                queue[queueStart + queueSize++] = seed;
                seed._flags |= Body.BodyFlags.Island;

                // Perform a breadth first search (BFS) on the contact/joint graph.
                while (queueSize > 0)
                {
                    // Grab the next body off the stack and add it to the island.
                    Body b = queue[queueStart++];
                    --queueSize;

                    island.Add(ref b);

                    // Make sure the body is awake.
                    b._flags &= ~Body.BodyFlags.Sleep;

                    // To keep islands as small as possible, we don't
                    // propagate islands across static bodies.
                    if (b.IsStatic())
                    {
                        continue;
                    }

                    // Search all contacts connected to this body.
                    for (ContactEdge cEdge = b._contactList; cEdge != null; cEdge = cEdge.Next)
                    {
                        // Does the TOI island still have space for contacts?
                        if (island.ContactCount == island.ContactCapacity)
                        {
                            break;
                        }

                        // Has this contact already been added to an island? Skip slow or non-solid contacts.
                        if ((cEdge.Contact.Flags & ContactFlag.IslandFlag) != 0)
                        {
                            continue;
                        }

                        // Is this contact touching? For performance we are not updating this contact.
                        if (cEdge.Contact.IsSolid() == false || cEdge.Contact.IsTouching() == false)
                        {
                            continue;
                        }

                        island.Add(ref cEdge.Contact);
                        cEdge.Contact.Flags |= ContactFlag.IslandFlag;

                        // Update other body.
                        Body other = cEdge.Other;

                        // Was the other body already added to this island?
                        if ((other._flags & Body.BodyFlags.Island) != 0)
                        {
                            continue;
                        }

                        // March forward, this can do no harm since this is the min TOI.
                        if (other.IsStatic() == false)
                        {
                            other.Advance(minTOI);
                            other.WakeUp();
                        }

                        Box2DXDebug.Assert(queueStart + queueSize < queueCapacity);
                        queue[queueStart + queueSize] = other;
                        ++queueSize;
                        other._flags |= Body.BodyFlags.Island;
                    }

                    for (JointEdge jEdge = b._jointList; jEdge != null; jEdge = jEdge.Next)
                    {
                        if (island.JointCount == island.JointCapacity)
                        {
                            continue;
                        }

                        if (jEdge.Joint._islandFlag == true)
                        {
                            continue;
                        }

                        island.Add(jEdge.Joint);

                        jEdge.Joint._islandFlag = true;

                        Body other = jEdge.Other;

                        if ((other._flags & Body.BodyFlags.Island) != 0)
                        {
                            continue;
                        }

                        if (!other.IsStatic())
                        {
                            other.Advance(minTOI);
                            other.WakeUp();
                        }

                        Box2DXDebug.Assert(queueStart + queueSize < queueCapacity);
                        queue[queueStart + queueSize] = other;
                        ++queueSize;
                        other._flags |= Body.BodyFlags.Island;
                    }
                }

                TimeStep subStep;
                subStep.WarmStarting       = false;
                subStep.Dt                 = (1.0f - minTOI) * step.Dt;
                subStep.Inv_Dt             = 1.0f / subStep.Dt;
                subStep.DtRatio            = 0.0f;
                subStep.VelocityIterations = step.VelocityIterations;
                subStep.PositionIterations = step.PositionIterations;

                island.SolveTOI(ref subStep);

                // Post solve cleanup.
                for (int i = 0; i < island.BodyCount; ++i)
                {
                    // Allow bodies to participate in future TOI islands.
                    Body b = island.Bodies[i];
                    b._flags &= ~Body.BodyFlags.Island;

                    if ((b._flags & Body.BodyFlags.Sleep) != 0)
                    {
                        continue;
                    }

                    if (b.IsStatic())
                    {
                        continue;
                    }

                    b.SynchronizeFixtures();

                    // Invalidate all contact TOIs associated with this body. Some of these
                    // may not be in the island because they were not touching.
                    for (ContactEdge ce = b._contactList; ce != null; ce = ce.Next)
                    {
                        ce.Contact.Flags &= ~ContactFlag.ToiFlag;
                    }
                }

                for (int i = 0; i < island.ContactCount; ++i)
                {
                    // Allow contacts to participate in future TOI islands.
                    Contact c = island.Contacts[i];
                    c.Flags &= ~(ContactFlag.ToiFlag | ContactFlag.IslandFlag);
                }

                for (int i = 0; i < island.JointCount; ++i)
                {
                    // Allow joints to participate in future TOI islands.
                    Joint j = island.Joints[i];
                    j._islandFlag = false;
                }

                // Commit fixture proxy movements to the broad-phase so that new contacts are created.
                // Also, some contacts can be destroyed.
                _contactManager.FindNewContacts();
            }

            queue = null;
        }
Esempio n. 6
0
        // Find islands, integrate and solve constraints, solve position constraints
        private void Solve(TimeStep step)
        {
            // Size the island for the worst case.
            Island island = new Island(_bodyCount,
                                       _contactManager._contactCount,
                                       _jointCount,
                                       _contactManager._contactListener);

            // Clear all the island flags.
            for (Body b = _bodyList; b != null; b = b._next)
            {
                b._flags &= ~Body.BodyFlags.Island;
            }
            for (Contact c = _contactManager._contactList; c != null; c = c.Next)
            {
                c.Flags &= ~ContactFlag.IslandFlag;
            }
            for (Joint j = _jointList; j != null; j = j._next)
            {
                j._islandFlag = false;
            }


            // Build and simulate all awake islands.
            int stackSize = _bodyCount;

            Body[] stack = new Body[stackSize];
            for (Body seed = _bodyList; seed != null; seed = seed._next)
            {
                if ((seed._flags & (Body.BodyFlags.Island | Body.BodyFlags.Sleep)) != 0)
                {
                    continue;
                }

                if (seed.IsStatic())
                {
                    continue;
                }

                // Reset island and stack.
                island.Clear();
                int stackCount = 0;
                stack[stackCount++] = seed;
                seed._flags        |= Body.BodyFlags.Island;

                // Perform a depth first search (DFS) on the constraint graph.
                while (stackCount > 0)
                {
                    // Grab the next body off the stack and add it to the island.
                    Body b = stack[--stackCount];
                    island.Add(ref b);

                    // Make sure the body is awake.
                    b._flags &= ~Body.BodyFlags.Sleep;

                    // To keep islands as small as possible, we don't
                    // propagate islands across static bodies.
                    if (b.IsStatic())
                    {
                        continue;
                    }

                    // Search all contacts connected to this body.
                    for (ContactEdge ce = b._contactList; ce != null; ce = ce.Next)
                    {
                        // Has this contact already been added to an island?
                        if ((ce.Contact.Flags & ContactFlag.IslandFlag) != 0)
                        {
                            continue;
                        }

                        // Is this contact touching?
                        if (ce.Contact.IsSolid() == false || ce.Contact.IsTouching() == false)
                        {
                            continue;
                        }

                        island.Add(ref ce.Contact);
                        ce.Contact.Flags |= ContactFlag.IslandFlag;

                        Body other = ce.Other;

                        // Was the other body already added to this island?
                        if ((other._flags & Body.BodyFlags.Island) != 0)
                        {
                            continue;
                        }

                        Box2DXDebug.Assert(stackCount < stackSize);
                        stack[stackCount++] = other;
                        other._flags       |= Body.BodyFlags.Island;
                    }

                    // Search all joints connect to this body.
                    for (JointEdge je = b._jointList; je != null; je = je.Next)
                    {
                        if (je.Joint._islandFlag == true)
                        {
                            continue;
                        }

                        island.Add(je.Joint);
                        je.Joint._islandFlag = true;

                        Body other = je.Other;
                        if ((other._flags & Body.BodyFlags.Island) != 0)
                        {
                            continue;
                        }

                        Box2DXDebug.Assert(stackCount < stackSize);
                        stack[stackCount++] = other;
                        other._flags       |= Body.BodyFlags.Island;
                    }
                }

                island.Solve(step, _gravity, _allowSleep);

                // Post solve cleanup.
                for (int i = 0; i < island.BodyCount; ++i)
                {
                    // Allow static bodies to participate in other islands.
                    Body b = island.Bodies[i];
                    if (b.IsStatic())
                    {
                        b._flags &= ~Body.BodyFlags.Island;
                    }
                }
            }

            stack = null;

            // Synchronize shapes, check for out of range bodies.
            for (Body b = _bodyList; b != null; b = b.GetNext())
            {
                if ((b._flags & Body.BodyFlags.Sleep) != 0)
                {
                    continue;
                }

                if (b.IsStatic())
                {
                    continue;
                }

                // Update fixtures (for broad-phase).
                b.SynchronizeFixtures();
            }

            // Look for new contacts.
            _contactManager.FindNewContacts();
        }
Esempio n. 7
0
        // Find TOI contacts and solve them.
        private void SolveTOI(TimeStep step)
        {
            // Reserve an island and a queue for TOI island solution.
            Island island = new Island(_bodyCount,
                            Settings.MaxTOIContactsPerIsland,
                             Settings.MaxTOIJointsPerIsland,
                            _contactManager._contactListener);

            //Simple one pass queue
            //Relies on the fact that we're only making one pass
            //through and each body can only be pushed/popped once.
            //To push:
            //  queue[queueStart+queueSize++] = newElement;
            //To pop:
            //	poppedElement = queue[queueStart++];
            //  --queueSize;
            int queueCapacity = _bodyCount;
            Body[] queue = new Body[queueCapacity];

            for (Body b = _bodyList; b != null; b = b._next)
            {
                b._flags &= ~Body.BodyFlags.Island;
                b._sweep.T0 = 0.0f;
            }

            for (Contact c = _contactManager._contactList; c != null; c = c.Next)
            {
                // Invalidate TOI
                c.Flags &= ~(ContactFlag.ToiFlag | ContactFlag.IslandFlag);
            }

            for (Joint j = _jointList; j != null; j = j._next)
            {
                j._islandFlag = false;
            }

            // Find TOI events and solve them.
            for (; ; )
            {
                // Find the first TOI.
                Contact minContact = null;
                float minTOI = 1.0f;

                for (Contact c = _contactManager._contactList; c != null; c = c.Next)
                {
                    // Can this contact generate a solid TOI contact?
                    if (c.IsSolid() == false || c.IsContinuous() == false)
                    {
                        continue;
                    }

                    // TODO_ERIN keep a counter on the contact, only respond to M TOIs per contact.

                    float toi = 1.0f;
                    if ((c.Flags & ContactFlag.ToiFlag) != 0)
                    {
                        // This contact has a valid cached TOI.
                        toi = c.Toi;
                    }
                    else
                    {
                        // Compute the TOI for this contact.
                        Fixture s1 = c.GetFixtureA();
                        Fixture s2 = c.GetFixtureB();
                        Body b1 = s1.GetBody();
                        Body b2 = s2.GetBody();

                        if ((b1.IsStatic() || b1.IsSleeping()) && (b2.IsStatic() || b2.IsSleeping()))
                        {
                            continue;
                        }

                        // Put the sweeps onto the same time interval.
                        float t0 = b1._sweep.T0;

                        if (b1._sweep.T0 < b2._sweep.T0)
                        {
                            t0 = b2._sweep.T0;
                            b1._sweep.Advance(t0);
                        }
                        else if (b2._sweep.T0 < b1._sweep.T0)
                        {
                            t0 = b1._sweep.T0;
                            b2._sweep.Advance(t0);
                        }

                        Box2DXDebug.Assert(t0 < 1.0f);

                        // Compute the time of impact.
                        toi = c.ComputeTOI(b1._sweep, b2._sweep);

                        Box2DXDebug.Assert(0.0f <= toi && toi <= 1.0f);

                        // If the TOI is in range ...
                        if (0.0f < toi && toi < 1.0f)
                        {
                            // Interpolate on the actual range.
                            toi = Math.Min((1.0f - toi) * t0 + toi, 1.0f);
                        }

                        c.Toi = toi;
                        c.Flags |= ContactFlag.ToiFlag;
                    }

                    if (Settings.FLT_EPSILON < toi && toi < minTOI)
                    {
                        // This is the minimum TOI found so far.
                        minContact = c;
                        minTOI = toi;
                    }
                }

                if (minContact == null || 1.0f - 100.0f * Settings.FLT_EPSILON < minTOI)
                {
                    // No more TOI events. Done!
                    break;
                }

                // Advance the bodies to the TOI.
                Fixture f1 = minContact.GetFixtureA();
                Fixture f2 = minContact.GetFixtureB();
                Body b3 = f1.GetBody();
                Body b4 = f2.GetBody();

                Sweep backup1 = b3._sweep;
                Sweep backup2 = b4._sweep;

                b3.Advance(minTOI);
                b4.Advance(minTOI);

                // The TOI contact likely has some new contact points.
                minContact.Update(_contactManager._contactListener);
                minContact.Flags &= ~ContactFlag.ToiFlag;

                // Is the contact solid?
                if (minContact.IsSolid() == false)
                {
                    // Restore the sweeps.
                    b3._sweep = backup1;
                    b4._sweep = backup2;
                    b3.SynchronizeTransform();
                    b4.SynchronizeTransform();
                    continue;
                }

                // Did numerical issues prevent a contact point from being generated?
                if (minContact.IsTouching() == false)
                {
                    // Give up on this TOI.
                    continue;
                }

                // Build the TOI island. We need a dynamic seed.
                Body seed = b3;
                if (seed.IsStatic())
                {
                    seed = b4;
                }

                // Reset island and queue.
                island.Clear();

                int queueStart = 0; // starting index for queue
                int queueSize = 0;  // elements in queue
                queue[queueStart + queueSize++] = seed;
                seed._flags |= Body.BodyFlags.Island;

                // Perform a breadth first search (BFS) on the contact/joint graph.
                while (queueSize > 0)
                {
                    // Grab the next body off the stack and add it to the island.
                    Body b = queue[queueStart++];
                    --queueSize;

                    island.Add(ref b);

                    // Make sure the body is awake.
                    b._flags &= ~Body.BodyFlags.Sleep;

                    // To keep islands as small as possible, we don't
                    // propagate islands across static bodies.
                    if (b.IsStatic())
                    {
                        continue;
                    }

                    // Search all contacts connected to this body.
                    for (ContactEdge cEdge = b._contactList; cEdge != null; cEdge = cEdge.Next)
                    {
                        // Does the TOI island still have space for contacts?
                        if (island.ContactCount == island.ContactCapacity)
                        {
                            break;
                        }

                        // Has this contact already been added to an island? Skip slow or non-solid contacts.
                        if ((cEdge.Contact.Flags & ContactFlag.IslandFlag) != 0)
                        {
                            continue;
                        }

                        // Is this contact touching? For performance we are not updating this contact.
                        if (cEdge.Contact.IsSolid() == false || cEdge.Contact.IsTouching() == false)
                        {
                            continue;
                        }

                        island.Add(ref cEdge.Contact);
                        cEdge.Contact.Flags |= ContactFlag.IslandFlag;

                        // Update other body.
                        Body other = cEdge.Other;

                        // Was the other body already added to this island?
                        if ((other._flags & Body.BodyFlags.Island) != 0)
                        {
                            continue;
                        }

                        // March forward, this can do no harm since this is the min TOI.
                        if (other.IsStatic() == false)
                        {
                            other.Advance(minTOI);
                            other.WakeUp();
                        }

                        Box2DXDebug.Assert(queueStart + queueSize < queueCapacity);
                        queue[queueStart + queueSize] = other;
                        ++queueSize;
                        other._flags |= Body.BodyFlags.Island;
                    }

                    for (JointEdge jEdge = b._jointList; jEdge != null; jEdge = jEdge.Next)
                    {
                        if (island.JointCount == island.JointCapacity)
                        {
                            continue;
                        }

                        if (jEdge.Joint._islandFlag == true)
                        {
                            continue;
                        }

                        island.Add(jEdge.Joint);

                        jEdge.Joint._islandFlag = true;

                        Body other = jEdge.Other;

                        if ((other._flags & Body.BodyFlags.Island) != 0)
                        {
                            continue;
                        }

                        if (!other.IsStatic())
                        {
                            other.Advance(minTOI);
                            other.WakeUp();
                        }

                        Box2DXDebug.Assert(queueStart + queueSize < queueCapacity);
                        queue[queueStart + queueSize] = other;
                        ++queueSize;
                        other._flags |= Body.BodyFlags.Island;
                    }
                }

                TimeStep subStep;
                subStep.WarmStarting = false;
                subStep.Dt = (1.0f - minTOI) * step.Dt;
                subStep.Inv_Dt = 1.0f / subStep.Dt;
                subStep.DtRatio = 0.0f;
                subStep.VelocityIterations = step.VelocityIterations;
                subStep.PositionIterations = step.PositionIterations;

                island.SolveTOI(ref subStep);

                // Post solve cleanup.
                for (int i = 0; i < island.BodyCount; ++i)
                {
                    // Allow bodies to participate in future TOI islands.
                    Body b = island.Bodies[i];
                    b._flags &= ~Body.BodyFlags.Island;

                    if ((b._flags & Body.BodyFlags.Sleep) != 0)
                    {
                        continue;
                    }

                    if (b.IsStatic())
                    {
                        continue;
                    }

                    b.SynchronizeFixtures();

                    // Invalidate all contact TOIs associated with this body. Some of these
                    // may not be in the island because they were not touching.
                    for (ContactEdge ce = b._contactList; ce != null; ce = ce.Next)
                    {
                        ce.Contact.Flags &= ~ContactFlag.ToiFlag;
                    }
                }

                for (int i = 0; i < island.ContactCount; ++i)
                {
                    // Allow contacts to participate in future TOI islands.
                    Contact c = island.Contacts[i];
                    c.Flags &= ~(ContactFlag.ToiFlag | ContactFlag.IslandFlag);
                }

                for (int i = 0; i < island.JointCount; ++i)
                {
                    // Allow joints to participate in future TOI islands.
                    Joint j = island.Joints[i];
                    j._islandFlag = false;
                }

                // Commit fixture proxy movements to the broad-phase so that new contacts are created.
                // Also, some contacts can be destroyed.
                _contactManager.FindNewContacts();
            }

            queue = null;
        }
Esempio n. 8
0
        // Find islands, integrate and solve constraints, solve position constraints
        private void Solve(TimeStep step)
        {
            // Size the island for the worst case.
            Island island = new Island(_bodyCount,
                            _contactManager._contactCount,
                            _jointCount,
                            _contactManager._contactListener);

            // Clear all the island flags.
            for (Body b = _bodyList; b != null; b = b._next)
            {
                b._flags &= ~Body.BodyFlags.Island;
            }
            for (Contact c = _contactManager._contactList; c != null; c = c.Next)
            {
                c.Flags &= ~ContactFlag.IslandFlag;
            }
            for (Joint j = _jointList; j != null; j = j._next)
            {
                j._islandFlag = false;
            }

            // Build and simulate all awake islands.
            int stackSize = _bodyCount;
            Body[] stack = new Body[stackSize];
            for (Body seed = _bodyList; seed != null; seed = seed._next)
            {
                if ((seed._flags & (Body.BodyFlags.Island | Body.BodyFlags.Sleep)) != 0)
                {
                    continue;
                }

                if (seed.IsStatic())
                {
                    continue;
                }

                // Reset island and stack.
                island.Clear();
                int stackCount = 0;
                stack[stackCount++] = seed;
                seed._flags |= Body.BodyFlags.Island;

                // Perform a depth first search (DFS) on the constraint graph.
                while (stackCount > 0)
                {
                    // Grab the next body off the stack and add it to the island.
                    Body b = stack[--stackCount];
                    island.Add(ref b);

                    // Make sure the body is awake.
                    b._flags &= ~Body.BodyFlags.Sleep;

                    // To keep islands as small as possible, we don't
                    // propagate islands across static bodies.
                    if (b.IsStatic())
                    {
                        continue;
                    }

                    // Search all contacts connected to this body.
                    for (ContactEdge ce = b._contactList; ce != null; ce = ce.Next)
                    {
                        // Has this contact already been added to an island?
                        if ((ce.Contact.Flags & ContactFlag.IslandFlag) != 0)
                        {
                            continue;
                        }

                        // Is this contact touching?
                        if (ce.Contact.IsSolid() == false || ce.Contact.IsTouching() == false)
                        {
                            continue;
                        }

                        island.Add(ref ce.Contact);
                        ce.Contact.Flags |= ContactFlag.IslandFlag;

                        Body other = ce.Other;

                        // Was the other body already added to this island?
                        if ((other._flags & Body.BodyFlags.Island) != 0)
                        {
                            continue;
                        }

                        Box2DXDebug.Assert(stackCount < stackSize);
                        stack[stackCount++] = other;
                        other._flags |= Body.BodyFlags.Island;
                    }

                    // Search all joints connect to this body.
                    for (JointEdge je = b._jointList; je != null; je = je.Next)
                    {
                        if (je.Joint._islandFlag == true)
                        {
                            continue;
                        }

                        island.Add(je.Joint);
                        je.Joint._islandFlag = true;

                        Body other = je.Other;
                        if ((other._flags & Body.BodyFlags.Island) != 0)
                        {
                            continue;
                        }

                        Box2DXDebug.Assert(stackCount < stackSize);
                        stack[stackCount++] = other;
                        other._flags |= Body.BodyFlags.Island;
                    }
                }

                island.Solve(step, _gravity, _allowSleep);

                // Post solve cleanup.
                for (int i = 0; i < island.BodyCount; ++i)
                {
                    // Allow static bodies to participate in other islands.
                    Body b = island.Bodies[i];
                    if (b.IsStatic())
                    {
                        b._flags &= ~Body.BodyFlags.Island;
                    }
                }
            }

            stack = null;

            // Synchronize shapes, check for out of range bodies.
            for (Body b = _bodyList; b != null; b = b.GetNext())
            {
                if ((b._flags & Body.BodyFlags.Sleep) != 0)
                {
                    continue;
                }

                if (b.IsStatic())
                {
                    continue;
                }

                // Update fixtures (for broad-phase).
                b.SynchronizeFixtures();
            }

            // Look for new contacts.
            _contactManager.FindNewContacts();
        }