// 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;
}
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();
}
// 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();
}
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();
}
}
}
// 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;
}
// 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();
}
// 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;
}
// 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();
}