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;
}
}
}
}
public Object pairAdded(final Object proxyUserData1, final Object proxyUserData2) {
Shape shape1 = (Shape) proxyUserData1;
Shape shape2 = (Shape) proxyUserData2;
Body body1 = shape1.getBody();
Body body2 = shape2.getBody();
if (body1.isStatic() && body2.isStatic()) {
return m_nullContact;
}
if (shape1.getBody() == shape2.getBody()) {
return m_nullContact;
}
if (body2.isConnected(body1)) {
return m_nullContact;
}
if (m_world.m_contactFilter != null
&& m_world.m_contactFilter.shouldCollide(shape1, shape2) == false) {
return m_nullContact;
}
// Call the factory.
final Contact c = Contact.createContact(shape1, shape2);
if (c == null) {
return m_nullContact;
}
// Contact creation may swap shapes.
shape1 = c.getShape1();
shape2 = c.getShape2();
body1 = shape1.getBody();
body2 = shape2.getBody();
// Insert into the world.
c.m_prev = null;
c.m_next = m_world.m_contactList;
if (m_world.m_contactList != null) {
m_world.m_contactList.m_prev = c;
}
m_world.m_contactList = c;
// Connect to island graph.
// Connect to body 1
c.m_node1.contact = c;
c.m_node1.other = body2;
c.m_node1.prev = null;
c.m_node1.next = body1.m_contactList;
if (body1.m_contactList != null) {
body1.m_contactList.prev = c.m_node1;
}
body1.m_contactList = c.m_node1;
// Connect to body 2
c.m_node2.contact = c;
c.m_node2.other = body1;
c.m_node2.prev = null;
c.m_node2.next = body2.m_contactList;
if (body2.m_contactList != null) {
body2.m_contactList.prev = c.m_node2;
}
body2.m_contactList = c.m_node2;
++m_world.m_contactCount;
return c;
}