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 * (b->m_gravityScale * 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.
b2ContactSolverDef solverDef;
solverDef.contacts = m_contacts;
solverDef.count = m_contactCount;
solverDef.allocator = m_allocator;
solverDef.impulseRatio = step.dtRatio;
solverDef.warmStarting = step.warmStarting;
b2ContactSolver contactSolver(&solverDef);
contactSolver.InitializeVelocityConstraints();
if (step.warmStarting)
{
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);
}
}
}
}
// From Real-time Collision Detection, p179.
bool b2AABB::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const
{
float32 tmin = -b2_maxFloat;
float32 tmax = b2_maxFloat;
b2Vec2 p = input.p1;
b2Vec2 d = input.p2 - input.p1;
b2Vec2 absD = b2Abs(d);
b2Vec2 normal;
for (int32 i = 0; i < 2; ++i)
{
if (absD(i) < b2_epsilon)
{
// Parallel.
if (p(i) < lowerBound(i) || upperBound(i) < p(i))
{
return false;
}
}
else
{
float32 inv_d = 1.0f / d(i);
float32 t1 = (lowerBound(i) - p(i)) * inv_d;
float32 t2 = (upperBound(i) - p(i)) * inv_d;
// Sign of the normal vector.
float32 s = -1.0f;
if (t1 > t2)
{
b2Swap(t1, t2);
s = 1.0f;
}
// Push the min up
if (t1 > tmin)
{
normal.SetZero();
normal(i) = s;
tmin = t1;
}
// Pull the max down
tmax = b2Min(tmax, t2);
if (tmin > tmax)
{
return false;
}
}
}
// Does the ray start inside the box?
// Does the ray intersect beyond the max fraction?
if (tmin < 0.0f || input.maxFraction < tmin)
{
return false;
}
// Intersection.
output->fraction = tmin;
output->normal = normal;
return true;
}
// Find TOI contacts and solve them.
void b2World::SolveTOI(const b2TimeStep& step)
{
b2Island island(2 * b2_maxTOIContacts, b2_maxTOIContacts, 0, &m_stackAllocator, m_contactManager.m_contactListener);
if (m_stepComplete)
{
for (b2Body* b = m_bodyList; b; b = b->m_next)
{
b->m_flags &= ~b2Body::e_islandFlag;
b->m_sweep.alpha0 = 0.0f;
}
for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next)
{
// Invalidate TOI
c->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag);
c->m_toiCount = 0;
c->m_toi = 1.0f;
}
}
// Find TOI events and solve them.
for (;;)
{
// Find the first TOI.
b2Contact* minContact = nullptr;
float32 minAlpha = 1.0f;
for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next)
{
// Is this contact disabled?
if (c->IsEnabled() == false)
{
continue;
}
// Prevent excessive sub-stepping.
if (c->m_toiCount > b2_maxSubSteps)
{
continue;
}
float32 alpha = 1.0f;
if (c->m_flags & b2Contact::e_toiFlag)
{
// This contact has a valid cached TOI.
alpha = c->m_toi;
}
else
{
b2Fixture* fA = c->GetFixtureA();
b2Fixture* fB = c->GetFixtureB();
// Is there a sensor?
if (fA->IsSensor() || fB->IsSensor())
{
continue;
}
b2Body* bA = fA->GetBody();
b2Body* bB = fB->GetBody();
b2BodyType typeA = bA->m_type;
b2BodyType typeB = bB->m_type;
b2Assert(typeA == b2_dynamicBody || typeB == b2_dynamicBody);
bool activeA = bA->IsAwake() && typeA != b2_staticBody;
bool activeB = bB->IsAwake() && typeB != b2_staticBody;
// Is at least one body active (awake and dynamic or kinematic)?
if (activeA == false && activeB == false)
{
continue;
}
bool collideA = bA->IsBullet() || typeA != b2_dynamicBody;
bool collideB = bB->IsBullet() || typeB != b2_dynamicBody;
// Are these two non-bullet dynamic bodies?
if (collideA == false && collideB == false)
{
continue;
}
// Compute the TOI for this contact.
// Put the sweeps onto the same time interval.
float32 alpha0 = bA->m_sweep.alpha0;
if (bA->m_sweep.alpha0 < bB->m_sweep.alpha0)
{
alpha0 = bB->m_sweep.alpha0;
bA->m_sweep.Advance(alpha0);
}
else if (bB->m_sweep.alpha0 < bA->m_sweep.alpha0)
{
alpha0 = bA->m_sweep.alpha0;
bB->m_sweep.Advance(alpha0);
}
b2Assert(alpha0 < 1.0f);
int32 indexA = c->GetChildIndexA();
int32 indexB = c->GetChildIndexB();
// Compute the time of impact in interval [0, minTOI]
b2TOIInput input;
input.proxyA.Set(fA->GetShape(), indexA);
input.proxyB.Set(fB->GetShape(), indexB);
input.sweepA = bA->m_sweep;
input.sweepB = bB->m_sweep;
input.tMax = 1.0f;
b2TOIOutput output;
b2TimeOfImpact(&output, &input);
// Beta is the fraction of the remaining portion of the .
float32 beta = output.t;
if (output.state == b2TOIOutput::e_touching)
{
alpha = b2Min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
}
else
{
alpha = 1.0f;
}
c->m_toi = alpha;
c->m_flags |= b2Contact::e_toiFlag;
}
if (alpha < minAlpha)
{
// This is the minimum TOI found so far.
minContact = c;
minAlpha = alpha;
}
}
if (minContact == nullptr || 1.0f - 10.0f * b2_epsilon < minAlpha)
{
// No more TOI events. Done!
m_stepComplete = true;
break;
}
// Advance the bodies to the TOI.
b2Fixture* fA = minContact->GetFixtureA();
b2Fixture* fB = minContact->GetFixtureB();
b2Body* bA = fA->GetBody();
b2Body* bB = fB->GetBody();
b2Sweep backup1 = bA->m_sweep;
b2Sweep backup2 = bB->m_sweep;
bA->Advance(minAlpha);
bB->Advance(minAlpha);
// The TOI contact likely has some new contact points.
minContact->Update(m_contactManager.m_contactListener);
minContact->m_flags &= ~b2Contact::e_toiFlag;
++minContact->m_toiCount;
// Is the contact solid?
if (minContact->IsEnabled() == false || minContact->IsTouching() == false)
{
// Restore the sweeps.
minContact->SetEnabled(false);
bA->m_sweep = backup1;
bB->m_sweep = backup2;
bA->SynchronizeTransform();
bB->SynchronizeTransform();
continue;
}
bA->SetAwake(true);
bB->SetAwake(true);
// Build the island
island.Clear();
island.Add(bA);
island.Add(bB);
island.Add(minContact);
bA->m_flags |= b2Body::e_islandFlag;
bB->m_flags |= b2Body::e_islandFlag;
minContact->m_flags |= b2Contact::e_islandFlag;
// Get contacts on bodyA and bodyB.
b2Body* bodies[2] = {bA, bB};
for (int32 i = 0; i < 2; ++i)
{
b2Body* body = bodies[i];
if (body->m_type == b2_dynamicBody)
{
for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next)
{
if (island.m_bodyCount == island.m_bodyCapacity)
{
break;
}
if (island.m_contactCount == island.m_contactCapacity)
{
break;
}
b2Contact* contact = ce->contact;
// Has this contact already been added to the island?
if (contact->m_flags & b2Contact::e_islandFlag)
{
continue;
}
// Only add static, kinematic, or bullet bodies.
b2Body* other = ce->other;
if (other->m_type == b2_dynamicBody &&
body->IsBullet() == false && other->IsBullet() == false)
{
continue;
}
// Skip sensors.
bool sensorA = contact->m_fixtureA->m_isSensor;
bool sensorB = contact->m_fixtureB->m_isSensor;
if (sensorA || sensorB)
{
continue;
}
// Tentatively advance the body to the TOI.
b2Sweep backup = other->m_sweep;
if ((other->m_flags & b2Body::e_islandFlag) == 0)
{
other->Advance(minAlpha);
}
// Update the contact points
contact->Update(m_contactManager.m_contactListener);
// Was the contact disabled by the user?
if (contact->IsEnabled() == false)
{
other->m_sweep = backup;
other->SynchronizeTransform();
continue;
}
// Are there contact points?
if (contact->IsTouching() == false)
{
other->m_sweep = backup;
other->SynchronizeTransform();
continue;
}
// Add the contact to the island
contact->m_flags |= b2Contact::e_islandFlag;
island.Add(contact);
// Has the other body already been added to the island?
if (other->m_flags & b2Body::e_islandFlag)
{
continue;
}
// Add the other body to the island.
other->m_flags |= b2Body::e_islandFlag;
if (other->m_type != b2_staticBody)
{
other->SetAwake(true);
}
island.Add(other);
}
}
}
b2TimeStep subStep;
subStep.dt = (1.0f - minAlpha) * step.dt;
subStep.inv_dt = 1.0f / subStep.dt;
subStep.dtRatio = 1.0f;
subStep.positionIterations = 20;
subStep.velocityIterations = step.velocityIterations;
subStep.warmStarting = false;
island.SolveTOI(subStep, bA->m_islandIndex, bB->m_islandIndex);
// Reset island flags and synchronize broad-phase proxies.
for (int32 i = 0; i < island.m_bodyCount; ++i)
{
b2Body* body = island.m_bodies[i];
body->m_flags &= ~b2Body::e_islandFlag;
if (body->m_type != b2_dynamicBody)
{
continue;
}
body->SynchronizeFixtures();
// Invalidate all contact TOIs on this displaced body.
for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next)
{
ce->contact->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag);
}
}
// Commit fixture proxy movements to the broad-phase so that new contacts are created.
// Also, some contacts can be destroyed.
m_contactManager.FindNewContacts();
if (m_subStepping)
{
m_stepComplete = false;
break;
}
}
}
void b2PolygonShape::Set(const b2Vec2* vertices, int32 count)
{
b2Assert(3 <= count && count <= b2_maxPolygonVertices);
if (count < 3)
{
SetAsBox(1.0f, 1.0f);
return;
}
int32 n = b2Min(count, b2_maxPolygonVertices);
// Perform welding and copy vertices into local buffer.
b2Vec2 ps[b2_maxPolygonVertices];
int32 tempCount = 0;
for (int32 i = 0; i < n; ++i)
{
b2Vec2 v = vertices[i];
bool unique = true;
for (int32 j = 0; j < tempCount; ++j)
{
if (b2DistanceSquared(v, ps[j]) < 0.5f * b2_linearSlop)
{
unique = false;
break;
}
}
if (unique)
{
ps[tempCount++] = v;
}
}
n = tempCount;
if (n < 3)
{
// Polygon is degenerate.
b2Assert(false);
SetAsBox(1.0f, 1.0f);
return;
}
// Create the convex hull using the Gift wrapping algorithm
// http://en.wikipedia.org/wiki/Gift_wrapping_algorithm
// Find the right most point on the hull
int32 i0 = 0;
float32 x0 = ps[0].x;
for (int32 i = 1; i < n; ++i)
{
float32 x = ps[i].x;
if (x > x0 || (x == x0 && ps[i].y < ps[i0].y))
{
i0 = i;
x0 = x;
}
}
int32 hull[b2_maxPolygonVertices];
int32 m = 0;
int32 ih = i0;
for (;;)
{
hull[m] = ih;
int32 ie = 0;
for (int32 j = 1; j < n; ++j)
{
if (ie == ih)
{
ie = j;
continue;
}
b2Vec2 r = ps[ie] - ps[hull[m]];
b2Vec2 v = ps[j] - ps[hull[m]];
float32 c = b2Cross(r, v);
if (c < 0.0f)
{
ie = j;
}
// Collinearity check
if (c == 0.0f && v.LengthSquared() > r.LengthSquared())
{
ie = j;
}
}
++m;
ih = ie;
if (ie == i0)
{
break;
}
}
if (m < 3)
{
// Polygon is degenerate.
b2Assert(false);
SetAsBox(1.0f, 1.0f);
return;
}
m_count = m;
// Copy vertices.
for (int32 i = 0; i < m; ++i)
{
m_vertices[i] = ps[hull[i]];
}
// Compute normals. Ensure the edges have non-zero length.
for (int32 i = 0; i < m; ++i)
{
int32 i1 = i;
int32 i2 = i + 1 < m ? i + 1 : 0;
b2Vec2 edge = m_vertices[i2] - m_vertices[i1];
b2Assert(edge.LengthSquared() > b2_epsilon * b2_epsilon);
m_normals[i] = b2Cross(edge, 1.0f);
m_normals[i].Normalize();
}
// Compute the polygon centroid.
m_centroid = ComputeCentroid(m_vertices, m);
}
b2PolyShape::b2PolyShape(const b2ShapeDef* def, b2Body* body,
const b2Vec2& newOrigin)
: b2Shape(def, body)
{
b2Assert(def->type == e_boxShape || def->type == e_polyShape);
m_type = e_polyShape;
b2Mat22 localR(def->localRotation);
// Get the vertices transformed into the body frame.
if (def->type == e_boxShape)
{
m_localCentroid = def->localPosition - newOrigin;
const b2BoxDef* box = (const b2BoxDef*)def;
m_vertexCount = 4;
b2Vec2 h = box->extents;
b2Vec2 hc = h;
hc.x = b2Max(0.0f, h.x - 2.0f * b2_linearSlop);
hc.y = b2Max(0.0f, h.y - 2.0f * b2_linearSlop);
m_vertices[0] = b2Mul(localR, b2Vec2(h.x, h.y));
m_vertices[1] = b2Mul(localR, b2Vec2(-h.x, h.y));
m_vertices[2] = b2Mul(localR, b2Vec2(-h.x, -h.y));
m_vertices[3] = b2Mul(localR, b2Vec2(h.x, -h.y));
m_coreVertices[0] = b2Mul(localR, b2Vec2(hc.x, hc.y));
m_coreVertices[1] = b2Mul(localR, b2Vec2(-hc.x, hc.y));
m_coreVertices[2] = b2Mul(localR, b2Vec2(-hc.x, -hc.y));
m_coreVertices[3] = b2Mul(localR, b2Vec2(hc.x, -hc.y));
}
else
{
const b2PolyDef* poly = (const b2PolyDef*)def;
m_vertexCount = poly->vertexCount;
b2Assert(3 <= m_vertexCount && m_vertexCount <= b2_maxPolyVertices);
b2Vec2 centroid = PolyCentroid(poly->vertices, poly->vertexCount);
m_localCentroid = def->localPosition + b2Mul(localR, centroid) - newOrigin;
for (int32 i = 0; i < m_vertexCount; ++i)
{
m_vertices[i] = b2Mul(localR, poly->vertices[i] - centroid);
b2Vec2 u = m_vertices[i];
float32 length = u.Length();
if (length > FLT_EPSILON)
{
u *= 1.0f / length;
}
m_coreVertices[i] = m_vertices[i] - 2.0f * b2_linearSlop * u;
}
}
// Compute bounding box. TODO_ERIN optimize OBB
b2Vec2 minVertex(FLT_MAX, FLT_MAX);
b2Vec2 maxVertex(-FLT_MAX, -FLT_MAX);
m_maxRadius = 0.0f;
for (int32 i = 0; i < m_vertexCount; ++i)
{
b2Vec2 v = m_vertices[i];
minVertex = b2Min(minVertex, v);
maxVertex = b2Max(maxVertex, v);
m_maxRadius = b2Max(m_maxRadius, v.Length());
}
m_localOBB.R.SetIdentity();
m_localOBB.center = 0.5f * (minVertex + maxVertex);
m_localOBB.extents = 0.5f * (maxVertex - minVertex);
// Compute the edge normals and next index map.
for (int32 i = 0; i < m_vertexCount; ++i)
{
int32 i1 = i;
int32 i2 = i + 1 < m_vertexCount ? i + 1 : 0;
b2Vec2 edge = m_vertices[i2] - m_vertices[i1];
m_normals[i] = b2Cross(edge, 1.0f);
m_normals[i].Normalize();
}
// Ensure the polygon in convex. TODO_ERIN compute convex hull.
for (int32 i = 0; i < m_vertexCount; ++i)
{
int32 i1 = i;
int32 i2 = i + 1 < m_vertexCount ? i + 1 : 0;
NOT_USED(i1);
NOT_USED(i2);
b2Assert(b2Cross(m_normals[i1], m_normals[i2]) > FLT_EPSILON);
}
m_R = m_body->m_R;
m_position = m_body->m_position + b2Mul(m_body->m_R, m_localCentroid);
b2Mat22 R = b2Mul(m_R, m_localOBB.R);
b2Mat22 absR = b2Abs(R);
b2Vec2 h = b2Mul(absR, m_localOBB.extents);
b2Vec2 position = m_position + b2Mul(m_R, m_localOBB.center);
b2AABB aabb;
aabb.minVertex = position - h;
aabb.maxVertex = position + h;
b2BroadPhase* broadPhase = m_body->m_world->m_broadPhase;
if (broadPhase->InRange(aabb))
{
m_proxyId = broadPhase->CreateProxy(aabb, this);
}
else
{
m_proxyId = b2_nullProxy;
}
if (m_proxyId == b2_nullProxy)
{
m_body->Freeze();
}
}
void Keyboard(unsigned char key, int x, int y)
{
B2_NOT_USED(x);
B2_NOT_USED(y);
switch (key)
{
case 27:
exit(0);
break;
// Press 'z' to zoom out.
case 'z':
viewZoom = b2Min(1.1f * viewZoom, 20.0f);
Resize(width, height);
break;
// Press 'x' to zoom in.
case 'x':
viewZoom = b2Max(0.9f * viewZoom, 0.02f);
Resize(width, height);
break;
// Press 'r' to reset.
case 'r':
delete test;
test = entry->createFcn();
break;
// Press space to launch a bomb.
case ' ':
if (test)
{
test->LaunchBomb();
}
break;
case 'p':
settings.pause = !settings.pause;
break;
// Press [ to prev test.
case '[':
--testSelection;
if (testSelection < 0)
{
testSelection = testCount - 1;
}
glui->sync_live();
break;
// Press ] to next test.
case ']':
++testSelection;
if (testSelection == testCount)
{
testSelection = 0;
}
glui->sync_live();
break;
case 's':
agent_message("save_policy results.dat");
printf("saved... value function\n");
break;
case 'l':
agent_message("load_policy results.dat");
printf("load... value function\n");
success = 0;
fail = 0;
break;
case 'f':
agent_message("freeze learning");
break;
default:
if (test)
{
test->Keyboard(key);
}
}
}
// Sequential position solver for position constraints.
bool b2ContactSolver::SolveTOIPositionConstraints(qreal baumgarte, const b2Body* toiBodyA, const b2Body* toiBodyB)
{
qreal minSeparation = 0.0f;
for (int32 i = 0; i < m_count; ++i)
{
b2ContactConstraint* c = m_constraints + i;
b2Body* bodyA = c->bodyA;
b2Body* bodyB = c->bodyB;
qreal massA = 0.0f;
if (bodyA == toiBodyA || bodyA == toiBodyB)
{
massA = bodyA->m_mass;
}
qreal massB = 0.0f;
if (bodyB == toiBodyA || bodyB == toiBodyB)
{
massB = bodyB->m_mass;
}
qreal invMassA = bodyA->m_mass * bodyA->m_invMass;
qreal invIA = bodyA->m_mass * bodyA->m_invI;
qreal invMassB = bodyB->m_mass * bodyB->m_invMass;
qreal invIB = bodyB->m_mass * bodyB->m_invI;
// Solve normal constraints
for (int32 j = 0; j < c->pointCount; ++j)
{
b2PositionSolverManifold psm;
psm.Initialize(c, j);
b2Vec2 normal = psm.normal;
b2Vec2 point = psm.point;
qreal separation = psm.separation;
b2Vec2 rA = point - bodyA->m_sweep.c;
b2Vec2 rB = point - bodyB->m_sweep.c;
// Track max constraint error.
minSeparation = b2Min(minSeparation, separation);
// Prevent large corrections and allow slop.
qreal C = b2Clamp(baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);
// Compute the effective mass.
qreal rnA = b2Cross(rA, normal);
qreal rnB = b2Cross(rB, normal);
qreal K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;
// Compute normal impulse
qreal impulse = K > 0.0f ? - C / K : 0.0f;
b2Vec2 P = impulse * normal;
bodyA->m_sweep.c -= invMassA * P;
bodyA->m_sweep.a -= invIA * b2Cross(rA, P);
bodyA->SynchronizeTransform();
bodyB->m_sweep.c += invMassB * P;
bodyB->m_sweep.a += invIB * b2Cross(rB, P);
bodyB->SynchronizeTransform();
}
}
// We can't expect minSpeparation >= -b2_linearSlop because we don't
// push the separation above -b2_linearSlop.
return minSeparation >= -1.5f * b2_linearSlop;
}
// Find TOI contacts and solve them.
void b2World::SolveTOI(const b2TimeStep& step)
{
// Reserve an island and a queue for TOI island solution.
b2Island island(m_bodyCount,
b2_maxTOIContactsPerIsland,
b2_maxTOIJointsPerIsland,
&m_stackAllocator,
m_contactManager.m_contactListener);
//Simple one pass queue
//Relies on the fact that we're only making one pass
//through and each body can only be pushed/popped once.
//To push:
// queue[queueStart+queueSize++] = newElement;
//To pop:
// poppedElement = queue[queueStart++];
// --queueSize;
int32 queueCapacity = m_bodyCount;
b2Body** queue = (b2Body**)m_stackAllocator.Allocate(queueCapacity* sizeof(b2Body*));
for (b2Body* b = m_bodyList; b; b = b->m_next)
{
b->m_flags &= ~b2Body::e_islandFlag;
b->m_sweep.t0 = 0.0f;
}
for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next)
{
// Invalidate TOI
c->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag);
}
for (b2Joint* j = m_jointList; j; j = j->m_next)
{
j->m_islandFlag = false;
}
// Find TOI events and solve them.
for (;;)
{
// Find the first TOI.
b2Contact* minContact = NULL;
float32 minTOI = 1.0f;
for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next)
{
// Can this contact generate a solid TOI contact?
if (c->IsSolid() == false || c->IsContinuous() == false)
{
continue;
}
// TODO_ERIN keep a counter on the contact, only respond to M TOIs per contact.
float32 toi = 1.0f;
if (c->m_flags & b2Contact::e_toiFlag)
{
// This contact has a valid cached TOI.
toi = c->m_toi;
}
else
{
// Compute the TOI for this contact.
b2Fixture* s1 = c->GetFixtureA();
b2Fixture* s2 = c->GetFixtureB();
b2Body* b1 = s1->GetBody();
b2Body* b2 = s2->GetBody();
if ((b1->IsStatic() || b1->IsSleeping()) && (b2->IsStatic() || b2->IsSleeping()))
{
continue;
}
// Put the sweeps onto the same time interval.
float32 t0 = b1->m_sweep.t0;
if (b1->m_sweep.t0 < b2->m_sweep.t0)
{
t0 = b2->m_sweep.t0;
b1->m_sweep.Advance(t0);
}
else if (b2->m_sweep.t0 < b1->m_sweep.t0)
{
t0 = b1->m_sweep.t0;
b2->m_sweep.Advance(t0);
}
b2Assert(t0 < 1.0f);
// Compute the time of impact.
toi = c->ComputeTOI(b1->m_sweep, b2->m_sweep);
b2Assert(0.0f <= toi && toi <= 1.0f);
// If the TOI is in range ...
if (0.0f < toi && toi < 1.0f)
{
// Interpolate on the actual range.
toi = b2Min((1.0f - toi) * t0 + toi, 1.0f);
}
c->m_toi = toi;
c->m_flags |= b2Contact::e_toiFlag;
}
if (B2_FLT_EPSILON < toi && toi < minTOI)
{
// This is the minimum TOI found so far.
minContact = c;
minTOI = toi;
}
}
if (minContact == NULL || 1.0f - 100.0f * B2_FLT_EPSILON < minTOI)
{
// No more TOI events. Done!
break;
}
// Advance the bodies to the TOI.
b2Fixture* s1 = minContact->GetFixtureA();
b2Fixture* s2 = minContact->GetFixtureB();
b2Body* b1 = s1->GetBody();
b2Body* b2 = s2->GetBody();
b2Sweep backup1 = b1->m_sweep;
b2Sweep backup2 = b2->m_sweep;
b1->Advance(minTOI);
b2->Advance(minTOI);
// The TOI contact likely has some new contact points.
minContact->Update(m_contactManager.m_contactListener);
minContact->m_flags &= ~b2Contact::e_toiFlag;
// Is the contact solid?
if (minContact->IsSolid() == false)
{
// Restore the sweeps.
b1->m_sweep = backup1;
b2->m_sweep = backup2;
b1->SynchronizeTransform();
b2->SynchronizeTransform();
continue;
}
// Did numerical issues prevent a contact point from being generated?
if (minContact->IsTouching() == false)
{
// Give up on this TOI.
continue;
}
// Build the TOI island. We need a dynamic seed.
b2Body* seed = b1;
if (seed->IsStatic())
{
seed = b2;
}
// Reset island and queue.
island.Clear();
int32 queueStart = 0; // starting index for queue
int32 queueSize = 0; // elements in queue
queue[queueStart + queueSize++] = seed;
seed->m_flags |= b2Body::e_islandFlag;
// Perform a breadth first search (BFS) on the contact/joint graph.
while (queueSize > 0)
{
// Grab the next body off the stack and add it to the island.
b2Body* b = queue[queueStart++];
--queueSize;
island.Add(b);
// Make sure the body is awake.
b->m_flags &= ~b2Body::e_sleepFlag;
// To keep islands as small as possible, we don't
// propagate islands across static bodies.
if (b->IsStatic())
{
continue;
}
// Search all contacts connected to this body.
for (b2ContactEdge* cEdge = b->m_contactList; cEdge; cEdge = cEdge->next)
{
// Does the TOI island still have space for contacts?
if (island.m_contactCount == island.m_contactCapacity)
{
break;
}
// Has this contact already been added to an island?
if (cEdge->contact->m_flags & b2Contact::e_islandFlag)
{
continue;
}
// Skip separate, sensor, or disabled contacts.
if (cEdge->contact->IsSolid() == false || cEdge->contact->IsTouching() == false)
{
continue;
}
island.Add(cEdge->contact);
cEdge->contact->m_flags |= b2Contact::e_islandFlag;
// Update other body.
b2Body* other = cEdge->other;
// Was the other body already added to this island?
if (other->m_flags & b2Body::e_islandFlag)
{
continue;
}
// March forward, this can do no harm since this is the min TOI.
if (other->IsStatic() == false)
{
other->Advance(minTOI);
other->WakeUp();
}
b2Assert(queueStart + queueSize < queueCapacity);
queue[queueStart + queueSize] = other;
++queueSize;
other->m_flags |= b2Body::e_islandFlag;
}
for (b2JointEdge* jEdge = b->m_jointList; jEdge; jEdge = jEdge->next)
{
if (island.m_jointCount == island.m_jointCapacity)
{
continue;
}
if (jEdge->joint->m_islandFlag == true)
{
continue;
}
island.Add(jEdge->joint);
jEdge->joint->m_islandFlag = true;
b2Body* other = jEdge->other;
if (other->m_flags & b2Body::e_islandFlag)
{
continue;
}
if (!other->IsStatic())
{
other->Advance(minTOI);
other->WakeUp();
}
b2Assert(queueStart + queueSize < queueCapacity);
queue[queueStart + queueSize] = other;
++queueSize;
other->m_flags |= b2Body::e_islandFlag;
}
}
b2TimeStep subStep;
subStep.warmStarting = false;
subStep.dt = (1.0f - minTOI) * step.dt;
subStep.inv_dt = 1.0f / subStep.dt;
subStep.dtRatio = 0.0f;
subStep.velocityIterations = step.velocityIterations;
subStep.positionIterations = step.positionIterations;
island.SolveTOI(subStep);
// Post solve cleanup.
for (int32 i = 0; i < island.m_bodyCount; ++i)
{
// Allow bodies to participate in future TOI islands.
b2Body* b = island.m_bodies[i];
b->m_flags &= ~b2Body::e_islandFlag;
if (b->m_flags & b2Body::e_sleepFlag)
{
continue;
}
if (b->IsStatic())
{
continue;
}
// Update fixtures (for broad-phase).
b->SynchronizeFixtures();
// Invalidate all contact TOIs associated with this body. Some of these
// may not be in the island because they were not touching.
for (b2ContactEdge* ce = b->m_contactList; ce; ce = ce->next)
{
ce->contact->m_flags &= ~b2Contact::e_toiFlag;
}
}
for (int32 i = 0; i < island.m_contactCount; ++i)
{
// Allow contacts to participate in future TOI islands.
b2Contact* c = island.m_contacts[i];
c->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag);
}
for (int32 i = 0; i < island.m_jointCount; ++i)
{
// Allow joints to participate in future TOI islands.
b2Joint* j = island.m_joints[i];
j->m_islandFlag = false;
}
// Commit fixture proxy movements to the broad-phase so that new contacts are created.
// Also, some contacts can be destroyed.
m_contactManager.FindNewContacts();
}
m_stackAllocator.Free(queue);
}
// Find TOI contacts and solve them.
void b2World::SolveTOI(const b2TimeStep& step)
{
// Reserve an island and a stack for TOI island solution.
b2Island island(m_bodyCount, b2_maxTOIContactsPerIsland, 0, &m_stackAllocator, m_contactListener);
int32 stackSize = m_bodyCount;
b2Body** stack = (b2Body**)m_stackAllocator.Allocate(stackSize * sizeof(b2Body*));
for (b2Body* b = m_bodyList; b; b = b->m_next)
{
b->m_flags &= ~b2Body::e_islandFlag;
b->m_sweep.t0 = 0.0f;
}
for (b2Contact* c = m_contactList; c; c = c->m_next)
{
// Invalidate TOI
c->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag);
}
// Find TOI events and solve them.
for (;;)
{
// Find the first TOI.
b2Contact* minContact = NULL;
float32 minTOI = 1.0f;
for (b2Contact* c = m_contactList; c; c = c->m_next)
{
if (c->m_flags & (b2Contact::e_slowFlag | b2Contact::e_nonSolidFlag))
{
continue;
}
// TODO_ERIN keep a counter on the contact, only respond to M TOIs per contact.
float32 toi = 1.0f;
if (c->m_flags & b2Contact::e_toiFlag)
{
// This contact has a valid cached TOI.
toi = c->m_toi;
}
else
{
// Compute the TOI for this contact.
b2Shape* s1 = c->GetShape1();
b2Shape* s2 = c->GetShape2();
b2Body* b1 = s1->GetBody();
b2Body* b2 = s2->GetBody();
if ((b1->IsStatic() || b1->IsSleeping()) && (b2->IsStatic() || b2->IsSleeping()))
{
continue;
}
// Put the sweeps onto the same time interval.
float32 t0 = b1->m_sweep.t0;
if (b1->m_sweep.t0 < b2->m_sweep.t0)
{
t0 = b2->m_sweep.t0;
b1->m_sweep.Advance(t0);
}
else if (b2->m_sweep.t0 < b1->m_sweep.t0)
{
t0 = b1->m_sweep.t0;
b2->m_sweep.Advance(t0);
}
b2Assert(t0 < 1.0f);
// Compute the time of impact.
toi = b2TimeOfImpact(c->m_shape1, b1->m_sweep, c->m_shape2, b2->m_sweep);
b2Assert(0.0f <= toi && toi <= 1.0f);
if (toi > 0.0f && toi < 1.0f)
{
toi = b2Min((1.0f - toi) * t0 + toi, 1.0f);
}
c->m_toi = toi;
c->m_flags |= b2Contact::e_toiFlag;
}
if (B2_FLT_EPSILON < toi && toi < minTOI)
{
// This is the minimum TOI found so far.
minContact = c;
minTOI = toi;
}
}
if (minContact == NULL || 1.0f - 100.0f * B2_FLT_EPSILON < minTOI)
{
// No more TOI events. Done!
break;
}
// Advance the bodies to the TOI.
b2Shape* s1 = minContact->GetShape1();
b2Shape* s2 = minContact->GetShape2();
b2Body* b1 = s1->GetBody();
b2Body* b2 = s2->GetBody();
b1->Advance(minTOI);
b2->Advance(minTOI);
// The TOI contact likely has some new contact points.
minContact->Update(m_contactListener);
minContact->m_flags &= ~b2Contact::e_toiFlag;
if (minContact->GetManifoldCount() == 0)
{
// This shouldn't happen. Numerical error?
//b2Assert(false);
continue;
}
// Build the TOI island. We need a dynamic seed.
b2Body* seed = b1;
if (seed->IsStatic())
{
seed = b2;
}
// Reset island and stack.
island.Clear();
int32 stackCount = 0;
stack[stackCount++] = seed;
seed->m_flags |= b2Body::e_islandFlag;
// Perform a depth first search (DFS) on the contact graph.
while (stackCount > 0)
{
// Grab the next body off the stack and add it to the island.
b2Body* b = stack[--stackCount];
island.Add(b);
// Make sure the body is awake.
b->m_flags &= ~b2Body::e_sleepFlag;
// To keep islands as small as possible, we don't
// propagate islands across static bodies.
if (b->IsStatic())
{
continue;
}
// Search all contacts connected to this body.
for (b2ContactEdge* cn = b->m_contactList; cn; cn = cn->next)
{
// Does the TOI island still have space for contacts?
if (island.m_contactCount == island.m_contactCapacity)
{
continue;
}
// Has this contact already been added to an island? Skip slow or non-solid contacts.
if (cn->contact->m_flags & (b2Contact::e_islandFlag | b2Contact::e_slowFlag | b2Contact::e_nonSolidFlag))
{
continue;
}
// Is this contact touching? For performance we are not updating this contact.
if (cn->contact->GetManifoldCount() == 0)
{
continue;
}
island.Add(cn->contact);
cn->contact->m_flags |= b2Contact::e_islandFlag;
// Update other body.
b2Body* other = cn->other;
// Was the other body already added to this island?
if (other->m_flags & b2Body::e_islandFlag)
{
continue;
}
// March forward, this can do no harm since this is the min TOI.
if (other->IsStatic() == false)
{
other->Advance(minTOI);
other->WakeUp();
}
b2Assert(stackCount < stackSize);
stack[stackCount++] = other;
other->m_flags |= b2Body::e_islandFlag;
}
}
b2TimeStep subStep;
subStep.dt = (1.0f - minTOI) * step.dt;
b2Assert(subStep.dt > B2_FLT_EPSILON);
subStep.inv_dt = 1.0f / subStep.dt;
subStep.maxIterations = step.maxIterations;
island.SolveTOI(subStep);
// Post solve cleanup.
for (int32 i = 0; i < island.m_bodyCount; ++i)
{
// Allow bodies to participate in future TOI islands.
b2Body* b = island.m_bodies[i];
b->m_flags &= ~b2Body::e_islandFlag;
if (b->m_flags & (b2Body::e_sleepFlag | b2Body::e_frozenFlag))
{
continue;
}
if (b->IsStatic())
{
continue;
}
// Update shapes (for broad-phase). If the shapes go out of
// the world AABB then shapes and contacts may be destroyed,
// including contacts that are
bool inRange = b->SynchronizeShapes();
// Did the body's shapes leave the world?
if (inRange == false && m_boundaryListener != NULL)
{
m_boundaryListener->Violation(b);
}
// Invalidate all contact TOIs associated with this body. Some of these
// may not be in the island because they were not touching.
for (b2ContactEdge* cn = b->m_contactList; cn; cn = cn->next)
{
cn->contact->m_flags &= ~b2Contact::e_toiFlag;
}
}
for (int32 i = 0; i < island.m_contactCount; ++i)
{
// Allow contacts to participate in future TOI islands.
b2Contact* c = island.m_contacts[i];
c->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag);
}
// Commit shape proxy movements to the broad-phase so that new contacts are created.
// Also, some contacts can be destroyed.
m_broadPhase->Commit();
}
m_stackAllocator.Free(stack);
}
