void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep)
{
b2Timer timer;
float32 h = step.dt;
// Integrate velocities and apply damping. Initialize the body state.
for (int32 i = 0; i < m_bodyCount; ++i)
{
b2Body* b = m_bodies[i];
b2Vec2 c = b->m_sweep.c;
float32 a = b->m_sweep.a;
b2Vec2 v = b->m_linearVelocity;
float32 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 == b2_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
// Taylor expansion:
// v2 = (1.0f - c * dt) * v1
v *= b2Clamp(1.0f - h * b->m_linearDamping, 0.0f, 1.0f);
w *= b2Clamp(1.0f - h * b->m_angularDamping, 0.0f, 1.0f);
}
m_positions[i].c = c;
m_positions[i].a = a;
m_velocities[i].v = v;
m_velocities[i].w = w;
}
timer.Reset();
// Solver data
b2SolverData solverData;
solverData.step = step;
solverData.positions = m_positions;
solverData.velocities = m_velocities;
// Initialize velocity constraints.
b2ContactSolverDef contactSolverDef;
contactSolverDef.step = step;
contactSolverDef.contacts = m_contacts;
contactSolverDef.count = m_contactCount;
contactSolverDef.positions = m_positions;
contactSolverDef.velocities = m_velocities;
contactSolverDef.allocator = m_allocator;
b2ContactSolver contactSolver(&contactSolverDef);
contactSolver.InitializeVelocityConstraints();
if (step.warmStarting)
{
contactSolver.WarmStart();
}
for (int32 i = 0; i < m_jointCount; ++i)
{
m_joints[i]->InitVelocityConstraints(solverData);
}
profile->solveInit = timer.GetMilliseconds();
// Solve velocity constraints
timer.Reset();
for (int32 i = 0; i < step.velocityIterations; ++i)
{
for (int32 j = 0; j < m_jointCount; ++j)
{
m_joints[j]->SolveVelocityConstraints(solverData);
}
contactSolver.SolveVelocityConstraints();
}
// Store impulses for warm starting
contactSolver.StoreImpulses();
profile->solveVelocity = timer.GetMilliseconds();
// Integrate positions
for (int32 i = 0; i < m_bodyCount; ++i)
{
b2Vec2 c = m_positions[i].c;
float32 a = m_positions[i].a;
b2Vec2 v = m_velocities[i].v;
float32 w = m_velocities[i].w;
// Check for large velocities
b2Vec2 translation = h * v;
if (b2Dot(translation, translation) > b2_maxTranslationSquared)
{
float32 ratio = b2_maxTranslation / translation.Length();
v *= ratio;
}
float32 rotation = h * w;
if (rotation * rotation > b2_maxRotationSquared)
{
float32 ratio = b2_maxRotation / b2Abs(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
timer.Reset();
bool positionSolved = false;
for (int32 i = 0; i < step.positionIterations; ++i)
{
bool contactsOkay = contactSolver.SolvePositionConstraints();
bool jointsOkay = true;
for (int32 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 (int32 i = 0; i < m_bodyCount; ++i)
{
b2Body* 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->SynchronizeTransform();
}
profile->solvePosition = timer.GetMilliseconds();
Report(contactSolver.m_velocityConstraints);
if (allowSleep)
{
float32 minSleepTime = b2_maxFloat;
const float32 linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance;
const float32 angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance;
for (int32 i = 0; i < m_bodyCount; ++i)
{
b2Body* b = m_bodies[i];
if (b->GetType() == b2_staticBody)
{
continue;
}
if ((b->m_flags & b2Body::e_autoSleepFlag) == 0 ||
b->m_angularVelocity * b->m_angularVelocity > angTolSqr ||
b2Dot(b->m_linearVelocity, b->m_linearVelocity) > linTolSqr)
{
b->m_sleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b->m_sleepTime += h;
minSleepTime = b2Min(minSleepTime, b->m_sleepTime);
}
}
if (minSleepTime >= b2_timeToSleep && positionSolved)
{
for (int32 i = 0; i < m_bodyCount; ++i)
{
b2Body* b = m_bodies[i];
b->SetAwake(false);
}
}
}
}
void b2Island::SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiIndexB)
{
b2Assert(toiIndexA < m_bodyCount);
b2Assert(toiIndexB < m_bodyCount);
// Initialize the body state.
for (int32 i = 0; i < m_bodyCount; ++i)
{
b2Body* 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;
}
b2ContactSolverDef contactSolverDef;
contactSolverDef.contacts = m_contacts;
contactSolverDef.count = m_contactCount;
contactSolverDef.allocator = m_allocator;
contactSolverDef.step = subStep;
contactSolverDef.positions = m_positions;
contactSolverDef.velocities = m_velocities;
b2ContactSolver contactSolver(&contactSolverDef);
// Solve position constraints.
for (int32 i = 0; i < subStep.positionIterations; ++i)
{
bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
if (contactsOkay)
{
break;
}
}
#if 0
// Is the new position really safe?
for (int32 i = 0; i < m_contactCount; ++i)
{
b2Contact* c = m_contacts[i];
b2Fixture* fA = c->GetFixtureA();
b2Fixture* fB = c->GetFixtureB();
b2Body* bA = fA->GetBody();
b2Body* bB = fB->GetBody();
int32 indexA = c->GetChildIndexA();
int32 indexB = c->GetChildIndexB();
b2DistanceInput input;
input.proxyA.Set(fA->GetShape(), indexA);
input.proxyB.Set(fB->GetShape(), indexB);
input.transformA = bA->GetTransform();
input.transformB = bB->GetTransform();
input.useRadii = false;
b2DistanceOutput output;
b2SimplexCache cache;
cache.count = 0;
b2Distance(&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 (int32 i = 0; i < subStep.velocityIterations; ++i)
{
contactSolver.SolveVelocityConstraints();
}
// Don't store the TOI contact forces for warm starting
// because they can be quite large.
float32 h = subStep.dt;
// Integrate positions
for (int32 i = 0; i < m_bodyCount; ++i)
{
b2Vec2 c = m_positions[i].c;
float32 a = m_positions[i].a;
b2Vec2 v = m_velocities[i].v;
float32 w = m_velocities[i].w;
// Check for large velocities
b2Vec2 translation = h * v;
if (b2Dot(translation, translation) > b2_maxTranslationSquared)
{
float32 ratio = b2_maxTranslation / translation.Length();
v *= ratio;
}
float32 rotation = h * w;
if (rotation * rotation > b2_maxRotationSquared)
{
float32 ratio = b2_maxRotation / b2Abs(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
b2Body* body = m_bodies[i];
body->m_sweep.c = c;
body->m_sweep.a = a;
body->m_linearVelocity = v;
body->m_angularVelocity = w;
body->SynchronizeTransform();
}
Report(contactSolver.m_velocityConstraints);
}
void b2Island::Solve(const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep)
{
// Integrate velocities and apply damping.
for (int32 i = 0; i < m_bodyCount; ++i)
{
b2Body* b = m_bodies[i];
if (b->GetType() != b2_dynamicBody)
{
continue;
}
// Integrate velocities.
b->m_linearVelocity += step.dt * (gravity + b->m_invMass * b->m_force);
b->m_angularVelocity += step.dt * 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
// Taylor expansion:
// v2 = (1.0f - c * dt) * v1
b->m_linearVelocity *= b2Clamp(1.0f - step.dt * b->m_linearDamping, 0.0f, 1.0f);
b->m_angularVelocity *= b2Clamp(1.0f - step.dt * b->m_angularDamping, 0.0f, 1.0f);
}
// Partition contacts so that contacts with static bodies are solved last.
int32 i1 = -1;
for (int32 i2 = 0; i2 < m_contactCount; ++i2)
{
b2Fixture* fixtureA = m_contacts[i2]->GetFixtureA();
b2Fixture* fixtureB = m_contacts[i2]->GetFixtureB();
b2Body* bodyA = fixtureA->GetBody();
b2Body* bodyB = fixtureB->GetBody();
bool nonStatic = bodyA->GetType() != b2_staticBody && bodyB->GetType() != b2_staticBody;
if (nonStatic)
{
++i1;
b2Swap(m_contacts[i1], m_contacts[i2]);
}
}
// Initialize velocity constraints.
b2ContactSolver contactSolver(m_contacts, m_contactCount, m_allocator, step.dtRatio);
contactSolver.WarmStart();
for (int32 i = 0; i < m_jointCount; ++i)
{
m_joints[i]->InitVelocityConstraints(step);
}
// Solve velocity constraints.
for (int32 i = 0; i < step.velocityIterations; ++i)
{
for (int32 j = 0; j < m_jointCount; ++j)
{
m_joints[j]->SolveVelocityConstraints(step);
}
contactSolver.SolveVelocityConstraints();
}
// Post-solve (store impulses for warm starting).
contactSolver.StoreImpulses();
// Integrate positions.
for (int32 i = 0; i < m_bodyCount; ++i)
{
b2Body* b = m_bodies[i];
if (b->GetType() == b2_staticBody)
{
continue;
}
// Check for large velocities.
b2Vec2 translation = step.dt * b->m_linearVelocity;
if (b2Dot(translation, translation) > b2_maxTranslationSquared)
{
float32 ratio = b2_maxTranslation / translation.Length();
b->m_linearVelocity *= ratio;
}
float32 rotation = step.dt * b->m_angularVelocity;
if (rotation * rotation > b2_maxRotationSquared)
{
float32 ratio = b2_maxRotation / b2Abs(rotation);
b->m_angularVelocity *= ratio;
}
// Store positions for continuous collision.
b->m_sweep.c0 = b->m_sweep.c;
b->m_sweep.a0 = b->m_sweep.a;
// Integrate
b->m_sweep.c += step.dt * b->m_linearVelocity;
b->m_sweep.a += step.dt * b->m_angularVelocity;
// Compute new transform
b->SynchronizeTransform();
// Note: shapes are synchronized later.
}
// Iterate over constraints.
for (int32 i = 0; i < step.positionIterations; ++i)
{
bool contactsOkay = contactSolver.SolvePositionConstraints(b2_contactBaumgarte);
bool jointsOkay = true;
for (int32 i = 0; i < m_jointCount; ++i)
{
bool jointOkay = m_joints[i]->SolvePositionConstraints(b2_contactBaumgarte);
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
break;
}
}
Report(contactSolver.m_constraints);
if (allowSleep)
{
float32 minSleepTime = b2_maxFloat;
const float32 linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance;
const float32 angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance;
for (int32 i = 0; i < m_bodyCount; ++i)
{
b2Body* b = m_bodies[i];
if (b->GetType() == b2_staticBody)
{
continue;
}
if ((b->m_flags & b2Body::e_autoSleepFlag) == 0)
{
b->m_sleepTime = 0.0f;
minSleepTime = 0.0f;
}
if ((b->m_flags & b2Body::e_autoSleepFlag) == 0 ||
b->m_angularVelocity * b->m_angularVelocity > angTolSqr ||
b2Dot(b->m_linearVelocity, b->m_linearVelocity) > linTolSqr)
{
b->m_sleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b->m_sleepTime += step.dt;
minSleepTime = b2Min(minSleepTime, b->m_sleepTime);
}
}
if (minSleepTime >= b2_timeToSleep)
{
for (int32 i = 0; i < m_bodyCount; ++i)
{
b2Body* b = m_bodies[i];
b->SetAwake(false);
}
}
}
}
void b3Island::Solve(const b3Vec3& gravityDir) {
r32 h = dt;
b3Vec3 gravityForce = B3_GRAVITY_ACC * gravityDir;
// Integrate velocities.
for (u32 i = 0; i < bodyCount; ++i) {
b3Body* b = bodies[i];
b3Vec3 v = b->m_linearVelocity;
b3Vec3 w = b->m_angularVelocity;
b3Vec3 x = b->m_worldCenter;
b3Quaternion q = b->m_orientation;
if (b->m_type == e_dynamicBody) {
// Use semi-implitic Euler.
b3Vec3 force = b->m_gravityScale * gravityForce + b->m_force;
v += (h * b->m_invMass) * force;
w += h * (b->m_invWorldInertia * b->m_torque);
// References: Box2D.
// 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 *= B3_ONE / (B3_ONE + h * r32(0.1));
w *= B3_ONE / (B3_ONE + h * r32(0.1));
}
velocities[i].v = v;
velocities[i].w = w;
positions[i].x = x;
positions[i].q = q;
}
b3JointSolverDef jointSolverDef;
jointSolverDef.dt = h;
jointSolverDef.joints = joints;
jointSolverDef.count = jointCount;
jointSolverDef.positions = positions;
jointSolverDef.velocities = velocities;
b3JointSolver jointSolver(&jointSolverDef);
jointSolver.InitializeVelocityConstraints();
b3ContactSolverDef contactSolverDef;
contactSolverDef.dt = h;
contactSolverDef.contacts = contacts;
contactSolverDef.count = contactCount;
contactSolverDef.positions = positions;
contactSolverDef.velocities = velocities;
contactSolverDef.allocator = allocator;
b3ContactSolver contactSolver(&contactSolverDef);
contactSolver.InitializeVelocityConstraints();
jointSolver.WarmStart();
contactSolver.WarmStart();
// Solve velocity constraints.
for (u32 i = 0; i < velocityIterations; ++i) {
jointSolver.SolveVelocityConstraints();
contactSolver.SolveVelocityConstraints();
}
contactSolver.StoreImpulses();
for (u32 i = 0; i < bodyCount; ++i) {
b3Body* b = bodies[i];
if (b->m_type == e_staticBody) {
continue;
}
b3Vec3 x = positions[i].x;
b3Quaternion q1 = positions[i].q;
b3Vec3 v = velocities[i].v;
b3Vec3 w = velocities[i].w;
x += h * v;
b3Quaternion q2 = Integrate(q1, w, h);
positions[i].x = x;
positions[i].q = q2;
velocities[i].v = v;
velocities[i].w = w;
}
for (u32 i = 0; i < bodyCount; ++i) {
b3Body* b = bodies[i];
if (b->m_type == e_staticBody) {
continue;
}
b->m_worldCenter = positions[i].x;
b->m_orientation = positions[i].q;
b->m_linearVelocity = velocities[i].v;
b->m_angularVelocity = velocities[i].w;
}
if (allowSleep) {
r32 minSleepTime = B3_MAX_FLOAT;
for (u32 i = 0; i < bodyCount; ++i) {
b3Body* b = bodies[i];
if (b->m_type == e_staticBody) {
continue;
}
// Compute the linear and angular speed of the body.
const r32 sqrLinVel = b3LenSq(b->m_linearVelocity);
const r32 sqrAngVel = b3LenSq(b->m_angularVelocity);
if (sqrLinVel > B3_SLEEP_LINEAR_TOL || sqrAngVel > B3_SLEEP_ANGULAR_TOL) {
minSleepTime = B3_ZERO;
b->m_sleepTime = B3_ZERO;
}
else {
b->m_sleepTime += h;
minSleepTime = b3Min(minSleepTime, b->m_sleepTime);
}
}
// Put the island to sleep so long as the minimum found sleep time
// is below the threshold.
if (minSleepTime >= B3_TIME_TO_SLEEP) {
for (u32 i = 0; i < bodyCount; ++i) {
bodies[i]->SetAwake(false);
}
}
}
}
void b3Island::Solve(const b3Vec3& gravity, float32 dt, u32 velocityIterations, u32 positionIterations, u32 flags)
{
float32 h = dt;
// 1. Integrate velocities
for (u32 i = 0; i < m_bodyCount; ++i)
{
b3Body* b = m_bodies[i];
b3Vec3 v = b->m_linearVelocity;
b3Vec3 w = b->m_angularVelocity;
b3Vec3 x = b->m_sweep.worldCenter;
b3Quat q = b->m_sweep.orientation;
// Remember the positions for CCD
b->m_sweep.worldCenter0 = b->m_sweep.worldCenter;
b->m_sweep.orientation0 = b->m_sweep.orientation;
if (b->m_type == e_dynamicBody)
{
// Integrate forces
v += h * (b->m_gravityScale * gravity + b->m_invMass * b->m_force);
// Clear forces
b->m_force.SetZero();
// Integrate torques
// Superposition Principle
// w2 - w1 = dw1 + dw2
// w2 - w1 = h * I^1 * bt + h * I^1 * -gt
// w2 = w1 + dw1 + dw2
// Explicit Euler on current inertia and applied torque
// w2 = w1 + h * I1^1 * bt1
b3Vec3 dw1 = h * b->m_worldInvI * b->m_torque;
// Implicit Euler on next inertia and angular velocity
// w2 = w1 - h * I2^1 * cross(w2, I2 * w2)
// w2 - w1 = -I2^1 * h * cross(w2, I2 * w2)
// I2 * (w2 - w1) = -h * cross(w2, I2 * w2)
// I2 * (w2 - w1) + h * cross(w2, I2 * w2) = 0
// Toss out I2 from f using local I2 (constant) and local w1
// to remove its time dependency.
b3Vec3 w2 = b3SolveGyro(q, b->m_I, w, h);
b3Vec3 dw2 = w2 - w;
w += dw1 + dw2;
// Clear torques
b->m_torque.SetZero();
// Apply local damping.
// ODE: dv/dt + c * v = 0
// Solution: v(t) = v0 * exp(-c * t)
// 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
// Padé approximation:
// 1 / (1 + c * dt)
v *= 1.0f / (1.0f + h * b->m_linearDamping);
w *= 1.0f / (1.0f + h * b->m_angularDamping);
}
m_velocities[i].v = v;
m_velocities[i].w = w;
m_positions[i].x = x;
m_positions[i].q = q;
m_invInertias[i] = b->m_worldInvI;
}
b3JointSolverDef jointSolverDef;
jointSolverDef.joints = m_joints;
jointSolverDef.count = m_jointCount;
jointSolverDef.positions = m_positions;
jointSolverDef.velocities = m_velocities;
jointSolverDef.invInertias = m_invInertias;
jointSolverDef.dt = h;
b3JointSolver jointSolver(&jointSolverDef);
b3ContactSolverDef contactSolverDef;
contactSolverDef.allocator = m_allocator;
contactSolverDef.contacts = m_contacts;
contactSolverDef.count = m_contactCount;
contactSolverDef.positions = m_positions;
contactSolverDef.velocities = m_velocities;
contactSolverDef.invInertias = m_invInertias;
contactSolverDef.dt = h;
b3ContactSolver contactSolver(&contactSolverDef);
// 2. Initialize constraints
{
B3_PROFILE("Initialize Constraints");
contactSolver.InitializeConstraints();
if (flags & e_warmStartBit)
{
contactSolver.WarmStart();
}
jointSolver.InitializeConstraints();
if (flags & e_warmStartBit)
{
jointSolver.WarmStart();
}
}
// 3. Solve velocity constraints
{
B3_PROFILE("Solve Velocity Constraints");
for (u32 i = 0; i < velocityIterations; ++i)
{
jointSolver.SolveVelocityConstraints();
contactSolver.SolveVelocityConstraints();
}
if (flags & e_warmStartBit)
{
contactSolver.StoreImpulses();
}
}
// 4. Integrate positions
for (u32 i = 0; i < m_bodyCount; ++i)
{
b3Body* b = m_bodies[i];
b3Vec3 x = m_positions[i].x;
b3Quat q = m_positions[i].q;
b3Vec3 v = m_velocities[i].v;
b3Vec3 w = m_velocities[i].w;
b3Mat33 invI = m_invInertias[i];
// Prevent numerical instability due to large velocity changes.
b3Vec3 translation = h * v;
if (b3Dot(translation, translation) > B3_MAX_TRANSLATION_SQUARED)
{
float32 ratio = B3_MAX_TRANSLATION / b3Length(translation);
v *= ratio;
}
b3Vec3 rotation = h * w;
if (b3Dot(rotation, rotation) > B3_MAX_ROTATION_SQUARED)
{
float32 ratio = B3_MAX_ROTATION / b3Length(rotation);
w *= ratio;
}
// Integrate
x += h * v;
q = b3Integrate(q, w, h);
invI = b3RotateToFrame(b->m_invI, q);
m_positions[i].x = x;
m_positions[i].q = q;
m_velocities[i].v = v;
m_velocities[i].w = w;
m_invInertias[i] = invI;
}
// 5. Solve position constraints
{
B3_PROFILE("Solve Position Constraints");
bool positionsSolved = false;
for (u32 i = 0; i < positionIterations; ++i)
{
bool contactsSolved = contactSolver.SolvePositionConstraints();
bool jointsSolved = jointSolver.SolvePositionConstraints();
if (contactsSolved && jointsSolved)
{
// Early out if the position errors are small.
positionsSolved = true;
break;
}
}
}
// 6. Copy state buffers back to the bodies
for (u32 i = 0; i < m_bodyCount; ++i)
{
b3Body* b = m_bodies[i];
b->m_sweep.worldCenter = m_positions[i].x;
b->m_sweep.orientation = m_positions[i].q;
b->m_sweep.orientation.Normalize();
b->m_linearVelocity = m_velocities[i].v;
b->m_angularVelocity = m_velocities[i].w;
b->m_worldInvI = m_invInertias[i];
b->SynchronizeTransform();
}
// 7. Put bodies under unconsiderable motion to sleep
if (flags & e_sleepBit)
{
float32 minSleepTime = B3_MAX_FLOAT;
for (u32 i = 0; i < m_bodyCount; ++i)
{
b3Body* b = m_bodies[i];
if (b->m_type == e_staticBody)
{
continue;
}
// Compute the linear and angular speed of the body.
float32 sqrLinVel = b3Dot(b->m_linearVelocity, b->m_linearVelocity);
float32 sqrAngVel = b3Dot(b->m_angularVelocity, b->m_angularVelocity);
if (sqrLinVel > B3_SLEEP_LINEAR_TOL || sqrAngVel > B3_SLEEP_ANGULAR_TOL)
{
minSleepTime = 0.0f;
b->m_sleepTime = 0.0f;
}
else
{
b->m_sleepTime += h;
minSleepTime = b3Min(minSleepTime, b->m_sleepTime);
}
}
// Put the island to sleep so long as the minimum found sleep time
// is below the threshold.
if (minSleepTime >= B3_TIME_TO_SLEEP)
{
for (u32 i = 0; i < m_bodyCount; ++i)
{
m_bodies[i]->SetAwake(false);
}
}
}
}
void b2Island::SolveTOI(const b2TimeStep& subStep, const b2Body* bodyA, const b2Body* bodyB)
{
b2ContactSolverDef solverDef;
solverDef.contacts = m_contacts;
solverDef.count = m_contactCount;
solverDef.allocator = m_allocator;
solverDef.impulseRatio = subStep.dtRatio;
solverDef.warmStarting = subStep.warmStarting;
b2ContactSolver contactSolver(&solverDef);
// Solve position constraints.
const float32 k_toiBaumgarte = 0.75f;
for (int32 i = 0; i < subStep.positionIterations; ++i)
{
bool contactsOkay = contactSolver.SolveTOIPositionConstraints(k_toiBaumgarte, bodyA, bodyB);
if (contactsOkay)
{
break;
}
if (i == subStep.positionIterations - 1)
{
i += 0;
}
}
#if 0
// Is the new position really safe?
for (int32 i = 0; i < m_contactCount; ++i)
{
b2Contact* c = m_contacts[i];
b2Fixture* fA = c->GetFixtureA();
b2Fixture* fB = c->GetFixtureB();
b2Body* bA = fA->GetBody();
b2Body* bB = fB->GetBody();
int32 indexA = c->GetChildIndexA();
int32 indexB = c->GetChildIndexB();
b2DistanceInput input;
input.proxyA.Set(fA->GetShape(), indexA);
input.proxyB.Set(fB->GetShape(), indexB);
input.transformA = bA->GetTransform();
input.transformB = bB->GetTransform();
input.useRadii = false;
b2DistanceOutput output;
b2SimplexCache cache;
cache.count = 0;
b2Distance(&output, &cache, &input);
if (output.distance == 0 || cache.count == 3)
{
cache.count += 0;
}
}
#endif
// Leap of faith to new safe state.
for (int32 i = 0; i < m_bodyCount; ++i)
{
m_bodies[i]->m_sweep.a0 = m_bodies[i]->m_sweep.a;
m_bodies[i]->m_sweep.c0 = m_bodies[i]->m_sweep.c;
}
// No warm starting is needed for TOI events because warm
// starting impulses were applied in the discrete solver.
contactSolver.InitializeVelocityConstraints();
// Solve velocity constraints.
for (int32 i = 0; i < subStep.velocityIterations; ++i)
{
contactSolver.SolveVelocityConstraints();
}
// Don't store the TOI contact forces for warm starting
// because they can be quite large.
// Integrate positions.
for (int32 i = 0; i < m_bodyCount; ++i)
{
b2Body* b = m_bodies[i];
if (b->GetType() == b2_staticBody)
{
continue;
}
// Check for large velocities.
b2Vec2 translation = subStep.dt * b->m_linearVelocity;
if (b2Dot(translation, translation) > b2_maxTranslationSquared)
{
translation.Normalize();
b->m_linearVelocity = (b2_maxTranslation * subStep.inv_dt) * translation;
}
float32 rotation = subStep.dt * b->m_angularVelocity;
if (rotation * rotation > b2_maxRotationSquared)
{
if (rotation < 0.0)
{
b->m_angularVelocity = -subStep.inv_dt * b2_maxRotation;
}
else
{
b->m_angularVelocity = subStep.inv_dt * b2_maxRotation;
}
}
// Integrate
b->m_sweep.c += subStep.dt * b->m_linearVelocity;
b->m_sweep.a += subStep.dt * b->m_angularVelocity;
// Compute new transform
b->SynchronizeTransform();
// Note: shapes are synchronized later.
}
Report(contactSolver.m_constraints);
}
void Island::Step(const TimeStep &timeStep)
{
ContactSolver contactSolver(*this);
// update velocity
for (auto &body : mRigidBodies)
{
switch (body.mType)
{
case RigidBody::DYNAMIC:
float dtscale = body.mParent->cphy->dtScale;
// apply gravity
if (body.mHasGravity)
{
Vector3 newG( mGravity + body.mGravityFactor);
body.mLinearVelocity += newG * timeStep.dt;
}
// integrate velocity
body.mLinearVelocity +=
body.mMassData.inverseMass * (body.mLinearImpulseAccumulator + body.mForceAccumulator * timeStep.dt * dtscale);
body.mAngularVelocity +=
body.mGlobalInverseInertiaTensor * (body.mAngularImpulseAccumulator + body.mTorqueAccumulator * timeStep.dt * dtscale);
body.mForceAccumulator.ZeroOut();
body.mTorqueAccumulator.ZeroOut();
body.mLinearImpulseAccumulator.ZeroOut();
body.mAngularImpulseAccumulator.ZeroOut();
body.mLinearVelocity *= mLinearDamp;
body.mAngularVelocity *= mAngularDamp;
break;
}
}
// warm start
if (timeStep.params.warmStart)
contactSolver.WarmStartVelocityConstraints(timeStep);
// initialize constraints
for (auto &joint : mJoints)
joint.InitializeVelocityConstraints(timeStep);
contactSolver.InitializeVelocityConstraints(timeStep);
// solve constraints
for (unsigned char i = 0; i < timeStep.velocityIterations; ++i)
{
for (auto &joint : mJoints)
joint.SolveVelocityConstraints(timeStep);
contactSolver.SolveVelocityConstraints(timeStep);
}
// cache velocity constraint results
if (timeStep.params.warmStart)
contactSolver.CacheVelocityConstraintResults(timeStep);
// integrate position
for (auto &body : mRigidBodies)
{
// integrate
body.IntegratePosition(timeStep);
// update
body.UpdateOrientation();
body.UpdatePositionFromGlobalCentroid();
}
// solve position constraints, NOPE
for (unsigned char i = 0; i < timeStep.positionIterations; ++i)
{
for (auto &joint : mJoints)
joint.SolvePositionConstraints(timeStep);
contactSolver.SolvePositionConstraints(timeStep);
}
// update proxies
for (auto &body : mRigidBodies)
body.UpdateProxies();
}
