float32 b2PolygonContact::ComputeTOI(const b2Sweep& sweepA, const b2Sweep& sweepB) const
{
b2TOIInput input;
input.sweepA = sweepA;
input.sweepB = sweepB;
input.tolerance = b2_linearSlop;
return b2TimeOfImpact(&input, (const b2PolygonShape*)m_fixtureA->GetShape(), (const b2PolygonShape*)m_fixtureB->GetShape());
}
float32 b2EdgeAndCircleContact::ComputeTOI(const b2Sweep& sweepA, const b2Sweep& sweepB) const
{
b2TOIInput input;
input.sweepA = sweepA;
input.sweepB = sweepB;
input.sweepRadiusA = m_fixtureA->ComputeSweepRadius(sweepA.localCenter);
input.sweepRadiusB = m_fixtureB->ComputeSweepRadius(sweepB.localCenter);
input.tolerance = b2_linearSlop;
return b2TimeOfImpact(&input, (const b2EdgeShape*)m_fixtureA->GetShape(), (const b2CircleShape*)m_fixtureB->GetShape());
}
float32 b2Contact::ComputeTOI(const b2Sweep& sweepA, const b2Sweep& sweepB) const
{
b2TOIInput input;
input.proxyA.Set(m_fixtureA->GetShape());
input.proxyB.Set(m_fixtureB->GetShape());
input.sweepA = sweepA;
input.sweepB = sweepB;
input.tolerance = b2_linearSlop;
return b2TimeOfImpact(&input);
}
// Find TOI contacts and solve them.
void b2World::SolveTOI(const b2TimeStep& step)
{
b2Island island(2 * b2_maxTOIContacts, b2_maxTOIContacts, 0, &m_stackAllocator, m_contactManager.m_contactListener);
if (m_stepComplete)
{
for (b2Body* b = m_bodyList; b; b = b->m_next)
{
b->m_flags &= ~b2Body::e_islandFlag;
b->m_sweep.alpha0 = 0.0f;
}
for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next)
{
// Invalidate TOI
c->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag);
c->m_toiCount = 0;
c->m_toi = 1.0f;
}
}
// Find TOI events and solve them.
for (;;)
{
// Find the first TOI.
b2Contact* minContact = NULL;
float32 minAlpha = 1.0f;
for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next)
{
// Is this contact disabled?
if (c->IsEnabled() == false)
{
continue;
}
// Prevent excessive sub-stepping.
if (c->m_toiCount > b2_maxSubSteps)
{
continue;
}
float32 alpha = 1.0f;
if (c->m_flags & b2Contact::e_toiFlag)
{
// This contact has a valid cached TOI.
alpha = c->m_toi;
}
else
{
b2Fixture* fA = c->GetFixtureA();
b2Fixture* fB = c->GetFixtureB();
// Is there a sensor?
if (fA->IsSensor() || fB->IsSensor())
{
continue;
}
b2Body* bA = fA->GetBody();
b2Body* bB = fB->GetBody();
b2BodyType typeA = bA->m_type;
b2BodyType typeB = bB->m_type;
b2Assert(typeA == b2_dynamicBody || typeB == b2_dynamicBody);
bool activeA = bA->IsAwake() && typeA != b2_staticBody;
bool activeB = bB->IsAwake() && typeB != b2_staticBody;
// Is at least one body active (awake and dynamic or kinematic)?
if (activeA == false && activeB == false)
{
continue;
}
bool collideA = bA->IsBullet() || typeA != b2_dynamicBody;
bool collideB = bB->IsBullet() || typeB != b2_dynamicBody;
// 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.
float32 alpha0 = bA->m_sweep.alpha0;
if (bA->m_sweep.alpha0 < bB->m_sweep.alpha0)
{
alpha0 = bB->m_sweep.alpha0;
bA->m_sweep.Advance(alpha0);
}
else if (bB->m_sweep.alpha0 < bA->m_sweep.alpha0)
{
alpha0 = bA->m_sweep.alpha0;
bB->m_sweep.Advance(alpha0);
}
b2Assert(alpha0 < 1.0f);
int32 indexA = c->GetChildIndexA();
int32 indexB = c->GetChildIndexB();
// Compute the time of impact in interval [0, minTOI]
b2TOIInput input;
input.proxyA.Set(fA->GetShape(), indexA);
input.proxyB.Set(fB->GetShape(), indexB);
input.sweepA = bA->m_sweep;
input.sweepB = bB->m_sweep;
input.tMax = 1.0f;
b2TOIOutput output;
b2TimeOfImpact(&output, &input);
// Beta is the fraction of the remaining portion of the .
float32 beta = output.t;
if (output.state == b2TOIOutput::e_touching)
{
alpha = b2Min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
}
else
{
alpha = 1.0f;
}
c->m_toi = alpha;
c->m_flags |= b2Contact::e_toiFlag;
}
if (alpha < minAlpha)
{
// This is the minimum TOI found so far.
minContact = c;
minAlpha = alpha;
}
}
if (minContact == NULL || 1.0f - 10.0f * b2_epsilon < minAlpha)
{
// No more TOI events. Done!
m_stepComplete = true;
break;
}
// Advance the bodies to the TOI.
b2Fixture* fA = minContact->GetFixtureA();
b2Fixture* fB = minContact->GetFixtureB();
b2Body* bA = fA->GetBody();
b2Body* bB = fB->GetBody();
b2Sweep backup1 = bA->m_sweep;
b2Sweep backup2 = bB->m_sweep;
bA->Advance(minAlpha);
bB->Advance(minAlpha);
// The TOI contact likely has some new contact points.
minContact->Update(m_contactManager.m_contactListener);
minContact->m_flags &= ~b2Contact::e_toiFlag;
++minContact->m_toiCount;
// Is the contact solid?
if (minContact->IsEnabled() == false || minContact->IsTouching() == false)
{
// Restore the sweeps.
minContact->SetEnabled(false);
bA->m_sweep = backup1;
bB->m_sweep = backup2;
bA->SynchronizeTransform();
bB->SynchronizeTransform();
continue;
}
bA->SetAwake(true);
bB->SetAwake(true);
// Build the island
island.Clear();
island.Add(bA);
island.Add(bB);
island.Add(minContact);
bA->m_flags |= b2Body::e_islandFlag;
bB->m_flags |= b2Body::e_islandFlag;
minContact->m_flags |= b2Contact::e_islandFlag;
// Get contacts on bodyA and bodyB.
b2Body* bodies[2] = {bA, bB};
for (int32 i = 0; i < 2; ++i)
{
b2Body* body = bodies[i];
if (body->m_type == b2_dynamicBody)
{
for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next)
{
if (island.m_bodyCount == island.m_bodyCapacity)
{
break;
}
if (island.m_contactCount == island.m_contactCapacity)
{
break;
}
b2Contact* contact = ce->contact;
// Has this contact already been added to the island?
if (contact->m_flags & b2Contact::e_islandFlag)
{
continue;
}
// Only add static, kinematic, or bullet bodies.
b2Body* other = ce->other;
if (other->m_type == b2_dynamicBody &&
body->IsBullet() == false && other->IsBullet() == false)
{
continue;
}
// Skip sensors.
bool sensorA = contact->m_fixtureA->m_isSensor;
bool sensorB = contact->m_fixtureB->m_isSensor;
if (sensorA || sensorB)
{
continue;
}
// Tentatively advance the body to the TOI.
b2Sweep backup = other->m_sweep;
if ((other->m_flags & b2Body::e_islandFlag) == 0)
{
other->Advance(minAlpha);
}
// Update the contact points
contact->Update(m_contactManager.m_contactListener);
// Was the contact disabled by the user?
if (contact->IsEnabled() == false)
{
other->m_sweep = backup;
other->SynchronizeTransform();
continue;
}
// Are there contact points?
if (contact->IsTouching() == false)
{
other->m_sweep = backup;
other->SynchronizeTransform();
continue;
}
// Add the contact to the island
contact->m_flags |= b2Contact::e_islandFlag;
island.Add(contact);
// Has the other body already been added to the island?
if (other->m_flags & b2Body::e_islandFlag)
{
continue;
}
// Add the other body to the island.
other->m_flags |= b2Body::e_islandFlag;
if (other->m_type != b2_staticBody)
{
other->SetAwake(true);
}
island.Add(other);
}
}
}
b2TimeStep subStep;
subStep.dt = (1.0f - minAlpha) * step.dt;
subStep.inv_dt = 1.0f / subStep.dt;
subStep.dtRatio = 1.0f;
subStep.positionIterations = 20;
subStep.velocityIterations = step.velocityIterations;
subStep.warmStarting = false;
island.SolveTOI(subStep, bA->m_islandIndex, bB->m_islandIndex);
// Reset island flags and synchronize broad-phase proxies.
for (int32 i = 0; i < island.m_bodyCount; ++i)
{
b2Body* body = island.m_bodies[i];
body->m_flags &= ~b2Body::e_islandFlag;
if (body->m_type != b2_dynamicBody)
{
continue;
}
body->SynchronizeFixtures();
// Invalidate all contact TOIs on this displaced body.
for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next)
{
ce->contact->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag);
}
}
// Commit fixture proxy movements to the broad-phase so that new contacts are created.
// Also, some contacts can be destroyed.
m_contactManager.FindNewContacts();
if (m_subStepping)
{
m_stepComplete = false;
break;
}
}
}
// Advance a dynamic body to its first time of contact
// and adjust the position to ensure clearance.
void b2World::SolveTOI(b2Body* body)
{
// Find the minimum contact.
b2Contact* toiContact = NULL;
float32 toi = 1.0f;
b2Body* toiOther = NULL;
bool found;
int32 count;
int32 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 (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next)
{
if (ce->contact == toiContact)
{
continue;
}
b2Body* other = ce->other;
b2BodyType 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->m_flags & b2Body::e_toiFlag) == 0)
{
continue;
}
// No repeated hits on non-static bodies
if (type != b2_staticBody && (ce->contact->m_flags & b2Contact::e_bulletHitFlag) != 0)
{
continue;
}
}
else if (type == b2_dynamicBody)
{
continue;
}
// Check for a disabled contact.
b2Contact* contact = ce->contact;
if (contact->IsEnabled() == false)
{
continue;
}
// Prevent infinite looping.
if (contact->m_toiCount > 10)
{
continue;
}
b2Fixture* fixtureA = contact->m_fixtureA;
b2Fixture* fixtureB = contact->m_fixtureB;
// Cull sensors.
if (fixtureA->IsSensor() || fixtureB->IsSensor())
{
continue;
}
b2Body* bodyA = fixtureA->m_body;
b2Body* bodyB = fixtureB->m_body;
// Compute the time of impact in interval [0, minTOI]
b2TOIInput input;
input.proxyA.Set(fixtureA->GetShape());
input.proxyB.Set(fixtureB->GetShape());
input.sweepA = bodyA->m_sweep;
input.sweepB = bodyB->m_sweep;
input.tMax = toi;
b2TOIOutput output;
b2TimeOfImpact(&output, &input);
if (output.state == b2TOIOutput::e_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;
}
b2Sweep backup = body->m_sweep;
body->Advance(toi);
toiContact->Update(m_contactManager.m_contactListener);
if (toiContact->IsEnabled() == false)
{
// Contact disabled. Backup and recurse.
body->m_sweep = backup;
SolveTOI(body);
}
++toiContact->m_toiCount;
// Update all the valid contacts on this body and build a contact island.
b2Contact* contacts[b2_maxTOIContacts];
count = 0;
for (b2ContactEdge* ce = body->m_contactList; ce && count < b2_maxTOIContacts; ce = ce->next)
{
b2Body* other = ce->other;
b2BodyType type = other->GetType();
// Only perform correction with static bodies, so the
// body won't get pushed out of the world.
if (type == b2_dynamicBody)
{
continue;
}
// Check for a disabled contact.
b2Contact* contact = ce->contact;
if (contact->IsEnabled() == false)
{
continue;
}
b2Fixture* fixtureA = contact->m_fixtureA;
b2Fixture* fixtureB = contact->m_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(m_contactManager.m_contactListener);
}
// Did the user disable the contact?
if (contact->IsEnabled() == false)
{
// Skip this contact.
continue;
}
if (contact->IsTouching() == false)
{
continue;
}
contacts[count] = contact;
++count;
}
// Reduce the TOI body's overlap with the contact island.
b2TOISolver solver(&m_stackAllocator);
solver.Initialize(contacts, count, body);
const float32 k_toiBaumgarte = 0.75f;
bool solved = false;
for (int32 i = 0; i < 20; ++i)
{
bool contactsOkay = solver.Solve(k_toiBaumgarte);
if (contactsOkay)
{
solved = true;
break;
}
}
if (toiOther->GetType() != b2_staticBody)
{
toiContact->m_flags |= b2Contact::e_bulletHitFlag;
}
}
// Find TOI contacts and solve them.
void b2World::SolveTOI(const b2TimeStep& step)
{
// Reserve an island and a queue for TOI island solution.
b2Island island(m_bodyCount, b2_maxTOIContactsPerIsland, b2_maxTOIJointsPerIsland, &m_stackAllocator, m_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;
int32 queueCapacity = m_bodyCount;
b2Body** queue = (b2Body**)m_stackAllocator.Allocate(queueCapacity* sizeof(b2Body*));
for (b2Body* b = m_bodyList; b; b = b->m_next)
{
b->m_flags &= ~b2Body::e_islandFlag;
b->m_sweep.t0 = 0.0f;
}
for (b2Contact* c = m_contactList; c; c = c->m_next)
{
// Invalidate TOI
c->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag);
}
for (b2Joint* j = m_jointList; j; j = j->m_next)
{
j->m_islandFlag = false;
}
// Find TOI events and solve them.
for (;;)
{
// Find the first TOI.
b2Contact* minContact = NULL;
float32 minTOI = 1.0f;
for (b2Contact* c = m_contactList; c; c = c->m_next)
{
if (c->m_flags & (b2Contact::e_slowFlag | b2Contact::e_nonSolidFlag))
{
continue;
}
// TODO_ERIN keep a counter on the contact, only respond to M TOIs per contact.
float32 toi = 1.0f;
if (c->m_flags & b2Contact::e_toiFlag)
{
// This contact has a valid cached TOI.
toi = c->m_toi;
}
else
{
// Compute the TOI for this contact.
b2Shape* s1 = c->GetShape1();
b2Shape* s2 = c->GetShape2();
b2Body* b1 = s1->GetBody();
b2Body* b2 = s2->GetBody();
if ((b1->IsStatic() || b1->IsSleeping()) && (b2->IsStatic() || b2->IsSleeping()))
{
continue;
}
// Put the sweeps onto the same time interval.
float32 t0 = b1->m_sweep.t0;
if (b1->m_sweep.t0 < b2->m_sweep.t0)
{
t0 = b2->m_sweep.t0;
b1->m_sweep.Advance(t0);
}
else if (b2->m_sweep.t0 < b1->m_sweep.t0)
{
t0 = b1->m_sweep.t0;
b2->m_sweep.Advance(t0);
}
b2Assert(t0 < 1.0f);
// Compute the time of impact.
toi = b2TimeOfImpact(c->m_shape1, b1->m_sweep, c->m_shape2, b2->m_sweep);
b2Assert(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 = b2Min((1.0f - toi) * t0 + toi, 1.0f);
}
c->m_toi = toi;
c->m_flags |= b2Contact::e_toiFlag;
}
if (B2_FLT_EPSILON < toi && toi < minTOI)
{
// This is the minimum TOI found so far.
minContact = c;
minTOI = toi;
}
}
if (minContact == NULL || 1.0f - 100.0f * B2_FLT_EPSILON < minTOI)
{
// No more TOI events. Done!
break;
}
// Advance the bodies to the TOI.
b2Shape* s1 = minContact->GetShape1();
b2Shape* s2 = minContact->GetShape2();
b2Body* b1 = s1->GetBody();
b2Body* b2 = s2->GetBody();
b1->Advance(minTOI);
b2->Advance(minTOI);
// The TOI contact likely has some new contact points.
minContact->Update(m_contactListener);
minContact->m_flags &= ~b2Contact::e_toiFlag;
if (minContact->GetManifoldCount() == 0)
{
// This shouldn't happen. Numerical error?
//b2Assert(false);
continue;
}
// Build the TOI island. We need a dynamic seed.
b2Body* seed = b1;
if (seed->IsStatic())
{
seed = b2;
}
// Reset island and queue.
island.Clear();
int32 queueStart = 0; // starting index for queue
int32 queueSize = 0; // elements in queue
queue[queueStart + queueSize++] = seed;
seed->m_flags |= b2Body::e_islandFlag;
// 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.
b2Body* b = queue[queueStart++];
--queueSize;
island.Add(b);
// Make sure the body is awake.
b->m_flags &= ~b2Body::e_sleepFlag;
// 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 (b2ContactEdge* cEdge = b->m_contactList; cEdge; cEdge = cEdge->next)
{
// Does the TOI island still have space for contacts?
if (island.m_contactCount == island.m_contactCapacity)
{
continue;
}
// Has this contact already been added to an island? Skip slow or non-solid contacts.
if (cEdge->contact->m_flags & (b2Contact::e_islandFlag | b2Contact::e_slowFlag | b2Contact::e_nonSolidFlag))
{
continue;
}
// Is this contact touching? For performance we are not updating this contact.
if (cEdge->contact->GetManifoldCount() == 0)
{
continue;
}
island.Add(cEdge->contact);
cEdge->contact->m_flags |= b2Contact::e_islandFlag;
// Update other body.
b2Body* other = cEdge->other;
// Was the other body already added to this island?
if (other->m_flags & b2Body::e_islandFlag)
{
continue;
}
// March forward, this can do no harm since this is the min TOI.
if (other->IsStatic() == false)
{
other->Advance(minTOI);
other->WakeUp();
}
b2Assert(queueStart + queueSize < queueCapacity);
queue[queueStart + queueSize] = other;
++queueSize;
other->m_flags |= b2Body::e_islandFlag;
}
for (b2JointEdge* jEdge = b->m_jointList; jEdge; jEdge = jEdge->next)
{
if (island.m_jointCount == island.m_jointCapacity)
{
continue;
}
if (jEdge->joint->m_islandFlag == true)
{
continue;
}
island.Add(jEdge->joint);
jEdge->joint->m_islandFlag = true;
b2Body* other = jEdge->other;
if (other->m_flags & b2Body::e_islandFlag)
{
continue;
}
if (!other->IsStatic())
{
other->Advance(minTOI);
other->WakeUp();
}
b2Assert(queueStart + queueSize < queueCapacity);
queue[queueStart + queueSize] = other;
++queueSize;
other->m_flags |= b2Body::e_islandFlag;
}
}
b2TimeStep 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(subStep);
// Post solve cleanup.
for (int32 i = 0; i < island.m_bodyCount; ++i)
{
// Allow bodies to participate in future TOI islands.
b2Body* b = island.m_bodies[i];
b->m_flags &= ~b2Body::e_islandFlag;
if (b->m_flags & (b2Body::e_sleepFlag | b2Body::e_frozenFlag))
{
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 && m_boundaryListener != NULL)
{
m_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 (b2ContactEdge* cn = b->m_contactList; cn; cn = cn->next)
{
cn->contact->m_flags &= ~b2Contact::e_toiFlag;
}
}
for (int32 i = 0; i < island.m_contactCount; ++i)
{
// Allow contacts to participate in future TOI islands.
b2Contact* c = island.m_contacts[i];
c->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag);
}
for (int32 i = 0; i < island.m_jointCount; ++i)
{
// Allow joints to participate in future TOI islands.
b2Joint* j = island.m_joints[i];
j->m_islandFlag = false;
}
// Commit shape proxy movements to the broad-phase so that new contacts are created.
// Also, some contacts can be destroyed.
m_broadPhase->Commit();
}
m_stackAllocator.Free(queue);
}
