private void SolveToi(TimeStep step)
{
Island island = toiIsland;
island.Init(2 * Settings.MAX_TOI_CONTACTS, Settings.MAX_TOI_CONTACTS, 0, ContactManager.ContactListener);
if (m_stepComplete)
{
for (Body b = BodyList; b != null; b = b.Next)
{
b.Flags &= ~Body.TypeFlags.Island;
b.Sweep.Alpha0 = 0.0f;
}
for (Contact c = ContactManager.ContactList; c != null; c = c.Next)
{
// Invalidate TOI
c.Flags &= ~(Contact.ContactFlags.ToiFlag | Contact.ContactFlags.Island);
c.ToiCount = 0;
c.Toi = 1.0f;
}
}
// Find TOI events and solve them.
for (; ; )
{
// Find the first TOI.
Contact minContact = null;
float minAlpha = 1.0f;
for (Contact c = ContactManager.ContactList; c != null; c = c.Next)
{
// Is this contact disabled?
if (c.Enabled == false)
{
continue;
}
// Prevent excessive sub-stepping.
if (c.ToiCount > Settings.MAX_SUB_STEPS)
{
continue;
}
float alpha;
if ((c.Flags & Contact.ContactFlags.ToiFlag) != 0)
{
// This contact has a valid cached TOI.
alpha = c.Toi;
}
else
{
Fixture fA = c.FixtureA;
Fixture fB = c.FixtureB;
// Is there a sensor?
if (fA.Sensor || fB.Sensor)
{
continue;
}
Body bA = fA.Body;
Body bB = fB.Body;
BodyType typeA = bA.Type;
BodyType typeB = bB.Type;
Debug.Assert(typeA == BodyType.Dynamic || typeB == BodyType.Dynamic);
bool activeA = bA.Awake && typeA != BodyType.Static;
bool activeB = bB.Awake && typeB != BodyType.Static;
// Is at least one body active (awake and dynamic or kinematic)?
if (activeA == false && activeB == false)
{
continue;
}
bool collideA = bA.Bullet || typeA != BodyType.Dynamic;
bool collideB = bB.Bullet || typeB != BodyType.Dynamic;
// Are these two non-bullet dynamic bodies?
if (collideA == false && collideB == false)
{
continue;
}
// Compute the TOI for this contact.
// Put the sweeps onto the same time interval.
float alpha0 = bA.Sweep.Alpha0;
if (bA.Sweep.Alpha0 < bB.Sweep.Alpha0)
{
alpha0 = bB.Sweep.Alpha0;
bA.Sweep.Advance(alpha0);
}
else if (bB.Sweep.Alpha0 < bA.Sweep.Alpha0)
{
alpha0 = bA.Sweep.Alpha0;
bB.Sweep.Advance(alpha0);
}
Debug.Assert(alpha0 < 1.0f);
int indexA = c.ChildIndexA;
int indexB = c.ChildIndexB;
// Compute the time of impact in interval [0, minTOI]
TimeOfImpact.TOIInput input = toiInput;
input.ProxyA.Set(fA.Shape, indexA);
input.ProxyB.Set(fB.Shape, indexB);
input.SweepA.Set(bA.Sweep);
input.SweepB.Set(bB.Sweep);
input.tMax = 1.0f;
Pool.GetTimeOfImpact().GetTimeOfImpact(toiOutput, input);
// Beta is the fraction of the remaining portion of the .
float beta = toiOutput.T;
if (toiOutput.State == TimeOfImpact.TOIOutputState.Touching)
{
alpha = MathUtils.Min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
}
else
{
alpha = 1.0f;
}
c.Toi = alpha;
c.Flags |= Contact.ContactFlags.ToiFlag;
}
if (alpha < minAlpha)
{
// This is the minimum TOI found so far.
minContact = c;
minAlpha = alpha;
}
}
if (minContact == null || 1.0f - 10.0f * Settings.EPSILON < minAlpha)
{
// No more TOI events. Done!
m_stepComplete = true;
break;
}
// Advance the bodies to the TOI.
Fixture fA2 = minContact.FixtureA;
Fixture fB2 = minContact.FixtureB;
Body bA2 = fA2.Body;
Body bB2 = fB2.Body;
backup1.Set(bA2.Sweep);
backup2.Set(bB2.Sweep);
bA2.Advance(minAlpha);
bB2.Advance(minAlpha);
// The TOI contact likely has some new contact points.
minContact.Update(ContactManager.ContactListener);
minContact.Flags &= ~Contact.ContactFlags.ToiFlag;
++minContact.ToiCount;
// Is the contact solid?
if (minContact.Enabled == false || minContact.Touching == false)
{
// Restore the sweeps.
minContact.Enabled = false;
bA2.Sweep.Set(backup1);
bB2.Sweep.Set(backup2);
bA2.SynchronizeTransform();
bB2.SynchronizeTransform();
continue;
}
bA2.Awake = true;
bB2.Awake = true;
// Build the island
island.Clear();
island.Add(bA2);
island.Add(bB2);
island.Add(minContact);
bA2.Flags |= Body.TypeFlags.Island;
bB2.Flags |= Body.TypeFlags.Island;
minContact.Flags |= Contact.ContactFlags.Island;
// Get contacts on bodyA and bodyB.
tempBodies[0] = bA2;
tempBodies[1] = bB2;
for (int i = 0; i < 2; ++i)
{
Body body = tempBodies[i];
if (body.Type == BodyType.Dynamic)
{
for (ContactEdge ce = body.ContactList; ce != null; ce = ce.Next)
{
if (island.BodyCount == island.BodyCapacity)
{
break;
}
if (island.ContactCount == island.ContactCapacity)
{
break;
}
Contact contact = ce.Contact;
// Has this contact already been added to the island?
if ((contact.Flags & Contact.ContactFlags.Island) != 0)
{
continue;
}
// Only add static, kinematic, or bullet bodies.
Body other = ce.Other;
if (other.Type == BodyType.Dynamic && body.Bullet == false && other.Bullet == false)
{
continue;
}
// Skip sensors.
bool sensorA = contact.FixtureA.IsSensor;
bool sensorB = contact.FixtureB.IsSensor;
if (sensorA || sensorB)
{
continue;
}
// Tentatively advance the body to the TOI.
backup1.Set(other.Sweep);
if ((other.Flags & Body.TypeFlags.Island) == 0)
{
other.Advance(minAlpha);
}
// Update the contact points
contact.Update(ContactManager.ContactListener);
// Was the contact disabled by the user?
if (contact.Enabled == false)
{
other.Sweep.Set(backup1);
other.SynchronizeTransform();
continue;
}
// Are there contact points?
if (contact.Touching == false)
{
other.Sweep.Set(backup1);
other.SynchronizeTransform();
continue;
}
// Add the contact to the island
contact.Flags |= Contact.ContactFlags.Island;
island.Add(contact);
// Has the other body already been added to the island?
if ((other.Flags & Body.TypeFlags.Island) != 0)
{
continue;
}
// Add the other body to the island.
other.Flags |= Body.TypeFlags.Island;
if (other.Type != BodyType.Static)
{
other.Awake = true;
}
island.Add(other);
}
}
}
subStep.Dt = (1.0f - minAlpha) * step.Dt;
subStep.InvDt = 1.0f / subStep.Dt;
subStep.DtRatio = 1.0f;
subStep.PositionIterations = 20;
subStep.VelocityIterations = step.VelocityIterations;
subStep.WarmStarting = false;
island.SolveToi(subStep, bA2.IslandIndex, bB2.IslandIndex);
// Reset island flags and synchronize broad-phase proxies.
for (int i = 0; i < island.BodyCount; ++i)
{
Body body = island.Bodies[i];
body.Flags &= ~Body.TypeFlags.Island;
if (body.Type != BodyType.Dynamic)
{
continue;
}
body.SynchronizeFixtures();
// Invalidate all contact TOIs on this displaced body.
for (ContactEdge ce = body.ContactList; ce != null; ce = ce.Next)
{
ce.Contact.Flags &= ~(Contact.ContactFlags.ToiFlag | Contact.ContactFlags.ToiFlag);
}
}
// Commit fixture proxy movements to the broad-phase so that new contacts are created.
// Also, some contacts can be destroyed.
ContactManager.FindNewContacts();
if (m_subStepping)
{
m_stepComplete = false;
break;
}
}
}