inline void Contact::GetWorldManifold(WorldManifold* worldManifold) const
{
const Body* bodyA = m_fixtureA->GetBody();
const Body* bodyB = m_fixtureB->GetBody();
const Shape* shapeA = m_fixtureA->GetShape();
const Shape* shapeB = m_fixtureB->GetShape();
worldManifold->Initialize(&m_manifold, bodyA->GetTransform(), shapeA->m_radius, bodyB->GetTransform(), shapeB->m_radius);
}
void Contact::Destroy(Contact* contact, BlockAllocator* allocator)
{
assert(s_initialized == true);
Fixture* fixtureA = contact->m_fixtureA;
Fixture* fixtureB = contact->m_fixtureB;
if (contact->m_manifold.pointCount > 0 &&
fixtureA->IsSensor() == false &&
fixtureB->IsSensor() == false)
{
fixtureA->GetBody()->SetAwake(true);
fixtureB->GetBody()->SetAwake(true);
}
Shape::Type typeA = fixtureA->GetType();
Shape::Type typeB = fixtureB->GetType();
assert(0 <= typeA && typeB < Shape::e_typeCount);
assert(0 <= typeA && typeB < Shape::e_typeCount);
ContactDestroyFcn* destroyFcn = s_registers[typeA][typeB].destroyFcn;
destroyFcn(contact, allocator);
}
ContactSolver::ContactSolver(ContactSolverDef* def)
{
m_step = def->step;
m_allocator = def->allocator;
m_count = def->count;
m_positionConstraints = (ContactPositionConstraint*)m_allocator->Allocate(m_count * sizeof(ContactPositionConstraint));
m_velocityConstraints = (ContactVelocityConstraint*)m_allocator->Allocate(m_count * sizeof(ContactVelocityConstraint));
m_positions = def->positions;
m_velocities = def->velocities;
m_contacts = def->contacts;
// Initialize position independent portions of the constraints.
for (int32 i = 0; i < m_count; ++i)
{
Contact* contact = m_contacts[i];
Fixture* fixtureA = contact->m_fixtureA;
Fixture* fixtureB = contact->m_fixtureB;
Shape* shapeA = fixtureA->GetShape();
Shape* shapeB = fixtureB->GetShape();
float32 radiusA = shapeA->m_radius;
float32 radiusB = shapeB->m_radius;
Body* bodyA = fixtureA->GetBody();
Body* bodyB = fixtureB->GetBody();
Manifold* manifold = contact->GetManifold();
int32 pointCount = manifold->pointCount;
assert(pointCount > 0);
ContactVelocityConstraint* vc = m_velocityConstraints + i;
vc->friction = contact->m_friction;
vc->restitution = contact->m_restitution;
vc->tangentSpeed = contact->m_tangentSpeed;
vc->indexA = bodyA->m_islandIndex;
vc->indexB = bodyB->m_islandIndex;
vc->invMassA = bodyA->m_invMass;
vc->invMassB = bodyB->m_invMass;
vc->invIA = bodyA->m_invI;
vc->invIB = bodyB->m_invI;
vc->contactIndex = i;
vc->pointCount = pointCount;
vc->K.SetZero();
vc->normalMass.SetZero();
ContactPositionConstraint* pc = m_positionConstraints + i;
pc->indexA = bodyA->m_islandIndex;
pc->indexB = bodyB->m_islandIndex;
pc->invMassA = bodyA->m_invMass;
pc->invMassB = bodyB->m_invMass;
pc->localCenterA = bodyA->m_sweep.localCenter;
pc->localCenterB = bodyB->m_sweep.localCenter;
pc->invIA = bodyA->m_invI;
pc->invIB = bodyB->m_invI;
pc->localNormal = manifold->localNormal;
pc->localPoint = manifold->localPoint;
pc->pointCount = pointCount;
pc->radiusA = radiusA;
pc->radiusB = radiusB;
pc->type = manifold->type;
for (int32 j = 0; j < pointCount; ++j)
{
ManifoldPoint* cp = manifold->points + j;
VelocityConstraintPoint* vcp = vc->points + j;
if (m_step.warmStarting)
{
vcp->normalImpulse = m_step.dtRatio * cp->normalImpulse;
vcp->tangentImpulse = m_step.dtRatio * cp->tangentImpulse;
}
else
{
vcp->normalImpulse = 0.0f;
vcp->tangentImpulse = 0.0f;
}
vcp->rA.SetZero();
vcp->rB.SetZero();
vcp->normalMass = 0.0f;
vcp->tangentMass = 0.0f;
vcp->velocityBias = 0.0f;
pc->localPoints[j] = cp->localPoint;
}
}
}
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 ContactManager::Destroy(Contact* c)
{
Fixture* fixtureA = c->GetFixtureA();
Fixture* fixtureB = c->GetFixtureB();
Body* bodyA = fixtureA->GetBody();
Body* bodyB = fixtureB->GetBody();
if (m_contactListener && c->IsTouching())
{
m_contactListener->EndContact(c);
}
// Remove from the world.
if (c->m_prev)
{
c->m_prev->m_next = c->m_next;
}
if (c->m_next)
{
c->m_next->m_prev = c->m_prev;
}
if (c == m_contactList)
{
m_contactList = c->m_next;
}
// Remove from body 1
if (c->m_nodeA.prev)
{
c->m_nodeA.prev->next = c->m_nodeA.next;
}
if (c->m_nodeA.next)
{
c->m_nodeA.next->prev = c->m_nodeA.prev;
}
if (&c->m_nodeA == bodyA->m_contactList)
{
bodyA->m_contactList = c->m_nodeA.next;
}
// Remove from body 2
if (c->m_nodeB.prev)
{
c->m_nodeB.prev->next = c->m_nodeB.next;
}
if (c->m_nodeB.next)
{
c->m_nodeB.next->prev = c->m_nodeB.prev;
}
if (&c->m_nodeB == bodyB->m_contactList)
{
bodyB->m_contactList = c->m_nodeB.next;
}
// Call the factory.
Contact::Destroy(c, m_allocator);
--m_contactCount;
}
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;
}
