// Advance a dynamic body to its first time of contact
// and adjust the position to ensure clearance.
void SolveTOI(Body body)
{
// Find the minimum contact.
Contact toiContact = null;
float toi = 1.0f;
bool found;
int count;
int iter = 0;
bool bullet = body.IsBullet;
// Iterate until all contacts agree on the minimum TOI. We have
// to iterate because the TOI algorithm may skip some intermediate
// collisions when objects rotate through each other.
do
{
count = 0;
found = false;
for (ContactEdge ce = body._contactList; ce != null; ce = ce.Next)
{
Body other = ce.Other;
BodyType type = other.GetType();
// Only bullets perform TOI with dynamic bodies.
if (bullet == true)
{
// Bullets only perform TOI with bodies that have their TOI resolved.
if ((other._flags & BodyFlags.Toi) == 0)
{
continue;
}
}
else if (type == BodyType.Dynamic)
{
continue;
}
// Check for a disabled contact.
Contact contact = ce.Contact;
if (contact.IsEnabled() == false)
{
continue;
}
// Prevent infinite looping.
if (contact._toiCount > 10)
{
continue;
}
Fixture fixtureA = contact._fixtureA;
Fixture fixtureB = contact._fixtureB;
// Cull sensors.
if (fixtureA.IsSensor() || fixtureB.IsSensor())
{
continue;
}
Body bodyA = fixtureA._body;
Body bodyB = fixtureB._body;
// Compute the time of impact in interval [0, minTOI]
TOIInput input = new TOIInput();
input.proxyA.Set(fixtureA.GetShape());
input.proxyB.Set(fixtureB.GetShape());
input.sweepA = bodyA._sweep;
input.sweepB = bodyB._sweep;
input.tMax = toi;
TOIOutput output;
TimeOfImpact.CalculateTimeOfImpact(out output, ref input);
if (output.State == TOIOutputState.Touching && output.t < toi)
{
toiContact = contact;
toi = output.t;
found = true;
}
++count;
}
++iter;
} while (found && count > 1 && iter < 50);
if (toiContact == null)
{
return;
}
// Advance the body to its safe time.
Sweep backup = body._sweep;
body.Advance(toi);
++toiContact._toiCount;
// Update all the valid contacts on this body and build a contact island.
count = 0;
for (ContactEdge ce = body._contactList; (ce != null) && (count < Settings.b2_maxTOIContacts); ce = ce.Next)
{
Body other = ce.Other;
BodyType type = other.GetType();
// Only perform correction with static bodies, so the
// body won't get pushed out of the world.
if (type != BodyType.Static)
{
continue;
}
// Check for a disabled contact.
Contact contact = ce.Contact;
if (contact.IsEnabled() == false)
{
continue;
}
Fixture fixtureA = contact._fixtureA;
Fixture fixtureB = contact._fixtureB;
// Cull sensors.
if (fixtureA.IsSensor() || fixtureB.IsSensor())
{
continue;
}
// The contact likely has some new contact points. The listener
// gives the user a chance to disable the contact;
contact.Update(_contactManager.ContactListener);
// Did the user disable the contact?
if (contact.IsEnabled() == false)
{
if (contact == toiContact)
{
// Restore the body's sweep.
body._sweep = backup;
body.SynchronizeTransform();
// Recurse because the TOI has been invalidated.
SolveTOI(body);
return;
}
// Skip this contact.
continue;
}
if (contact.IsTouching() == false)
{
continue;
}
_toiContacts[count] = contact;
++count;
}
// Reduce the TOI body's overlap with the contact island.
_toiSolver.Initialize(_toiContacts, count, body);
const float k_toiBaumgarte = 0.75f;
//bool solved = false;
for (int i = 0; i < 20; ++i)
{
bool contactsOkay = _toiSolver.Solve(k_toiBaumgarte);
if (contactsOkay)
{
//solved = true;
break;
}
}
}