void b2World::Step(float32 dt, int32 velocityIterations, int32 positionIterations)
{
// If new fixtures were added, we need to find the new contacts.
if (m_flags & e_newFixture)
{
m_contactManager.FindNewContacts();
m_flags &= ~e_newFixture;
}
m_flags |= e_locked;
b2TimeStep step;
step.dt = dt;
step.velocityIterations = velocityIterations;
step.positionIterations = positionIterations;
if (dt > 0.0f)
{
step.inv_dt = 1.0f / dt;
}
else
{
step.inv_dt = 0.0f;
}
step.dtRatio = m_inv_dt0 * dt;
step.warmStarting = m_warmStarting;
// Update contacts. This is where some contacts are destroyed.
m_contactManager.Collide();
// Integrate velocities, solve velocity constraints, and integrate positions.
if (step.dt > 0.0f)
{
Solve(step);
}
// Handle TOI events.
if (m_continuousPhysics && step.dt > 0.0f)
{
SolveTOI();
}
if (step.dt > 0.0f)
{
m_inv_dt0 = step.inv_dt;
}
if (m_flags & e_clearForces)
{
ClearForces();
}
m_flags &= ~e_locked;
}
//MIGUEL MODIFICATION: RESET FORCES TRIGGERING
void b2World::Step(float32 dt, int32 velocityIterations, int32 positionIterations, bool resetForces)
{
m_lock = true;
b2TimeStep step;
step.dt = dt;
step.velocityIterations = velocityIterations;
step.positionIterations = positionIterations;
step.resetForces = resetForces; //MIGUEL MODIFICATION: RESET FORCES TRIGGERING
if (dt > 0.0f)
{
step.inv_dt = 1.0f / dt;
}
else
{
step.inv_dt = 0.0f;
}
step.dtRatio = m_inv_dt0 * dt;
step.warmStarting = m_warmStarting;
// Update contacts.
m_contactManager.Collide();
// Integrate velocities, solve velocity constraints, and integrate positions.
if (step.dt > 0.0f)
{
Solve(step);
}
// Handle TOI events.
if (m_continuousPhysics && step.dt > 0.0f)
{
SolveTOI(step);
}
// Draw debug information.
DrawDebugData();
m_inv_dt0 = step.inv_dt;
m_lock = false;
}
void b2World::Step(float32 dt, int32 iterations)
{
m_lock = true;
b2TimeStep step;
step.dt = dt;
step.maxIterations = iterations;
if (dt > 0.0f)
{
step.inv_dt = 1.0f / dt;
}
else
{
step.inv_dt = 0.0f;
}
step.dtRatio = m_inv_dt0 * dt;
step.positionCorrection = m_positionCorrection;
step.warmStarting = m_warmStarting;
// Update contacts.
m_contactManager.Collide();
// Integrate velocities, solve velocity constraints, and integrate positions.
if (step.dt > 0.0f)
{
Solve(step);
}
// Handle TOI events.
if (m_continuousPhysics && step.dt > 0.0f)
{
SolveTOI(step);
}
// Draw debug information.
DrawDebugData();
m_inv_dt0 = step.inv_dt;
m_lock = false;
}
void b2World::Step(float32 dt, int32 iterations)
{
m_lock = true;
b2TimeStep step;
step.dt = dt;
step.maxIterations = iterations;
if (dt > 0.0f)
{
step.inv_dt = 1.0f / dt;
}
else
{
step.inv_dt = 0.0f;
}
// Update contacts.
m_contactManager.Collide();
// Integrate velocities, solve velocity constraints, and integrate positions.
if (step.dt > 0.0f)
{
Solve(step);
}
// Handle TOI events.
if (s_enableTOI && step.dt > 0.0f)
{
SolveTOI(step);
}
// Draw debug information.
DrawDebugData();
m_lock = false;
}
// Sequentially solve TOIs for each body. We bring each
// body to the time of contact and perform some position correction.
// Time is not conserved.
void b2World::SolveTOI()
{
// Prepare all contacts.
for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next)
{
// Enable the contact
c->m_flags |= b2Contact::e_enabledFlag;
// Set the number of TOI events for this contact to zero.
c->m_toiCount = 0;
}
// Initialize the TOI flag.
for (b2Body* body = m_bodyList; body; body = body->m_next)
{
// Kinematic, and static bodies will not be affected by the TOI event.
// If a body was not in an island then it did not move.
if ((body->m_flags & b2Body::e_islandFlag) == 0 || body->GetType() == b2_kinematicBody || body->GetType() == b2_staticBody)
{
body->m_flags |= b2Body::e_toiFlag;
}
else
{
body->m_flags &= ~b2Body::e_toiFlag;
}
}
// Collide non-bullets.
for (b2Body* body = m_bodyList; body; body = body->m_next)
{
if (body->m_flags & b2Body::e_toiFlag)
{
continue;
}
if (body->IsBullet() == true)
{
continue;
}
SolveTOI(body);
body->m_flags |= b2Body::e_toiFlag;
}
// Collide bullets.
for (b2Body* body = m_bodyList; body; body = body->m_next)
{
if (body->m_flags & b2Body::e_toiFlag)
{
continue;
}
if (body->IsBullet() == false)
{
continue;
}
SolveTOI(body);
body->m_flags |= b2Body::e_toiFlag;
}
}
// Advance a dynamic body to its first time of contact
// and adjust the position to ensure clearance.
void b2World::SolveTOI(b2Body* body)
{
// Find the minimum contact.
b2Contact* toiContact = NULL;
float32 toi = 1.0f;
b2Body* toiOther = NULL;
bool found;
int32 count;
int32 iter = 0;
bool bullet = body->IsBullet();
// Iterate until all contacts agree on the minimum TOI. We have
// to iterate because the TOI algorithm may skip some intermediate
// collisions when objects rotate through each other.
do
{
count = 0;
found = false;
for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next)
{
if (ce->contact == toiContact)
{
continue;
}
b2Body* other = ce->other;
b2BodyType type = other->GetType();
// Only bullets perform TOI with dynamic bodies.
if (bullet == true)
{
// Bullets only perform TOI with bodies that have their TOI resolved.
if ((other->m_flags & b2Body::e_toiFlag) == 0)
{
continue;
}
// No repeated hits on non-static bodies
if (type != b2_staticBody && (ce->contact->m_flags & b2Contact::e_bulletHitFlag) != 0)
{
continue;
}
}
else if (type == b2_dynamicBody)
{
continue;
}
// Check for a disabled contact.
b2Contact* contact = ce->contact;
if (contact->IsEnabled() == false)
{
continue;
}
// Prevent infinite looping.
if (contact->m_toiCount > 10)
{
continue;
}
b2Fixture* fixtureA = contact->m_fixtureA;
b2Fixture* fixtureB = contact->m_fixtureB;
// Cull sensors.
if (fixtureA->IsSensor() || fixtureB->IsSensor())
{
continue;
}
b2Body* bodyA = fixtureA->m_body;
b2Body* bodyB = fixtureB->m_body;
// Compute the time of impact in interval [0, minTOI]
b2TOIInput input;
input.proxyA.Set(fixtureA->GetShape());
input.proxyB.Set(fixtureB->GetShape());
input.sweepA = bodyA->m_sweep;
input.sweepB = bodyB->m_sweep;
input.tMax = toi;
b2TOIOutput output;
b2TimeOfImpact(&output, &input);
if (output.state == b2TOIOutput::e_touching && output.t < toi)
{
toiContact = contact;
toi = output.t;
toiOther = other;
found = true;
}
++count;
}
++iter;
} while (found && count > 1 && iter < 50);
if (toiContact == NULL)
{
body->Advance(1.0f);
return;
}
b2Sweep backup = body->m_sweep;
body->Advance(toi);
toiContact->Update(m_contactManager.m_contactListener);
if (toiContact->IsEnabled() == false)
{
// Contact disabled. Backup and recurse.
body->m_sweep = backup;
SolveTOI(body);
}
++toiContact->m_toiCount;
// Update all the valid contacts on this body and build a contact island.
b2Contact* contacts[b2_maxTOIContacts];
count = 0;
for (b2ContactEdge* ce = body->m_contactList; ce && count < b2_maxTOIContacts; ce = ce->next)
{
b2Body* other = ce->other;
b2BodyType type = other->GetType();
// Only perform correction with static bodies, so the
// body won't get pushed out of the world.
if (type == b2_dynamicBody)
{
continue;
}
// Check for a disabled contact.
b2Contact* contact = ce->contact;
if (contact->IsEnabled() == false)
{
continue;
}
b2Fixture* fixtureA = contact->m_fixtureA;
b2Fixture* fixtureB = contact->m_fixtureB;
// Cull sensors.
if (fixtureA->IsSensor() || fixtureB->IsSensor())
{
continue;
}
// The contact likely has some new contact points. The listener
// gives the user a chance to disable the contact.
if (contact != toiContact)
{
contact->Update(m_contactManager.m_contactListener);
}
// Did the user disable the contact?
if (contact->IsEnabled() == false)
{
// Skip this contact.
continue;
}
if (contact->IsTouching() == false)
{
continue;
}
contacts[count] = contact;
++count;
}
// Reduce the TOI body's overlap with the contact island.
b2TOISolver solver(&m_stackAllocator);
solver.Initialize(contacts, count, body);
const float32 k_toiBaumgarte = 0.75f;
bool solved = false;
for (int32 i = 0; i < 20; ++i)
{
bool contactsOkay = solver.Solve(k_toiBaumgarte);
if (contactsOkay)
{
solved = true;
break;
}
}
if (toiOther->GetType() != b2_staticBody)
{
toiContact->m_flags |= b2Contact::e_bulletHitFlag;
}
}
void b2World::Step(float32 dt, int32 velocityIterations, int32 positionIterations)
{
b2Timer stepTimer;
memset(&m_profile, 0, sizeof(m_profile));
// If new fixtures were added, we need to find the new contacts.
if (m_flags & e_newFixture)
{
b2Timer timer;
if (IsMultithreadedStepEnabled())
{
FindNewContactsMT();
}
else
{
m_contactManager.FindNewContacts(0, m_contactManager.m_broadPhase.GetMoveCount());
}
m_flags &= ~e_newFixture;
float32 elapsed = timer.GetMilliseconds();
m_profile.broadphase += elapsed;
m_profile.broadphaseFindContacts += elapsed;
}
m_flags |= e_locked;
b2TimeStep step;
step.dt = dt;
step.velocityIterations = velocityIterations;
step.positionIterations = positionIterations;
if (dt > 0.0f)
{
step.inv_dt = 1.0f / dt;
}
else
{
step.inv_dt = 0.0f;
}
step.dtRatio = m_inv_dt0 * dt;
step.warmStarting = m_warmStarting;
// Update contacts. This is where some contacts are destroyed.
{
b2Timer timer;
if (IsMultithreadedStepEnabled())
{
CollideMT();
}
else
{
m_contactManager.Collide(m_contactManager.m_contactsNonTOI.Data(), m_contactManager.m_contactsNonTOI.GetCount());
m_contactManager.Collide(m_contactManager.m_contactsTOI.Data(), m_contactManager.m_contactsTOI.GetCount());
}
m_profile.collide += timer.GetMilliseconds();
}
// Integrate velocities, solve velocity constraints, and integrate positions.
if (m_stepComplete && step.dt > 0.0f)
{
b2Timer timer;
if (IsMultithreadedStepEnabled())
{
SolveMT(step);
}
else
{
Solve(step);
}
m_profile.solve += timer.GetMilliseconds();
}
// Handle TOI events.
if (m_continuousPhysics && step.dt > 0.0f)
{
b2Timer timer;
SolveTOI(step);
m_profile.solveTOI += timer.GetMilliseconds();
}
if (step.dt > 0.0f)
{
m_inv_dt0 = step.inv_dt;
}
if (m_flags & e_clearForces)
{
ClearForces();
}
m_flags &= ~e_locked;
m_profile.step = stepTimer.GetMilliseconds();
}
void b2World::Step(
float32 dt,
int32 velocityIterations,
int32 positionIterations,
int32 particleIterations)
{
b2Timer stepTimer;
// If new fixtures were added, we need to find the new contacts.
if (m_flags & e_newFixture)
{
m_contactManager.FindNewContacts();
m_flags &= ~e_newFixture;
}
m_flags |= e_locked;
b2TimeStep step;
step.dt = dt;
step.velocityIterations = velocityIterations;
step.positionIterations = positionIterations;
step.particleIterations = particleIterations;
if (dt > 0.0f)
{
step.inv_dt = 1.0f / dt;
}
else
{
step.inv_dt = 0.0f;
}
step.dtRatio = m_inv_dt0 * dt;
step.warmStarting = m_warmStarting;
// Update contacts. This is where some contacts are destroyed.
{
b2Timer timer;
m_contactManager.Collide();
m_profile.collide = timer.GetMilliseconds();
}
// Integrate velocities, solve velocity constraints, and integrate positions.
if (m_stepComplete && step.dt > 0.0f)
{
b2Timer timer;
for (b2ParticleSystem* p = m_particleSystemList; p; p = p->GetNext())
{
p->Solve(step); // Particle Simulation
}
Solve(step);
m_profile.solve = timer.GetMilliseconds();
}
// Handle TOI events.
if (m_continuousPhysics && step.dt > 0.0f)
{
b2Timer timer;
SolveTOI(step);
m_profile.solveTOI = timer.GetMilliseconds();
}
if (step.dt > 0.0f)
{
m_inv_dt0 = step.inv_dt;
}
if (m_flags & e_clearForces)
{
ClearForces();
}
m_flags &= ~e_locked;
m_profile.step = stepTimer.GetMilliseconds();
}