public void solveTOI(TimeStep subStep) {
ContactSolver contactSolver = new ContactSolver(subStep, m_contacts, m_contactCount);
// No warm starting needed for TOI events.
// Solve velocity constraints.
for (int i = 0; i < subStep.maxIterations; ++i) {
contactSolver.solveVelocityConstraints();
}
// Don't store the TOI contact forces for warm starting
// because they can be quite large.
// Integrate positions.
for (int i = 0; i < m_bodyCount; ++i) {
Body b = m_bodies[i];
if (b.isStatic()) continue;
// System.out.println("(Island::SolveTOI 1) :"+b.m_sweep);
// Store positions for continuous collision.
b.m_sweep.c0.set(b.m_sweep.c);
b.m_sweep.a0 = b.m_sweep.a;
// Integrate
b.m_sweep.c.x += subStep.dt * b.m_linearVelocity.x;
b.m_sweep.c.y += subStep.dt * b.m_linearVelocity.y;
b.m_sweep.a += subStep.dt * b.m_angularVelocity;
// System.out.println("(Island::SolveTOI 2) :"+b.m_sweep);
// Compute new transform
b.synchronizeTransform();
// System.out.println("(Island::SolveTOI 3) :"+b.m_sweep);
// Note: shapes are synchronized later.
}
// Solve position constraints.
final float k_toiBaumgarte = 0.75f;
for (int i = 0; i < subStep.maxIterations; ++i) {
boolean contactsOkay = contactSolver.solvePositionConstraints(k_toiBaumgarte);
if (contactsOkay) {
break;
}
}
report(contactSolver.m_constraints);
}
public void solve(TimeStep step, Vec2 gravity, boolean correctPositions, boolean allowSleep) {
// Integrate velocities and apply damping.
for (int i = 0; i < m_bodyCount; ++i) {
Body b = m_bodies[i];
if (b.isStatic()) continue;
// Integrate velocities.
b.m_linearVelocity.x += step.dt * (gravity.x + b.m_invMass * b.m_force.x);
b.m_linearVelocity.y += step.dt * (gravity.y + b.m_invMass * b.m_force.y);
b.m_angularVelocity += step.dt * b.m_invI * b.m_torque;
// Reset forces.
b.m_force.set(0.0f, 0.0f);
b.m_torque = 0.0f;
// 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
// Taylor expansion:
// v2 = (1.0f - c * dt) * v1
b.m_linearVelocity.mulLocal(MathUtils.clamp(1.0f - step.dt * b.m_linearDamping, 0.0f, 1.0f));
b.m_angularVelocity *= MathUtils.clamp(1.0f - step.dt * b.m_angularDamping, 0.0f, 1.0f);
// Check for large velocities.
if (Vec2.dot(b.m_linearVelocity, b.m_linearVelocity) > Settings.maxLinearVelocitySquared) {
b.m_linearVelocity.normalize();
b.m_linearVelocity.mulLocal(Settings.maxLinearVelocity);
}
if (b.m_angularVelocity * b.m_angularVelocity > Settings.maxAngularVelocitySquared) {
if (b.m_angularVelocity < 0.0f) {
b.m_angularVelocity = -Settings.maxAngularVelocity;
} else {
b.m_angularVelocity = Settings.maxAngularVelocity;
}
}
}
ContactSolver contactSolver = new ContactSolver(step, m_contacts, m_contactCount);
// Initialize velocity constraints.
contactSolver.initVelocityConstraints(step);
for (int i = 0; i < m_jointCount; ++i) {
m_joints[i].initVelocityConstraints(step);
}
// Solve velocity constraints.
for (int i = 0; i < step.maxIterations; ++i) {
contactSolver.solveVelocityConstraints();
for (int j = 0; j < m_jointCount; ++j) {
m_joints[j].solveVelocityConstraints(step);
}
}
// Post-solve (store impulses for warm starting).
contactSolver.finalizeVelocityConstraints();
// Integrate positions.
for (int i = 0; i < m_bodyCount; ++i) {
Body b = m_bodies[i];
if (b.isStatic()) continue;
// Store positions for continuous collision.
b.m_sweep.c0.set(b.m_sweep.c);
b.m_sweep.a0 = b.m_sweep.a;
// Integrate
b.m_sweep.c.x += step.dt * b.m_linearVelocity.x;
b.m_sweep.c.y += step.dt * b.m_linearVelocity.y;
b.m_sweep.a += step.dt * b.m_angularVelocity;
// Compute new transform
b.synchronizeTransform();
// Note: shapes are synchronized later.
}
if (correctPositions) {
// Initialize position constraints.
// Contacts don't need initialization.
for (int i = 0; i < m_jointCount; ++i) {
m_joints[i].initPositionConstraints();
}
// Iterate over constraints.
for (m_positionIterationCount = 0;
m_positionIterationCount < step.maxIterations;
++m_positionIterationCount) {
boolean contactsOkay = contactSolver.solvePositionConstraints(Settings.contactBaumgarte);
boolean jointsOkay = true;
for (int i = 0; i < m_jointCount; ++i) {
boolean jointOkay = m_joints[i].solvePositionConstraints();
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay) {
break;
}
}
}
report(contactSolver.m_constraints);
if (allowSleep) {
float minSleepTime = Float.MAX_VALUE;
final float linTolSqr = Settings.linearSleepTolerance * Settings.linearSleepTolerance;
final float angTolSqr = Settings.angularSleepTolerance * Settings.angularSleepTolerance;
for (int i = 0; i < m_bodyCount; ++i) {
Body b = m_bodies[i];
if (b.m_invMass == 0.0f) {
continue;
}
if ((b.m_flags & Body.e_allowSleepFlag) == 0) {
b.m_sleepTime = 0.0f;
minSleepTime = 0.0f;
}
if ((b.m_flags & Body.e_allowSleepFlag) == 0
|| b.m_angularVelocity * b.m_angularVelocity > angTolSqr
|| Vec2.dot(b.m_linearVelocity, b.m_linearVelocity) > linTolSqr) {
b.m_sleepTime = 0.0f;
minSleepTime = 0.0f;
} else {
b.m_sleepTime += step.dt;
minSleepTime = Math.min(minSleepTime, b.m_sleepTime);
}
}
if (minSleepTime >= Settings.timeToSleep) {
for (int i = 0; i < m_bodyCount; ++i) {
Body b = m_bodies[i];
b.m_flags |= Body.e_sleepFlag;
b.m_linearVelocity = new Vec2(0.0f, 0.0f);
b.m_angularVelocity = 0.0f;
}
}
}
}