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);
}
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);
}
}
}
}
