/// <summary>
// Advance a dynamic body to its first time of contact
// and adjust the position to ensure clearance.
/// </summary>
/// <param name="body">The body.</param>
private void SolveTOI(Body body)
{
// Find the minimum contact.
Contact toiContact = null;
float toi = 1.0f;
Body toiOther = null;
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)
{
if (ce.Contact == toiContact)
{
continue;
}
Body other = ce.Other;
BodyType type = other.BodyType;
// Only bullets perform TOI with dynamic bodies.
if (bullet)
{
// Bullets only perform TOI with bodies that have their TOI resolved.
if ((other.Flags & BodyFlags.Toi) == 0)
{
continue;
}
// No repeated hits on non-static bodies
if (type != BodyType.Static && (ce.Contact.Flags & ContactFlags.BulletHit) != 0)
{
continue;
}
}
else if (type == BodyType.Dynamic)
{
continue;
}
// Check for a disabled contact.
Contact contact = ce.Contact;
if (contact.Enabled == false)
{
continue;
}
// Prevent infinite looping.
if (contact.TOICount > 10)
{
continue;
}
Fixture fixtureA = contact.FixtureA;
Fixture fixtureB = contact.FixtureB;
int indexA = contact.ChildIndexA;
int indexB = contact.ChildIndexB;
// 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.Shape, indexA);
input.ProxyB.Set(fixtureB.Shape, indexB);
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;
toiOther = other;
found = true;
}
++count;
}
++iter;
} while (found && count > 1 && iter < 50);
if (toiContact == null)
{
body.Advance(1.0f);
return;
}
Sweep backup = body.Sweep;
body.Advance(toi);
toiContact.Update(ContactManager);
if (toiContact.Enabled == false)
{
// Contact disabled. Backup and recurse.
body.Sweep = backup;
SolveTOI(body);
}
++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.MaxTOIContacts); ce = ce.Next)
{
Body other = ce.Other;
BodyType type = other.BodyType;
// Only perform correction with static bodies, so the
// body won't get pushed out of the world.
if (type == BodyType.Dynamic)
{
continue;
}
// Check for a disabled contact.
Contact contact = ce.Contact;
if (contact.Enabled == 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.
if (contact != toiContact)
{
contact.Update(ContactManager);
}
// Did the user disable the contact?
if (contact.Enabled == false)
{
// 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;
}
}
if (toiOther.BodyType != BodyType.Static)
{
toiContact.Flags |= ContactFlags.BulletHit;
}
}