Example 1 with Sweep
use of spacegraph.space2d.phys.common.Sweep in project narchy by automenta.
the class Island method solve.
public void solve(Dynamics2D.Profile profile, TimeStep step, Tuple2f gravity, boolean allowSleep) {
// System.out.println("Solving Island");
float h = step.dt;
// Integrate velocities and apply damping. Initialize the body state.
for (int i = 0; i < m_bodyCount; ++i) {
final Body2D b = bodies[i];
final Sweep bm_sweep = b.sweep;
final Tuple2f c = bm_sweep.c;
float a = bm_sweep.a;
final Tuple2f v = b.vel;
float w = b.velAngular;
// Store positions for continuous collision.
bm_sweep.c0.set(bm_sweep.c);
bm_sweep.a0 = bm_sweep.a;
if (b.type == BodyType.DYNAMIC) {
// Integrate velocities.
// v += h * (b.m_gravityScale * gravity + b.m_invMass * b.m_force);
v.x += h * (b.m_gravityScale * gravity.x + b.m_invMass * b.force.x);
v.y += h * (b.m_gravityScale * gravity.y + b.m_invMass * b.force.y);
w += h * b.m_invI * b.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.x *= 1.0f / (1.0f + h * b.m_linearDamping);
v.y *= 1.0f / (1.0f + h * b.m_linearDamping);
w *= 1.0f / (1.0f + h * b.m_angularDamping);
}
positions[i].x = c.x;
positions[i].y = c.y;
positions[i].a = a;
velocities[i].x = v.x;
velocities[i].y = v.y;
velocities[i].w = w;
}
timer.reset();
// Solver data
solverData.step = step;
solverData.positions = positions;
solverData.velocities = velocities;
// Initialize velocity constraints.
solverDef.step = step;
solverDef.contacts = contacts;
solverDef.count = m_contactCount;
solverDef.positions = positions;
solverDef.velocities = velocities;
contactSolver.init(solverDef);
// System.out.println("island init vel");
contactSolver.initializeVelocityConstraints();
if (step.warmStarting) {
// System.out.println("island warm start");
contactSolver.warmStart();
}
for (int i = 0; i < m_jointCount; ++i) {
joints[i].initVelocityConstraints(solverData);
}
profile.solveInit.accum(timer::getMilliseconds);
// Solve velocity constraints
timer.reset();
// System.out.println("island solving velocities");
for (int i = 0; i < step.velocityIterations; ++i) {
for (int j = 0; j < m_jointCount; ++j) {
joints[j].solveVelocityConstraints(solverData);
}
contactSolver.solveVelocityConstraints();
}
// Store impulses for warm starting
contactSolver.storeImpulses();
profile.solveVelocity.accum(timer::getMilliseconds);
// Integrate positions
for (int i = 0; i < m_bodyCount; ++i) {
final Tuple2f c = positions[i];
float a = positions[i].a;
final Tuple2f v = velocities[i];
float w = velocities[i].w;
// Check for large velocities
float translationx = v.x * h;
float translationy = v.y * h;
if (translationx * translationx + translationy * translationy > Settings.maxTranslationSquared) {
float ratio = Settings.maxTranslation / (float) Math.sqrt(translationx * translationx + translationy * translationy);
v.x *= ratio;
v.y *= ratio;
}
float rotation = h * w;
if (rotation * rotation > Settings.maxRotationSquared) {
float ratio = Settings.maxRotation / Math.abs(rotation);
w *= ratio;
}
// Integrate
c.x += h * v.x;
c.y += h * v.y;
a += h * w;
positions[i].a = a;
velocities[i].w = w;
}
// Solve position constraints
timer.reset();
boolean positionSolved = false;
for (int i = 0; i < step.positionIterations; ++i) {
boolean contactsOkay = contactSolver.solvePositionConstraints();
boolean jointsOkay = true;
for (int j = 0; j < m_jointCount; ++j) {
boolean jointOkay = joints[j].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 (int i = 0; i < m_bodyCount; ++i) {
Body2D body = bodies[i];
body.sweep.c.x = positions[i].x;
body.sweep.c.y = positions[i].y;
body.sweep.a = positions[i].a;
body.vel.x = velocities[i].x;
body.vel.y = velocities[i].y;
body.velAngular = velocities[i].w;
body.synchronizeTransform();
}
profile.solvePosition.accum(timer::getMilliseconds);
report(contactSolver.m_velocityConstraints);
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) {
Body2D b = bodies[i];
if (b.getType() == BodyType.STATIC) {
continue;
}
if ((b.flags & Body2D.e_autoSleepFlag) == 0 || b.velAngular * b.velAngular > angTolSqr || Tuple2f.dot(b.vel, b.vel) > linTolSqr) {
b.m_sleepTime = 0.0f;
minSleepTime = 0.0f;
} else {
b.m_sleepTime += h;
minSleepTime = MathUtils.min(minSleepTime, b.m_sleepTime);
}
}
if (minSleepTime >= Settings.timeToSleep && positionSolved) {
for (int i = 0; i < m_bodyCount; ++i) {
Body2D b = bodies[i];
b.setAwake(false);
}
}
}
}