void Island::SolveTOI(const PTimeStep& subStep, s32 toiIndexA, s32 toiIndexB)
{
assert(toiIndexA < m_bodyCount);
assert(toiIndexB < m_bodyCount);
// Initialize the body state.
for (s32 i = 0; i < m_bodyCount; ++i)
{
Body* b = m_bodies[i];
m_positions[i].c = b->m_sweep.c;
m_positions[i].a = b->m_sweep.a;
m_velocities[i].v = b->m_linearVelocity;
m_velocities[i].w = b->m_angularVelocity;
}
ContactSolverDef contactSolverDef;
contactSolverDef.contacts = m_contacts;
contactSolverDef.count = m_contactCount;
contactSolverDef.allocator = m_allocator;
contactSolverDef.step = subStep;
contactSolverDef.positions = m_positions;
contactSolverDef.velocities = m_velocities;
ContactSolver contactSolver(&contactSolverDef);
// Solve position constraints.
for (s32 i = 0; i < subStep.positionIterations; ++i)
{
bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
if (contactsOkay)
{
break;
}
}
#if 0
// Is the new position really safe?
for (s32 i = 0; i < m_contactCount; ++i)
{
Contact* c = m_contacts[i];
Fixture* fA = c->GetFixtureA();
Fixture* fB = c->GetFixtureB();
Body* bA = fA->GetBody();
Body* bB = fB->GetBody();
s32 indexA = c->GetChildIndexA();
s32 indexB = c->GetChildIndexB();
DistanceInput input;
input.proxyA.Set(fA->GetShape(), indexA);
input.proxyB.Set(fB->GetShape(), indexB);
input.Transform2DA = bA->GetTransform2D();
input.Transform2DB = bB->GetTransform2D();
input.useRadii = false;
DistanceOutput output;
SimplexCache cache;
cache.count = 0;
Distance(&output, &cache, &input);
if (output.distance == 0 || cache.count == 3)
{
cache.count += 0;
}
}
#endif
// Leap of faith to new safe state.
m_bodies[toiIndexA]->m_sweep.c0 = m_positions[toiIndexA].c;
m_bodies[toiIndexA]->m_sweep.a0 = m_positions[toiIndexA].a;
m_bodies[toiIndexB]->m_sweep.c0 = m_positions[toiIndexB].c;
m_bodies[toiIndexB]->m_sweep.a0 = m_positions[toiIndexB].a;
// No warm starting is needed for TOI events because warm
// starting impulses were applied in the discrete solver.
contactSolver.InitializeVelocityConstraints();
// Solve velocity constraints.
for (s32 i = 0; i < subStep.velocityIterations; ++i)
{
contactSolver.SolveVelocityConstraints();
}
// Don't store the TOI contact forces for warm starting
// because they can be quite large.
real32 h = subStep.delta;
// Integrate positions
for (s32 i = 0; i < m_bodyCount; ++i)
{
glm::vec2 c = m_positions[i].c;
real32 a = m_positions[i].a;
glm::vec2 v = m_velocities[i].v;
real32 w = m_velocities[i].w;
// Check for large velocities
glm::vec2 translation = h * v;
if (glm::dot(translation, translation) > maxTranslationSquared)
{
real32 ratio = maxTranslation / translation.length();
v *= ratio;
}
real32 rotation = h * w;
if (rotation * rotation > maxRotationSquared)
{
real32 ratio = maxRotation / glm::abs(rotation);
w *= ratio;
}
// Integrate
c += h * v;
a += h * w;
m_positions[i].c = c;
m_positions[i].a = a;
m_velocities[i].v = v;
m_velocities[i].w = w;
// Sync bodies
Body* body = m_bodies[i];
body->m_sweep.c = c;
body->m_sweep.a = a;
body->m_linearVelocity = v;
body->m_angularVelocity = w;
body->SynchronizeTransform2D();
}
Report(contactSolver.m_velocityConstraints);
}
// This is the top level collision call for the time step. Here
// all the narrow phase collision is processed for the world
// contact list.
void ContactManager::Collide()
{
// Update awake contacts.
Contact* c = m_contactList;
while (c)
{
Fixture* fixtureA = c->GetFixtureA();
Fixture* fixtureB = c->GetFixtureB();
s32 indexA = c->GetChildIndexA();
s32 indexB = c->GetChildIndexB();
Body* bodyA = fixtureA->GetBody();
Body* bodyB = fixtureB->GetBody();
// Is this contact flagged for filtering?
if (c->m_flags & Contact::filterFlag)
{
// Should these bodies collide?
if (bodyB->ShouldCollide(bodyA) == false)
{
Contact* cNuke = c;
c = cNuke->GetNext();
Destroy(cNuke);
continue;
}
// Check user filtering.
if (m_contactFilter && m_contactFilter->ShouldCollide(fixtureA, fixtureB) == false)
{
Contact* cNuke = c;
c = cNuke->GetNext();
Destroy(cNuke);
continue;
}
// Clear the filtering flag.
c->m_flags &= ~Contact::filterFlag;
}
bool activeA = bodyA->IsAwake() && bodyA->m_type != staticBody;
bool activeB = bodyB->IsAwake() && bodyB->m_type != staticBody;
// At least one body must be awake and it must be dynamic or kinematic.
if (activeA == false && activeB == false)
{
c = c->GetNext();
continue;
}
s32 proxyIdA = fixtureA->m_proxies[indexA].proxyId;
s32 proxyIdB = fixtureB->m_proxies[indexB].proxyId;
bool overlap = m_broadPhase.TestOverlap(proxyIdA, proxyIdB);
// Here we destroy contacts that cease to overlap in the broad-phase.
if (overlap == false)
{
Contact* cNuke = c;
c = cNuke->GetNext();
Destroy(cNuke);
continue;
}
// The contact persists.
c->Update(m_contactListener);
c = c->GetNext();
}
}
void Body::DestroyFixture(Fixture* fixture)
{
assert(m_world->IsLocked() == false);
if (m_world->IsLocked() == true)
{
return;
}
assert(fixture->m_body == this);
// Remove the fixture from this body's singly linked list.
assert(m_fixtureCount > 0);
Fixture** node = &m_fixtureList;
bool found = false;
while (*node != NULL)
{
if (*node == fixture)
{
*node = fixture->m_next;
found = true;
break;
}
node = &(*node)->m_next;
}
// You tried to remove a shape that is not attached to this body.
assert(found);
// Destroy any contacts associated with the fixture.
ContactEdge* edge = m_contactList;
while (edge)
{
Contact* c = edge->contact;
edge = edge->next;
Fixture* fixtureA = c->GetFixtureA();
Fixture* fixtureB = c->GetFixtureB();
if (fixture == fixtureA || fixture == fixtureB)
{
// This destroys the contact and removes it from
// this body's contact list.
m_world->m_contactManager.Destroy(c);
}
}
BlockAllocator* allocator = &m_world->m_blockAllocator;
if (m_flags & activeFlag)
{
BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
fixture->DestroyProxies(broadPhase);
}
fixture->Destroy(allocator);
fixture->m_body = NULL;
fixture->m_next = NULL;
fixture->~Fixture();
allocator->Free(fixture, sizeof(Fixture));
--m_fixtureCount;
// Reset the mass data.
ResetMassData();
}
void ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB)
{
FixtureProxy* proxyA = (FixtureProxy*)proxyUserDataA;
FixtureProxy* proxyB = (FixtureProxy*)proxyUserDataB;
Fixture* fixtureA = proxyA->fixture;
Fixture* fixtureB = proxyB->fixture;
s32 indexA = proxyA->childIndex;
s32 indexB = proxyB->childIndex;
Body* bodyA = fixtureA->GetBody();
Body* bodyB = fixtureB->GetBody();
// Are the fixtures on the same body?
if (bodyA == bodyB)
{
return;
}
// TODO_ERIN use a hash table to remove a potential bottleneck when both
// bodies have a lot of contacts.
// Does a contact already exist?
ContactEdge* edge = bodyB->GetContactList();
while (edge)
{
if (edge->other == bodyA)
{
Fixture* fA = edge->contact->GetFixtureA();
Fixture* fB = edge->contact->GetFixtureB();
s32 iA = edge->contact->GetChildIndexA();
s32 iB = edge->contact->GetChildIndexB();
if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB)
{
// A contact already exists.
return;
}
if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA)
{
// A contact already exists.
return;
}
}
edge = edge->next;
}
// Does a joint override collision? Is at least one body dynamic?
if (bodyB->ShouldCollide(bodyA) == false)
{
return;
}
// Check user filtering.
if (m_contactFilter && m_contactFilter->ShouldCollide(fixtureA, fixtureB) == false)
{
return;
}
// Call the factory.
Contact* c = Contact::Create(fixtureA, indexA, fixtureB, indexB, m_allocator);
if (c == NULL)
{
return;
}
// Contact creation may swap fixtures.
fixtureA = c->GetFixtureA();
fixtureB = c->GetFixtureB();
indexA = c->GetChildIndexA();
indexB = c->GetChildIndexB();
bodyA = fixtureA->GetBody();
bodyB = fixtureB->GetBody();
// Insert into the world.
c->m_prev = NULL;
c->m_next = m_contactList;
if (m_contactList != NULL)
{
m_contactList->m_prev = c;
}
m_contactList = c;
// Connect to island graph.
// Connect to body A
c->m_nodeA.contact = c;
c->m_nodeA.other = bodyB;
c->m_nodeA.prev = NULL;
c->m_nodeA.next = bodyA->m_contactList;
if (bodyA->m_contactList != NULL)
{
bodyA->m_contactList->prev = &c->m_nodeA;
}
bodyA->m_contactList = &c->m_nodeA;
// Connect to body B
c->m_nodeB.contact = c;
c->m_nodeB.other = bodyA;
c->m_nodeB.prev = NULL;
c->m_nodeB.next = bodyB->m_contactList;
if (bodyB->m_contactList != NULL)
{
bodyB->m_contactList->prev = &c->m_nodeB;
}
bodyB->m_contactList = &c->m_nodeB;
// Wake up the bodies
if (fixtureA->IsSensor() == false && fixtureB->IsSensor() == false)
{
bodyA->SetAwake(true);
bodyB->SetAwake(true);
}
++m_contactCount;
}
