// Update the contact manifold and touching status.
// Note: do not assume the fixture AABBs are overlapping or are valid.
void Contact::Update(ContactListener* listener)
{
Manifold oldManifold = m_manifold;
// Re-enable this contact.
m_flags |= e_enabledFlag;
bool touching = false;
bool wasTouching = (m_flags & e_touchingFlag) == e_touchingFlag;
bool sensorA = m_fixtureA->IsSensor();
bool sensorB = m_fixtureB->IsSensor();
bool sensor = sensorA || sensorB;
Body* bodyA = m_fixtureA->GetBody();
Body* bodyB = m_fixtureB->GetBody();
const Transform& xfA = bodyA->GetTransform();
const Transform& xfB = bodyB->GetTransform();
// Is this contact a sensor?
if (sensor)
{
const Shape* shapeA = m_fixtureA->GetShape();
const Shape* shapeB = m_fixtureB->GetShape();
touching = TestOverlap(shapeA, m_indexA, shapeB, m_indexB, xfA, xfB);
// Sensors don't generate manifolds.
m_manifold.pointCount = 0;
}
else
{
Evaluate(&m_manifold, xfA, xfB);
touching = m_manifold.pointCount > 0;
// Match old contact ids to new contact ids and copy the
// stored impulses to warm start the solver.
for (int32 i = 0; i < m_manifold.pointCount; ++i)
{
ManifoldPoint* mp2 = m_manifold.points + i;
mp2->normalImpulse = 0.0f;
mp2->tangentImpulse = 0.0f;
ContactID id2 = mp2->id;
for (int32 j = 0; j < oldManifold.pointCount; ++j)
{
ManifoldPoint* mp1 = oldManifold.points + j;
if (mp1->id.key == id2.key)
{
mp2->normalImpulse = mp1->normalImpulse;
mp2->tangentImpulse = mp1->tangentImpulse;
break;
}
}
}
if (touching != wasTouching)
{
bodyA->SetAwake(true);
bodyB->SetAwake(true);
}
}
if (touching)
{
m_flags |= e_touchingFlag;
}
else
{
m_flags &= ~e_touchingFlag;
}
if (wasTouching == false && touching == true && listener)
{
listener->BeginContact(this);
}
if (wasTouching == true && touching == false && listener)
{
listener->EndContact(this);
}
if (sensor == false && touching && listener)
{
listener->PreSolve(this, &oldManifold);
}
}
void Island::Solve(Profile* profile, const PTimeStep& step, const glm::vec2& gravity, bool allowSleep)
{
Time timer;
real32 h = step.delta;
// Integrate velocities and apply damping. Initialize the body state.
for (s32 i = 0; i < m_bodyCount; ++i)
{
Body* b = m_bodies[i];
glm::vec2 c = b->m_sweep.c;
real32 a = b->m_sweep.a;
glm::vec2 v = b->m_linearVelocity;
real32 w = b->m_angularVelocity;
// Store positions for continuous collision.
b->m_sweep.c0 = b->m_sweep.c;
b->m_sweep.a0 = b->m_sweep.a;
if (b->m_type == dynamicBody)
{
// Integrate velocities.
v += h * (b->m_gravityScale * gravity + b->m_invMass * b->m_force);
w += h * b->m_invI * b->m_torque;
// Apply damping.
// ODE: dv/dt + c * v = 0
// Solution: v(t) = v0 * exp(-c * t)
// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
// v2 = exp(-c * dt) * v1
// Pade approximation:
// v2 = v1 * 1 / (1 + c * dt)
v *= 1.0f / (1.0f + h * b->m_linearDamping);
w *= 1.0f / (1.0f + h * b->m_angularDamping);
}
m_positions[i].c = c;
m_positions[i].a = a;
m_velocities[i].v = v;
m_velocities[i].w = w;
}
auto captureTimer = Services::getPlatform()->getTime();
// Solver data
SolverData solverData;
solverData.step = step;
solverData.positions = m_positions;
solverData.velocities = m_velocities;
// Initialize velocity constraints.
ContactSolverDef contactSolverDef;
contactSolverDef.step = step;
contactSolverDef.contacts = m_contacts;
contactSolverDef.count = m_contactCount;
contactSolverDef.positions = m_positions;
contactSolverDef.velocities = m_velocities;
contactSolverDef.allocator = m_allocator;
ContactSolver contactSolver(&contactSolverDef);
contactSolver.InitializeVelocityConstraints();
if (step.warmStarting)
{
contactSolver.WarmStart();
}
for (s32 i = 0; i < m_jointCount; ++i)
{
m_joints[i]->InitVelocityConstraints(solverData);
}
profile->solveInit = Services::getPlatform()->getTime()-captureTimer;
// Solve velocity constraints
captureTimer = Services::getPlatform()->getTime();
for (s32 i = 0; i < step.velocityIterations; ++i)
{
for (s32 j = 0; j < m_jointCount; ++j)
{
m_joints[j]->SolveVelocityConstraints(solverData);
}
contactSolver.SolveVelocityConstraints();
}
// Store impulses for warm starting
contactSolver.StoreImpulses();
profile->solveVelocity = Services::getPlatform()->getTime()-captureTimer;
// 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;
}
// Solve position constraints
captureTimer = Services::getPlatform()->getTime();
bool positionSolved = false;
for (s32 i = 0; i < step.positionIterations; ++i)
{
bool contactsOkay = contactSolver.SolvePositionConstraints();
bool jointsOkay = true;
for (s32 i = 0; i < m_jointCount; ++i)
{
bool jointOkay = m_joints[i]->SolvePositionConstraints(solverData);
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
positionSolved = true;
break;
}
}
// Copy state buffers back to the bodies
for (s32 i = 0; i < m_bodyCount; ++i)
{
Body* body = m_bodies[i];
body->m_sweep.c = m_positions[i].c;
body->m_sweep.a = m_positions[i].a;
body->m_linearVelocity = m_velocities[i].v;
body->m_angularVelocity = m_velocities[i].w;
body->SynchronizeTransform2D();
}
profile->solvePosition = Services::getPlatform()->getTime()-captureTimer;
Report(contactSolver.m_velocityConstraints);
if (allowSleep)
{
real32 minSleepTime = FLT_MAX;
const real32 linTolSqr = linearSleepTolerance * linearSleepTolerance;
const real32 angTolSqr = angularSleepTolerance * angularSleepTolerance;
for (s32 i = 0; i < m_bodyCount; ++i)
{
Body* b = m_bodies[i];
if (b->GetType() == staticBody)
{
continue;
}
if ((b->m_flags & Body::autoSleepFlag) == 0 ||
b->m_angularVelocity * b->m_angularVelocity > angTolSqr ||
glm::dot(b->m_linearVelocity, b->m_linearVelocity) > linTolSqr)
{
b->m_sleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b->m_sleepTime += h;
minSleepTime = glm::min(minSleepTime, b->m_sleepTime);
}
}
if (minSleepTime >= timeToSleep && positionSolved)
{
for (s32 i = 0; i < m_bodyCount; ++i)
{
Body* b = m_bodies[i];
b->SetAwake(false);
}
}
}
}
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;
}
