void b2MouseJoint::InitVelocityConstraints(const b2SolverData& data)
{
m_indexB = m_bodyB->m_islandIndex;
m_localCenterB = m_bodyB->m_sweep.localCenter;
m_invMassB = m_bodyB->m_invMass;
m_invIB = m_bodyB->m_invI;
b2Vec2 cB = data.positions[m_indexB].c;
float32 aB = data.positions[m_indexB].a;
b2Vec2 vB = data.velocities[m_indexB].v;
float32 wB = data.velocities[m_indexB].w;
b2Rot qB(aB);
float32 mass = m_bodyB->GetMass();
// Frequency
float32 omega = 2.0f * b2_pi * m_frequencyHz;
// Damping coefficient
float32 d = 2.0f * mass * m_dampingRatio * omega;
// Spring stiffness
float32 k = mass * (omega * omega);
// magic formulas
// gamma has units of inverse mass.
// beta has units of inverse time.
float32 h = data.step.dt;
b2Assert(d + h * k > b2_epsilon);
m_gamma = h * (d + h * k);
if (m_gamma != 0.0f)
{
m_gamma = 1.0f / m_gamma;
}
m_beta = h * k * m_gamma;
// Compute the effective mass matrix.
m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
// = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
// [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x]
b2Mat22 K;
K.ex.x = m_invMassB + m_invIB * m_rB.y * m_rB.y + m_gamma;
K.ex.y = -m_invIB * m_rB.x * m_rB.y;
K.ey.x = K.ex.y;
K.ey.y = m_invMassB + m_invIB * m_rB.x * m_rB.x + m_gamma;
m_mass = K.GetInverse();
m_C = cB + m_rB - m_targetA;
m_C *= m_beta;
// Cheat with some damping
wB *= 0.98f;
if (data.step.warmStarting)
{
m_impulse *= data.step.dtRatio;
vB += m_invMassB * m_impulse;
wB += m_invIB * b2Cross(m_rB, m_impulse);
}
else
{
m_impulse.SetZero();
}
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
void b2World::DestroyBody(b2Body* b)
{
b2Assert(m_bodyCount > 0);
b2Assert(IsLocked() == false);
if (IsLocked())
{
return;
}
// Delete the attached joints.
b2JointEdge* je = b->m_jointList;
while (je)
{
b2JointEdge* je0 = je;
je = je->next;
if (m_destructionListener)
{
m_destructionListener->SayGoodbye(je0->joint);
}
DestroyJoint(je0->joint);
b->m_jointList = je;
}
b->m_jointList = NULL;
// Delete the attached contacts.
b2ContactEdge* ce = b->m_contactList;
while (ce)
{
b2ContactEdge* ce0 = ce;
ce = ce->next;
m_contactManager.Destroy(ce0->contact);
}
b->m_contactList = NULL;
// Delete the attached fixtures. This destroys broad-phase proxies.
b2Fixture* f = b->m_fixtureList;
while (f)
{
b2Fixture* f0 = f;
f = f->m_next;
if (m_destructionListener)
{
m_destructionListener->SayGoodbye(f0);
}
f0->DestroyProxies(&m_contactManager.m_broadPhase);
f0->Destroy(&m_blockAllocator);
f0->~b2Fixture();
m_blockAllocator.Free(f0, sizeof(b2Fixture));
b->m_fixtureList = f;
b->m_fixtureCount -= 1;
}
b->m_fixtureList = NULL;
b->m_fixtureCount = 0;
// Remove world body list.
if (b->m_prev)
{
b->m_prev->m_next = b->m_next;
}
if (b->m_next)
{
b->m_next->m_prev = b->m_prev;
}
if (b == m_bodyList)
{
m_bodyList = b->m_next;
}
--m_bodyCount;
b->~b2Body();
m_blockAllocator.Free(b, sizeof(b2Body));
}
// Find islands, integrate and solve constraints, solve position constraints
void b2World::Solve(const b2TimeStep& step)
{
// Size the island for the worst case.
b2Island island(m_bodyCount,
m_contactManager.m_contactCount,
m_jointCount,
&m_stackAllocator,
m_contactManager.m_contactListener);
// Clear all the island flags.
for (b2Body* b = m_bodyList; b; b = b->m_next)
{
b->m_flags &= ~b2Body::e_islandFlag;
}
for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next)
{
c->m_flags &= ~b2Contact::e_islandFlag;
}
for (b2Joint* j = m_jointList; j; j = j->m_next)
{
j->m_islandFlag = false;
}
// Build and simulate all awake islands.
b2_int32 stackSize = m_bodyCount;
b2Body** stack = (b2Body**)m_stackAllocator.Allocate(stackSize * sizeof(b2Body*));
for (b2Body* seed = m_bodyList; seed; seed = seed->m_next)
{
if (seed->m_flags & b2Body::e_islandFlag)
{
continue;
}
if (seed->IsAwake() == false || seed->IsActive() == false)
{
continue;
}
// The seed can be dynamic or kinematic.
if (seed->GetType() == b2_staticBody)
{
continue;
}
// Reset island and stack.
island.Clear();
b2_int32 stackCount = 0;
stack[stackCount++] = seed;
seed->m_flags |= b2Body::e_islandFlag;
// Perform a depth first search (DFS) on the constraint graph.
while (stackCount > 0)
{
// Grab the next body off the stack and add it to the island.
b2Body* b = stack[--stackCount];
b2Assert(b->IsActive() == true);
island.Add(b);
// Make sure the body is awake.
b->SetAwake(true);
// To keep islands as small as possible, we don't
// propagate islands across static bodies.
if (b->GetType() == b2_staticBody)
{
continue;
}
// Search all contacts connected to this body.
for (b2ContactEdge* ce = b->m_contactList; ce; ce = ce->next)
{
b2Contact* contact = ce->contact;
// Has this contact already been added to an island?
if (contact->m_flags & b2Contact::e_islandFlag)
{
continue;
}
// Is this contact solid and touching?
if (contact->IsEnabled() == false ||
contact->IsTouching() == false)
{
continue;
}
// Skip sensors.
bool sensorA = contact->m_fixtureA->m_isSensor;
bool sensorB = contact->m_fixtureB->m_isSensor;
if (sensorA || sensorB)
{
continue;
}
island.Add(contact);
contact->m_flags |= b2Contact::e_islandFlag;
b2Body* other = ce->other;
// Was the other body already added to this island?
if (other->m_flags & b2Body::e_islandFlag)
{
continue;
}
b2Assert(stackCount < stackSize);
stack[stackCount++] = other;
other->m_flags |= b2Body::e_islandFlag;
}
// Search all joints connect to this body.
for (b2JointEdge* je = b->m_jointList; je; je = je->next)
{
if (je->joint->m_islandFlag == true)
{
continue;
}
b2Body* other = je->other;
// Don't simulate joints connected to inactive bodies.
if (other->IsActive() == false)
{
continue;
}
island.Add(je->joint);
je->joint->m_islandFlag = true;
if (other->m_flags & b2Body::e_islandFlag)
{
continue;
}
b2Assert(stackCount < stackSize);
stack[stackCount++] = other;
other->m_flags |= b2Body::e_islandFlag;
}
}
island.Solve(step, m_gravity, m_allowSleep);
// Post solve cleanup.
for (b2_int32 i = 0; i < island.m_bodyCount; ++i)
{
// Allow static bodies to participate in other islands.
b2Body* b = island.m_bodies[i];
if (b->GetType() == b2_staticBody)
{
b->m_flags &= ~b2Body::e_islandFlag;
}
}
}
m_stackAllocator.Free(stack);
// Synchronize fixtures, check for out of range bodies.
for (b2Body* b = m_bodyList; b; b = b->GetNext())
{
// If a body was not in an island then it did not move.
if ((b->m_flags & b2Body::e_islandFlag) == 0)
{
continue;
}
if (b->GetType() == b2_staticBody)
{
continue;
}
// Update fixtures (for broad-phase).
b->SynchronizeFixtures();
}
// Look for new contacts.
m_contactManager.FindNewContacts();
}
b2ContactSolver::b2ContactSolver(b2ContactSolverDef* def)
{
m_step = def->step;
m_allocator = def->allocator;
m_count = def->count;
m_positionConstraints = (b2ContactPositionConstraint*)m_allocator->Allocate(m_count * sizeof(b2ContactPositionConstraint));
m_velocityConstraints = (b2ContactVelocityConstraint*)m_allocator->Allocate(m_count * sizeof(b2ContactVelocityConstraint));
m_positions = def->positions;
m_velocities = def->velocities;
m_contacts = def->contacts;
// Initialize position independent portions of the constraints.
for (int32 i = 0; i < m_count; ++i)
{
b2Contact* contact = m_contacts[i];
b2Fixture* fixtureA = contact->m_fixtureA;
b2Fixture* fixtureB = contact->m_fixtureB;
b2Shape* shapeA = fixtureA->GetShape();
b2Shape* shapeB = fixtureB->GetShape();
float32 radiusA = shapeA->m_radius;
float32 radiusB = shapeB->m_radius;
b2Body* bodyA = fixtureA->GetBody();
b2Body* bodyB = fixtureB->GetBody();
b2Manifold* manifold = contact->GetManifold();
int32 pointCount = manifold->pointCount;
b2Assert(pointCount > 0);
b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
vc->friction = contact->m_friction;
vc->restitution = contact->m_restitution;
vc->tangentSpeed = contact->m_tangentSpeed;
vc->indexA = bodyA->m_islandIndex;
vc->indexB = bodyB->m_islandIndex;
vc->invMassA = bodyA->m_invMass;
vc->invMassB = bodyB->m_invMass;
vc->invIA = bodyA->m_invI;
vc->invIB = bodyB->m_invI;
vc->contactIndex = i;
vc->pointCount = pointCount;
vc->K.SetZero();
vc->normalMass.SetZero();
b2ContactPositionConstraint* pc = m_positionConstraints + i;
pc->indexA = bodyA->m_islandIndex;
pc->indexB = bodyB->m_islandIndex;
pc->invMassA = bodyA->m_invMass;
pc->invMassB = bodyB->m_invMass;
pc->localCenterA = bodyA->m_sweep.localCenter;
pc->localCenterB = bodyB->m_sweep.localCenter;
pc->invIA = bodyA->m_invI;
pc->invIB = bodyB->m_invI;
pc->localNormal = manifold->localNormal;
pc->localPoint = manifold->localPoint;
pc->pointCount = pointCount;
pc->radiusA = radiusA;
pc->radiusB = radiusB;
pc->type = manifold->type;
for (int32 j = 0; j < pointCount; ++j)
{
b2ManifoldPoint* cp = manifold->points + j;
b2VelocityConstraintPoint* vcp = vc->points + j;
if (m_step.warmStarting)
{
vcp->normalImpulse = m_step.dtRatio * cp->normalImpulse;
vcp->tangentImpulse = m_step.dtRatio * cp->tangentImpulse;
}
else
{
vcp->normalImpulse = 0.0f;
vcp->tangentImpulse = 0.0f;
}
vcp->rA.SetZero();
vcp->rB.SetZero();
vcp->normalMass = 0.0f;
vcp->tangentMass = 0.0f;
vcp->velocityBias = 0.0f;
pc->localPoints[j] = cp->localPoint;
}
}
}
bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
const b2Transform& xf, int32 childIndex) const
{
B2_NOT_USED(childIndex);
// Put the ray into the polygon's frame of reference.
b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p);
b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p);
b2Vec2 d = p2 - p1;
float32 lower = 0.0f, upper = input.maxFraction;
int32 index = -1;
for (int32 i = 0; i < m_vertexCount; ++i)
{
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1);
float32 denominator = b2Dot(m_normals[i], d);
if (denominator == 0.0f)
{
if (numerator < 0.0f)
{
return false;
}
}
else
{
// Note: we want this predicate without division:
// lower < numerator / denominator, where denominator < 0
// Since denominator < 0, we have to flip the inequality:
// lower < numerator / denominator <==> denominator * lower > numerator.
if (denominator < 0.0f && numerator < lower * denominator)
{
// Increase lower.
// The segment enters this half-space.
lower = numerator / denominator;
index = i;
}
else if (denominator > 0.0f && numerator < upper * denominator)
{
// Decrease upper.
// The segment exits this half-space.
upper = numerator / denominator;
}
}
// The use of epsilon here causes the assert on lower to trip
// in some cases. Apparently the use of epsilon was to make edge
// shapes work, but now those are handled separately.
//if (upper < lower - b2_epsilon)
if (upper < lower)
{
return false;
}
}
b2Assert(0.0f <= lower && lower <= input.maxFraction);
if (index >= 0)
{
output->fraction = lower;
output->normal = b2Mul(xf.q, m_normals[index]);
return true;
}
return false;
}
void b2Body::ResetMassData()
{
// Compute mass data from shapes. Each shape has its own density.
m_mass = 0.0f;
m_invMass = 0.0f;
m_I = 0.0f;
m_invI = 0.0f;
m_sweep.localCenter.SetZero();
// Static and kinematic bodies have zero mass.
if (m_type == b2_staticBody || m_type == b2_kinematicBody)
{
m_sweep.c0 = m_sweep.c = m_xf.position;
return;
}
b2Assert(m_type == b2_dynamicBody);
// Accumulate mass over all fixtures.
b2Vec2 center = b2Vec2_zero;
for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
{
if (f->m_density == 0.0f)
{
continue;
}
b2MassData massData;
f->GetMassData(&massData);
m_mass += massData.mass;
center += massData.mass * massData.center;
m_I += massData.I;
}
// Compute center of mass.
if (m_mass > 0.0f)
{
m_invMass = 1.0f / m_mass;
center *= m_invMass;
}
else
{
// Force all dynamic bodies to have a positive mass.
m_mass = 1.0f;
m_invMass = 1.0f;
}
if (m_I > 0.0f && (m_flags & e_fixedRotationFlag) == 0)
{
// Center the inertia about the center of mass.
m_I -= m_mass * b2Dot(center, center);
b2Assert(m_I > 0.0f);
m_invI = 1.0f / m_I;
}
else
{
m_I = 0.0f;
m_invI = 0.0f;
}
// Move center of mass.
b2Vec2 oldCenter = m_sweep.c;
m_sweep.localCenter = center;
m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
// Update center of mass velocity.
m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter);
}
// Initialize position dependent portions of the velocity constraints.
void b2ContactSolver::InitializeVelocityConstraints()
{
for (int32 i = 0; i < m_count; ++i)
{
b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
b2ContactPositionConstraint* pc = m_positionConstraints + i;
float32 radiusA = pc->radiusA;
float32 radiusB = pc->radiusB;
b2Manifold* manifold = m_contacts[vc->contactIndex]->GetManifold();
int32 indexA = vc->indexA;
int32 indexB = vc->indexB;
float32 mA = vc->invMassA;
float32 mB = vc->invMassB;
float32 iA = vc->invIA;
float32 iB = vc->invIB;
b2Vec2 localCenterA = pc->localCenterA;
b2Vec2 localCenterB = pc->localCenterB;
b2Vec2 cA = m_positions[indexA].c;
float32 aA = m_positions[indexA].a;
b2Vec2 vA = m_velocities[indexA].v;
float32 wA = m_velocities[indexA].w;
b2Vec2 cB = m_positions[indexB].c;
float32 aB = m_positions[indexB].a;
b2Vec2 vB = m_velocities[indexB].v;
float32 wB = m_velocities[indexB].w;
b2Assert(manifold->pointCount > 0);
b2Transform xfA, xfB;
xfA.q.Set(aA);
xfB.q.Set(aB);
xfA.p = cA - b2Mul(xfA.q, localCenterA);
xfB.p = cB - b2Mul(xfB.q, localCenterB);
b2WorldManifold worldManifold;
worldManifold.Initialize(manifold, xfA, radiusA, xfB, radiusB);
vc->normal = worldManifold.normal;
int32 pointCount = vc->pointCount;
for (int32 j = 0; j < pointCount; ++j)
{
b2VelocityConstraintPoint* vcp = vc->points + j;
vcp->rA = worldManifold.points[j] - cA;
vcp->rB = worldManifold.points[j] - cB;
float32 rnA = b2Cross(vcp->rA, vc->normal);
float32 rnB = b2Cross(vcp->rB, vc->normal);
float32 kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
vcp->normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;
b2Vec2 tangent = b2Cross(vc->normal, 1.0f);
float32 rtA = b2Cross(vcp->rA, tangent);
float32 rtB = b2Cross(vcp->rB, tangent);
float32 kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;
vcp->tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f;
// Setup a velocity bias for restitution.
vcp->velocityBias = 0.0f;
float32 vRel = b2Dot(vc->normal, vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA));
if (vRel < -b2_velocityThreshold)
{
vcp->velocityBias = -vc->restitution * vRel;
}
}
// If we have two points, then prepare the block solver.
if (vc->pointCount == 2 && g_blockSolve)
{
b2VelocityConstraintPoint* vcp1 = vc->points + 0;
b2VelocityConstraintPoint* vcp2 = vc->points + 1;
float32 rn1A = b2Cross(vcp1->rA, vc->normal);
float32 rn1B = b2Cross(vcp1->rB, vc->normal);
float32 rn2A = b2Cross(vcp2->rA, vc->normal);
float32 rn2B = b2Cross(vcp2->rB, vc->normal);
float32 k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
float32 k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
float32 k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B;
// Ensure a reasonable condition number.
const float32 k_maxConditionNumber = 1000.0f;
if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
{
// K is safe to invert.
vc->K.ex.Set(k11, k12);
vc->K.ey.Set(k12, k22);
vc->normalMass = vc->K.GetInverse();
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
vc->pointCount = 1;
}
}
}
}
// Compute contact points for edge versus circle.
// This accounts for edge connectivity.
void b2CollideEdgeAndCircle(b2Manifold* manifold,
const b2EdgeShape* edgeA, const b2Transform& xfA,
const b2CircleShape* circleB, const b2Transform& xfB)
{
manifold->pointCount = 0;
// Compute circle in frame of edge
b2Vec2 Q = b2MulT(xfA, b2Mul(xfB, circleB->m_p));
b2Vec2 A = edgeA->m_vertex1, B = edgeA->m_vertex2;
b2Vec2 e = B - A;
// Barycentric coordinates
float32 u = b2Dot(e, B - Q);
float32 v = b2Dot(e, Q - A);
float32 radius = edgeA->m_radius + circleB->m_radius;
b2ContactFeature cf;
cf.indexB = 0;
cf.typeB = b2ContactFeature::e_vertex;
// Region A
if (v <= 0.0f)
{
b2Vec2 P = A;
b2Vec2 d = Q - P;
float32 dd = b2Dot(d, d);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to A?
if (edgeA->m_hasVertex0)
{
b2Vec2 A1 = edgeA->m_vertex0;
b2Vec2 B1 = A;
b2Vec2 e1 = B1 - A1;
float32 u1 = b2Dot(e1, B1 - Q);
// Is the circle in Region AB of the previous edge?
if (u1 > 0.0f)
{
return;
}
}
cf.indexA = 0;
cf.typeA = b2ContactFeature::e_vertex;
manifold->pointCount = 1;
manifold->type = b2Manifold::e_circles;
manifold->localNormal.SetZero();
manifold->localPoint = P;
manifold->points[0].id.key = 0;
manifold->points[0].id.cf = cf;
manifold->points[0].localPoint = circleB->m_p;
return;
}
// Region B
if (u <= 0.0f)
{
b2Vec2 P = B;
b2Vec2 d = Q - P;
float32 dd = b2Dot(d, d);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to B?
if (edgeA->m_hasVertex3)
{
b2Vec2 B2 = edgeA->m_vertex3;
b2Vec2 A2 = B;
b2Vec2 e2 = B2 - A2;
float32 v2 = b2Dot(e2, Q - A2);
// Is the circle in Region AB of the next edge?
if (v2 > 0.0f)
{
return;
}
}
cf.indexA = 1;
cf.typeA = b2ContactFeature::e_vertex;
manifold->pointCount = 1;
manifold->type = b2Manifold::e_circles;
manifold->localNormal.SetZero();
manifold->localPoint = P;
manifold->points[0].id.key = 0;
manifold->points[0].id.cf = cf;
manifold->points[0].localPoint = circleB->m_p;
return;
}
// Region AB
float32 den = b2Dot(e, e);
b2Assert(den > 0.0f);
b2Vec2 P = (1.0f / den) * (u * A + v * B);
b2Vec2 d = Q - P;
float32 dd = b2Dot(d, d);
if (dd > radius * radius)
{
return;
}
b2Vec2 n(-e.y, e.x);
if (b2Dot(n, Q - A) < 0.0f)
{
n.Set(-n.x, -n.y);
}
n.Normalize();
cf.indexA = 0;
cf.typeA = b2ContactFeature::e_face;
manifold->pointCount = 1;
manifold->type = b2Manifold::e_faceA;
manifold->localNormal = n;
manifold->localPoint = A;
manifold->points[0].id.key = 0;
manifold->points[0].id.cf = cf;
manifold->points[0].localPoint = circleB->m_p;
}
void b2PolygonShape::Set(const b2Vec2* vertices, int32 count)
{
if (count < 3)
{
SetAsBox(0.01f, 0.01f);
return;
}
count = b2Min(count, b2_maxPolygonVertices);
// Copy vertices into local buffer
b2Vec2 ps[b2_maxPolygonVertices];
for (int32 i = 0; i < count; ++i)
{
ps[i] = vertices[i];
}
// 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 < count; ++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 < count; ++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)
{
SetAsBox(0.01f, 0.01f);
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);
}
// TODO_ERIN adjust linear velocity and torque to account for movement of center.
void b2Body::SetMassFromShapes()
{
// Compute mass data from shapes. Each shape has its own density.
m_mass = 0.0f;
m_invMass = 0.0f;
m_I = 0.0f;
m_invI = 0.0f;
b2Vec2 center = b2Vec2_zero;
for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
{
b2MassData massData;
f->ComputeMass(&massData);
m_mass += massData.mass;
center += massData.mass * massData.center;
m_I += massData.I;
}
// Compute center of mass, and shift the origin to the COM.
if (m_mass > 0.0f)
{
m_invMass = 1.0f / m_mass;
center *= m_invMass;
}
if (m_I > 0.0f && (m_flags & e_fixedRotationFlag) == 0)
{
// Center the inertia about the center of mass.
m_I -= m_mass * b2Dot(center, center);
b2Assert(m_I > 0.0f);
m_invI = 1.0f / m_I;
}
else
{
m_I = 0.0f;
m_invI = 0.0f;
}
// Move center of mass.
m_sweep.localCenter = center;
m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
int16 oldType = m_type;
if (m_invMass == 0.0f && m_invI == 0.0f)
{
m_type = e_staticType;
}
else
{
m_type = e_dynamicType;
}
// If the body type changed, we need to refilter the broad-phase proxies.
if (oldType != m_type)
{
for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
{
f->RefilterProxy(m_world->m_broadPhase, m_xf);
}
}
}
b2Joint* b2Joint::Create(const b2JointDef* def, b2BlockAllocator* allocator)
{
b2Joint* joint = NULL;
switch (def->type)
{
case e_distanceJoint:
{
void* mem = allocator->Allocate(sizeof(b2DistanceJoint));
joint = new (mem) b2DistanceJoint((b2DistanceJointDef*)def);
}
break;
case e_mouseJoint:
{
void* mem = allocator->Allocate(sizeof(b2MouseJoint));
joint = new (mem) b2MouseJoint((b2MouseJointDef*)def);
}
break;
case e_prismaticJoint:
{
void* mem = allocator->Allocate(sizeof(b2PrismaticJoint));
joint = new (mem) b2PrismaticJoint((b2PrismaticJointDef*)def);
}
break;
case e_revoluteJoint:
{
void* mem = allocator->Allocate(sizeof(b2RevoluteJoint));
joint = new (mem) b2RevoluteJoint((b2RevoluteJointDef*)def);
}
break;
case e_pulleyJoint:
{
void* mem = allocator->Allocate(sizeof(b2PulleyJoint));
joint = new (mem) b2PulleyJoint((b2PulleyJointDef*)def);
}
break;
case e_gearJoint:
{
void* mem = allocator->Allocate(sizeof(b2GearJoint));
joint = new (mem) b2GearJoint((b2GearJointDef*)def);
}
break;
case e_wheelJoint:
{
void* mem = allocator->Allocate(sizeof(b2WheelJoint));
joint = new (mem) b2WheelJoint((b2WheelJointDef*)def);
}
break;
case e_weldJoint:
{
void* mem = allocator->Allocate(sizeof(b2WeldJoint));
joint = new (mem) b2WeldJoint((b2WeldJointDef*)def);
}
break;
case e_frictionJoint:
{
void* mem = allocator->Allocate(sizeof(b2FrictionJoint));
joint = new (mem) b2FrictionJoint((b2FrictionJointDef*)def);
}
break;
case e_ropeJoint:
{
void* mem = allocator->Allocate(sizeof(b2RopeJoint));
joint = new (mem) b2RopeJoint((b2RopeJointDef*)def);
}
break;
default:
b2Assert(false);
break;
}
return joint;
}
float32 Initialize(const b2SimplexCache* cache,
const b2DistanceProxy* proxyA, const b2Sweep& sweepA,
const b2DistanceProxy* proxyB, const b2Sweep& sweepB,
float32 t1)
{
m_proxyA = proxyA;
m_proxyB = proxyB;
int32 count = cache->count;
b2Assert(0 < count && count < 3);
m_sweepA = sweepA;
m_sweepB = sweepB;
b2Transform xfA, xfB;
m_sweepA.GetTransform(&xfA, t1);
m_sweepB.GetTransform(&xfB, t1);
if (count == 1)
{
m_type = e_points;
b2Vec2 localPointA = m_proxyA->GetVertex(cache->indexA[0]);
b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
b2Vec2 pointA = b2Mul(xfA, localPointA);
b2Vec2 pointB = b2Mul(xfB, localPointB);
m_axis = pointB - pointA;
float32 s = m_axis.Normalize();
return s;
}
else if (cache->indexA[0] == cache->indexA[1])
{
// Two points on B and one on A.
m_type = e_faceB;
b2Vec2 localPointB1 = proxyB->GetVertex(cache->indexB[0]);
b2Vec2 localPointB2 = proxyB->GetVertex(cache->indexB[1]);
m_axis = b2Cross(localPointB2 - localPointB1, 1.0f);
m_axis.Normalize();
b2Vec2 normal = b2Mul(xfB.R, m_axis);
m_localPoint = 0.5f * (localPointB1 + localPointB2);
b2Vec2 pointB = b2Mul(xfB, m_localPoint);
b2Vec2 localPointA = proxyA->GetVertex(cache->indexA[0]);
b2Vec2 pointA = b2Mul(xfA, localPointA);
float32 s = b2Dot(pointA - pointB, normal);
if (s < 0.0f)
{
m_axis = -m_axis;
s = -s;
}
return s;
}
else
{
// Two points on A and one or two points on B.
m_type = e_faceA;
b2Vec2 localPointA1 = m_proxyA->GetVertex(cache->indexA[0]);
b2Vec2 localPointA2 = m_proxyA->GetVertex(cache->indexA[1]);
m_axis = b2Cross(localPointA2 - localPointA1, 1.0f);
m_axis.Normalize();
b2Vec2 normal = b2Mul(xfA.R, m_axis);
m_localPoint = 0.5f * (localPointA1 + localPointA2);
b2Vec2 pointA = b2Mul(xfA, m_localPoint);
b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
b2Vec2 pointB = b2Mul(xfB, localPointB);
float32 s = b2Dot(pointB - pointA, normal);
if (s < 0.0f)
{
m_axis = -m_axis;
s = -s;
}
return s;
}
}
// CCD via the local separating axis method. This seeks progression
// by computing the largest time at which separation is maintained.
void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
{
++b2_toiCalls;
output->state = b2TOIOutput::e_unknown;
output->t = input->tMax;
const b2DistanceProxy* proxyA = &input->proxyA;
const b2DistanceProxy* proxyB = &input->proxyB;
b2Sweep sweepA = input->sweepA;
b2Sweep sweepB = input->sweepB;
// Large rotations can make the root finder fail, so we normalize the
// sweep angles.
sweepA.Normalize();
sweepB.Normalize();
float32 tMax = input->tMax;
float32 totalRadius = proxyA->m_radius + proxyB->m_radius;
float32 target = b2Max(b2_linearSlop, totalRadius - 3.0f * b2_linearSlop);
float32 tolerance = 0.25f * b2_linearSlop;
b2Assert(target > tolerance);
float32 t1 = 0.0f;
const int32 k_maxIterations = 20; // TODO_ERIN b2Settings
int32 iter = 0;
// Prepare input for distance query.
b2SimplexCache cache;
cache.count = 0;
b2DistanceInput distanceInput;
distanceInput.proxyA = input->proxyA;
distanceInput.proxyB = input->proxyB;
distanceInput.useRadii = false;
// The outer loop progressively attempts to compute new separating axes.
// This loop terminates when an axis is repeated (no progress is made).
for(;;)
{
b2Transform xfA, xfB;
sweepA.GetTransform(&xfA, t1);
sweepB.GetTransform(&xfB, t1);
// Get the distance between shapes. We can also use the results
// to get a separating axis.
distanceInput.transformA = xfA;
distanceInput.transformB = xfB;
b2DistanceOutput distanceOutput;
b2Distance(&distanceOutput, &cache, &distanceInput);
// If the shapes are overlapped, we give up on continuous collision.
if (distanceOutput.distance <= 0.0f)
{
// Failure!
output->state = b2TOIOutput::e_overlapped;
output->t = 0.0f;
break;
}
if (distanceOutput.distance < target + tolerance)
{
// Victory!
output->state = b2TOIOutput::e_touching;
output->t = t1;
break;
}
// Initialize the separating axis.
b2SeparationFunction fcn;
fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1);
#if 0
// Dump the curve seen by the root finder
{
const int32 N = 100;
float32 dx = 1.0f / N;
float32 xs[N+1];
float32 fs[N+1];
float32 x = 0.0f;
for (int32 i = 0; i <= N; ++i)
{
sweepA.GetTransform(&xfA, x);
sweepB.GetTransform(&xfB, x);
float32 f = fcn.Evaluate(xfA, xfB) - target;
printf("%g %g\n", x, f);
xs[i] = x;
fs[i] = f;
x += dx;
}
}
#endif
// Compute the TOI on the separating axis. We do this by successively
// resolving the deepest point. This loop is bounded by the number of vertices.
bool done = false;
float32 t2 = tMax;
int32 pushBackIter = 0;
for (;;)
{
// Find the deepest point at t2. Store the witness point indices.
int32 indexA, indexB;
float32 s2 = fcn.FindMinSeparation(&indexA, &indexB, t2);
// Is the final configuration separated?
if (s2 > target + tolerance)
{
// Victory!
output->state = b2TOIOutput::e_separated;
output->t = tMax;
done = true;
break;
}
// Has the separation reached tolerance?
if (s2 > target - tolerance)
{
// Advance the sweeps
t1 = t2;
break;
}
// Compute the initial separation of the witness points.
float32 s1 = fcn.Evaluate(indexA, indexB, t1);
// Check for initial overlap. This might happen if the root finder
// runs out of iterations.
if (s1 < target - tolerance)
{
output->state = b2TOIOutput::e_failed;
output->t = t1;
done = true;
break;
}
// Check for touching
if (s1 <= target + tolerance)
{
// Victory! t1 should hold the TOI (could be 0.0).
output->state = b2TOIOutput::e_touching;
output->t = t1;
done = true;
break;
}
// Compute 1D root of: f(x) - target = 0
int32 rootIterCount = 0;
float32 a1 = t1, a2 = t2;
for (;;)
{
// Use a mix of the secant rule and bisection.
float32 t;
if (rootIterCount & 1)
{
// Secant rule to improve convergence.
t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
}
else
{
// Bisection to guarantee progress.
t = 0.5f * (a1 + a2);
}
float32 s = fcn.Evaluate(indexA, indexB, t);
if (b2Abs(s - target) < tolerance)
{
// t2 holds a tentative value for t1
t2 = t;
break;
}
// Ensure we continue to bracket the root.
if (s > target)
{
a1 = t;
s1 = s;
}
else
{
a2 = t;
s2 = s;
}
++rootIterCount;
++b2_toiRootIters;
if (rootIterCount == 50)
{
break;
}
}
b2_toiMaxRootIters = b2Max(b2_toiMaxRootIters, rootIterCount);
++pushBackIter;
if (pushBackIter == b2_maxPolygonVertices)
{
break;
}
}
++iter;
++b2_toiIters;
if (done)
{
break;
}
if (iter == k_maxIterations)
{
// Root finder got stuck. Semi-victory.
output->state = b2TOIOutput::e_failed;
output->t = t1;
break;
}
}
b2_toiMaxIters = b2Max(b2_toiMaxIters, iter);
}
float32 FindMinSeparation(int32* indexA, int32* indexB, float32 t) const
{
b2Transform xfA, xfB;
m_sweepA.GetTransform(&xfA, t);
m_sweepB.GetTransform(&xfB, t);
switch (m_type)
{
case e_points:
{
b2Vec2 axisA = b2MulT(xfA.R, m_axis);
b2Vec2 axisB = b2MulT(xfB.R, -m_axis);
*indexA = m_proxyA->GetSupport(axisA);
*indexB = m_proxyB->GetSupport(axisB);
b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
b2Vec2 pointA = b2Mul(xfA, localPointA);
b2Vec2 pointB = b2Mul(xfB, localPointB);
float32 separation = b2Dot(pointB - pointA, m_axis);
return separation;
}
case e_faceA:
{
b2Vec2 normal = b2Mul(xfA.R, m_axis);
b2Vec2 pointA = b2Mul(xfA, m_localPoint);
b2Vec2 axisB = b2MulT(xfB.R, -normal);
*indexA = -1;
*indexB = m_proxyB->GetSupport(axisB);
b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
b2Vec2 pointB = b2Mul(xfB, localPointB);
float32 separation = b2Dot(pointB - pointA, normal);
return separation;
}
case e_faceB:
{
b2Vec2 normal = b2Mul(xfB.R, m_axis);
b2Vec2 pointB = b2Mul(xfB, m_localPoint);
b2Vec2 axisA = b2MulT(xfA.R, -normal);
*indexB = -1;
*indexA = m_proxyA->GetSupport(axisA);
b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
b2Vec2 pointA = b2Mul(xfA, localPointA);
float32 separation = b2Dot(pointA - pointB, normal);
return separation;
}
default:
b2Assert(false);
*indexA = -1;
*indexB = -1;
return 0.0f;
}
}
void b2Body::DestroyFixture(b2Fixture* fixture)
{
b2Assert(m_world->IsLocked() == false);
if (m_world->IsLocked() == true)
{
return;
}
b2Assert(fixture->m_body == this);
// Remove the fixture from this body's singly linked list.
b2Assert(m_fixtureCount > 0);
b2Fixture** node = &m_fixtureList;
bool found = false;
while (*node != NULL)
{
if (*node == fixture)
{
*node = fixture->m_next;
found = true;
break;
}
node = &(*node)->m_next;
}
// You tried to remove a shape that is not attached to this body.
b2Assert(found);
// Destroy any contacts associated with the fixture.
b2ContactEdge* edge = m_contactList;
while (edge)
{
b2Contact* c = edge->contact;
edge = edge->next;
b2Fixture* fixtureA = c->GetFixtureA();
b2Fixture* fixtureB = c->GetFixtureB();
if (fixture == fixtureA || fixture == fixtureB)
{
// This destroys the contact and removes it from
// this body's contact list.
m_world->m_contactManager.Destroy(c);
}
}
b2BlockAllocator* allocator = &m_world->m_blockAllocator;
if (m_flags & e_activeFlag)
{
b2Assert(fixture->m_proxyId != b2BroadPhase::e_nullProxy);
b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
fixture->DestroyProxy(broadPhase);
}
else
{
b2Assert(fixture->m_proxyId == b2BroadPhase::e_nullProxy);
}
fixture->Destroy(allocator);
fixture->m_body = NULL;
fixture->m_next = NULL;
fixture->~b2Fixture();
allocator->Free(fixture, sizeof(b2Fixture));
--m_fixtureCount;
// Reset the mass data.
ResetMassData();
}
void b2ContactSolver::SolveVelocityConstraints()
{
for (int32 i = 0; i < m_constraintCount; ++i)
{
b2ContactConstraint* c = m_constraints + i;
b2Body* bodyA = c->bodyA;
b2Body* bodyB = c->bodyB;
float32 wA = bodyA->m_angularVelocity;
float32 wB = bodyB->m_angularVelocity;
b2Vec2 vA = bodyA->m_linearVelocity;
b2Vec2 vB = bodyB->m_linearVelocity;
float32 invMassA = bodyA->m_invMass;
float32 invIA = bodyA->m_invI;
float32 invMassB = bodyB->m_invMass;
float32 invIB = bodyB->m_invI;
b2Vec2 normal = c->normal;
b2Vec2 tangent = b2Cross(normal, 1.0f);
float32 friction = c->friction;
b2Assert(c->pointCount == 1 || c->pointCount == 2);
// Solve tangent constraints
for (int32 j = 0; j < c->pointCount; ++j)
{
b2ContactConstraintPoint* ccp = c->points + j;
// Relative velocity at contact
b2Vec2 dv = vB + b2Cross(wB, ccp->rB) - vA - b2Cross(wA, ccp->rA);
// Compute tangent force
float32 vt = b2Dot(dv, tangent);
float32 lambda = ccp->tangentMass * (-vt);
// b2Clamp the accumulated force
float32 maxFriction = friction * ccp->normalImpulse;
float32 newImpulse = b2Clamp(ccp->tangentImpulse + lambda, -maxFriction, maxFriction);
lambda = newImpulse - ccp->tangentImpulse;
// Apply contact impulse
b2Vec2 P = lambda * tangent;
vA -= invMassA * P;
wA -= invIA * b2Cross(ccp->rA, P);
vB += invMassB * P;
wB += invIB * b2Cross(ccp->rB, P);
ccp->tangentImpulse = newImpulse;
}
// Solve normal constraints
if (c->pointCount == 1)
{
b2ContactConstraintPoint* ccp = c->points + 0;
// Relative velocity at contact
b2Vec2 dv = vB + b2Cross(wB, ccp->rB) - vA - b2Cross(wA, ccp->rA);
// Compute normal impulse
float32 vn = b2Dot(dv, normal);
float32 lambda = -ccp->normalMass * (vn - ccp->velocityBias);
// b2Clamp the accumulated impulse
float32 newImpulse = b2Max(ccp->normalImpulse + lambda, 0.0f);
lambda = newImpulse - ccp->normalImpulse;
// Apply contact impulse
b2Vec2 P = lambda * normal;
vA -= invMassA * P;
wA -= invIA * b2Cross(ccp->rA, P);
vB += invMassB * P;
wB += invIB * b2Cross(ccp->rB, P);
ccp->normalImpulse = newImpulse;
}
else
{
// Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
// Build the mini LCP for this contact patch
//
// vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
//
// A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
// b = vn_0 - velocityBias
//
// The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
// implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
// vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid
// solution that satisfies the problem is chosen.
//
// In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
// that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
//
// Substitute:
//
// x = x' - a
//
// Plug into above equation:
//
// vn = A * x + b
// = A * (x' - a) + b
// = A * x' + b - A * a
// = A * x' + b'
// b' = b - A * a;
b2ContactConstraintPoint* cp1 = c->points + 0;
b2ContactConstraintPoint* cp2 = c->points + 1;
b2Vec2 a(cp1->normalImpulse, cp2->normalImpulse);
b2Assert(a.x >= 0.0f && a.y >= 0.0f);
// Relative velocity at contact
b2Vec2 dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
b2Vec2 dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
// Compute normal velocity
float32 vn1 = b2Dot(dv1, normal);
float32 vn2 = b2Dot(dv2, normal);
b2Vec2 b;
b.x = vn1 - cp1->velocityBias;
b.y = vn2 - cp2->velocityBias;
b -= b2Mul(c->K, a);
const float32 k_errorTol = 1e-3f;
B2_NOT_USED(k_errorTol);
for (;;)
{
//
// Case 1: vn = 0
//
// 0 = A * x' + b'
//
// Solve for x':
//
// x' = - inv(A) * b'
//
b2Vec2 x = - b2Mul(c->normalMass, b);
if (x.x >= 0.0f && x.y >= 0.0f)
{
// Resubstitute for the incremental impulse
b2Vec2 d = x - a;
// Apply incremental impulse
b2Vec2 P1 = d.x * normal;
b2Vec2 P2 = d.y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
vB += invMassB * (P1 + P2);
wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
cp2->normalImpulse = x.y;
#if B2_DEBUG_SOLVER == 1
// Postconditions
dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
// Compute normal velocity
vn1 = b2Dot(dv1, normal);
vn2 = b2Dot(dv2, normal);
b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol);
b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 2: vn1 = 0 and x2 = 0
//
// 0 = a11 * x1' + a12 * 0 + b1'
// vn2 = a21 * x1' + a22 * 0 + b2'
//
x.x = - cp1->normalMass * b.x;
x.y = 0.0f;
vn1 = 0.0f;
vn2 = c->K.col1.y * x.x + b.y;
if (x.x >= 0.0f && vn2 >= 0.0f)
{
// Resubstitute for the incremental impulse
b2Vec2 d = x - a;
// Apply incremental impulse
b2Vec2 P1 = d.x * normal;
b2Vec2 P2 = d.y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
vB += invMassB * (P1 + P2);
wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
cp2->normalImpulse = x.y;
#if B2_DEBUG_SOLVER == 1
// Postconditions
dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
// Compute normal velocity
vn1 = b2Dot(dv1, normal);
b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 3: wB = 0 and x1 = 0
//
// vn1 = a11 * 0 + a12 * x2' + b1'
// 0 = a21 * 0 + a22 * x2' + b2'
//
x.x = 0.0f;
x.y = - cp2->normalMass * b.y;
vn1 = c->K.col2.x * x.y + b.x;
vn2 = 0.0f;
if (x.y >= 0.0f && vn1 >= 0.0f)
{
// Resubstitute for the incremental impulse
b2Vec2 d = x - a;
// Apply incremental impulse
b2Vec2 P1 = d.x * normal;
b2Vec2 P2 = d.y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
vB += invMassB * (P1 + P2);
wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
cp2->normalImpulse = x.y;
#if B2_DEBUG_SOLVER == 1
// Postconditions
dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
// Compute normal velocity
vn2 = b2Dot(dv2, normal);
b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 4: x1 = 0 and x2 = 0
//
// vn1 = b1
// vn2 = b2;
x.x = 0.0f;
x.y = 0.0f;
vn1 = b.x;
vn2 = b.y;
if (vn1 >= 0.0f && vn2 >= 0.0f )
{
// Resubstitute for the incremental impulse
b2Vec2 d = x - a;
// Apply incremental impulse
b2Vec2 P1 = d.x * normal;
b2Vec2 P2 = d.y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
vB += invMassB * (P1 + P2);
wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
cp2->normalImpulse = x.y;
break;
}
// No solution, give up. This is hit sometimes, but it doesn't seem to matter.
break;
}
}
bodyA->m_linearVelocity = vA;
bodyA->m_angularVelocity = wA;
bodyB->m_linearVelocity = vB;
bodyB->m_angularVelocity = wB;
}
}
b2Body::b2Body(const b2BodyDef* bd, b2World* world)
{
b2Assert(bd->position.IsValid());
b2Assert(bd->linearVelocity.IsValid());
b2Assert(b2IsValid(bd->angle));
b2Assert(b2IsValid(bd->angularVelocity));
b2Assert(b2IsValid(bd->inertiaScale) && bd->inertiaScale >= 0.0f);
b2Assert(b2IsValid(bd->angularDamping) && bd->angularDamping >= 0.0f);
b2Assert(b2IsValid(bd->linearDamping) && bd->linearDamping >= 0.0f);
m_flags = 0;
if (bd->bullet)
{
m_flags |= e_bulletFlag;
}
if (bd->fixedRotation)
{
m_flags |= e_fixedRotationFlag;
}
if (bd->allowSleep)
{
m_flags |= e_autoSleepFlag;
}
if (bd->awake)
{
m_flags |= e_awakeFlag;
}
if (bd->active)
{
m_flags |= e_activeFlag;
}
m_world = world;
m_xf.position = bd->position;
m_xf.R.Set(bd->angle);
m_sweep.localCenter.SetZero();
m_sweep.a0 = m_sweep.a = bd->angle;
m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
m_jointList = NULL;
m_contactList = NULL;
m_prev = NULL;
m_next = NULL;
m_linearVelocity = bd->linearVelocity;
m_angularVelocity = bd->angularVelocity;
m_linearDamping = bd->linearDamping;
m_angularDamping = bd->angularDamping;
m_force.SetZero();
m_torque = 0.0f;
m_sleepTime = 0.0f;
m_type = bd->type;
if (m_type == b2_dynamicBody)
{
m_mass = 1.0f;
m_invMass = 1.0f;
}
else
{
m_mass = 0.0f;
m_invMass = 0.0f;
}
m_I = 0.0f;
m_invI = 0.0f;
m_userData = bd->userData;
m_fixtureList = NULL;
m_fixtureCount = 0;
}
b2ContactSolver::b2ContactSolver(const b2TimeStep& step, b2Contact** contacts, int32 contactCount, b2StackAllocator* allocator)
{
m_step = step;
m_allocator = allocator;
m_constraintCount = contactCount;
m_constraints = (b2ContactConstraint*)m_allocator->Allocate(m_constraintCount * sizeof(b2ContactConstraint));
for (int32 i = 0; i < m_constraintCount; ++i)
{
b2Contact* contact = contacts[i];
b2Fixture* fixtureA = contact->m_fixtureA;
b2Fixture* fixtureB = contact->m_fixtureB;
b2Shape* shapeA = fixtureA->GetShape();
b2Shape* shapeB = fixtureB->GetShape();
float32 radiusA = shapeA->m_radius;
float32 radiusB = shapeB->m_radius;
b2Body* bodyA = fixtureA->GetBody();
b2Body* bodyB = fixtureB->GetBody();
b2Manifold* manifold = contact->GetManifold();
float32 friction = b2MixFriction(fixtureA->GetFriction(), fixtureB->GetFriction());
float32 restitution = b2MixRestitution(fixtureA->GetRestitution(), fixtureB->GetRestitution());
b2Vec2 vA = bodyA->m_linearVelocity;
b2Vec2 vB = bodyB->m_linearVelocity;
float32 wA = bodyA->m_angularVelocity;
float32 wB = bodyB->m_angularVelocity;
b2Assert(manifold->m_pointCount > 0);
b2WorldManifold worldManifold;
worldManifold.Initialize(manifold, bodyA->m_xf, radiusA, bodyB->m_xf, radiusB);
b2ContactConstraint* cc = m_constraints + i;
cc->bodyA = bodyA;
cc->bodyB = bodyB;
cc->manifold = manifold;
cc->normal = worldManifold.m_normal;
cc->pointCount = manifold->m_pointCount;
cc->friction = friction;
cc->restitution = restitution;
cc->localPlaneNormal = manifold->m_localPlaneNormal;
cc->localPoint = manifold->m_localPoint;
cc->radius = radiusA + radiusB;
cc->type = manifold->m_type;
for (int32 j = 0; j < cc->pointCount; ++j)
{
b2ManifoldPoint* cp = manifold->m_points + j;
b2ContactConstraintPoint* ccp = cc->points + j;
ccp->normalImpulse = cp->m_normalImpulse;
ccp->tangentImpulse = cp->m_tangentImpulse;
ccp->localPoint = cp->m_localPoint;
ccp->rA = worldManifold.m_points[j] - bodyA->m_sweep.c;
ccp->rB = worldManifold.m_points[j] - bodyB->m_sweep.c;
float32 rnA = b2Cross(ccp->rA, cc->normal);
float32 rnB = b2Cross(ccp->rB, cc->normal);
rnA *= rnA;
rnB *= rnB;
float32 kNormal = bodyA->m_invMass + bodyB->m_invMass + bodyA->m_invI * rnA + bodyB->m_invI * rnB;
b2Assert(kNormal > B2_FLT_EPSILON);
ccp->normalMass = 1.0f / kNormal;
float32 kEqualized = bodyA->m_mass * bodyA->m_invMass + bodyB->m_mass * bodyB->m_invMass;
kEqualized += bodyA->m_mass * bodyA->m_invI * rnA + bodyB->m_mass * bodyB->m_invI * rnB;
b2Assert(kEqualized > B2_FLT_EPSILON);
ccp->equalizedMass = 1.0f / kEqualized;
b2Vec2 tangent = b2Cross(cc->normal, 1.0f);
float32 rtA = b2Cross(ccp->rA, tangent);
float32 rtB = b2Cross(ccp->rB, tangent);
rtA *= rtA;
rtB *= rtB;
float32 kTangent = bodyA->m_invMass + bodyB->m_invMass + bodyA->m_invI * rtA + bodyB->m_invI * rtB;
b2Assert(kTangent > B2_FLT_EPSILON);
ccp->tangentMass = 1.0f / kTangent;
// Setup a velocity bias for restitution.
ccp->velocityBias = 0.0f;
float32 vRel = b2Dot(cc->normal, vB + b2Cross(wB, ccp->rB) - vA - b2Cross(wA, ccp->rA));
if (vRel < -b2_velocityThreshold)
{
ccp->velocityBias = -cc->restitution * vRel;
}
}
// If we have two points, then prepare the block solver.
if (cc->pointCount == 2)
{
b2ContactConstraintPoint* ccp1 = cc->points + 0;
b2ContactConstraintPoint* ccp2 = cc->points + 1;
float32 invMassA = bodyA->m_invMass;
float32 invIA = bodyA->m_invI;
float32 invMassB = bodyB->m_invMass;
float32 invIB = bodyB->m_invI;
float32 rn1A = b2Cross(ccp1->rA, cc->normal);
float32 rn1B = b2Cross(ccp1->rB, cc->normal);
float32 rn2A = b2Cross(ccp2->rA, cc->normal);
float32 rn2B = b2Cross(ccp2->rB, cc->normal);
float32 k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B;
float32 k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B;
float32 k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B;
// Ensure a reasonable condition number.
const float32 k_maxConditionNumber = 100.0f;
if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
{
// K is safe to invert.
cc->K.col1.Set(k11, k12);
cc->K.col2.Set(k12, k22);
cc->normalMass = cc->K.GetInverse();
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
cc->pointCount = 1;
}
}
}
}
b2Fixture::~b2Fixture()
{
b2Assert(m_shape == NULL);
b2Assert(m_proxyId == b2BroadPhase::e_nullProxy);
}
void b2GearJoint::SetRatio(float32 ratio)
{
b2Assert(b2IsValid(ratio));
m_ratio = ratio;
}
void b2ContactSolver::SolveVelocityConstraints()
{
for (int32 i = 0; i < m_count; ++i)
{
b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
int32 indexA = vc->indexA;
int32 indexB = vc->indexB;
float32 mA = vc->invMassA;
float32 iA = vc->invIA;
float32 mB = vc->invMassB;
float32 iB = vc->invIB;
int32 pointCount = vc->pointCount;
b2Vec2 vA = m_velocities[indexA].v;
float32 wA = m_velocities[indexA].w;
b2Vec2 vB = m_velocities[indexB].v;
float32 wB = m_velocities[indexB].w;
b2Vec2 normal = vc->normal;
b2Vec2 tangent = b2Cross(normal, 1.0f);
float32 friction = vc->friction;
b2Assert(pointCount == 1 || pointCount == 2);
// Solve tangent constraints first because non-penetration is more important
// than friction.
for (int32 j = 0; j < pointCount; ++j)
{
b2VelocityConstraintPoint* vcp = vc->points + j;
// Relative velocity at contact
b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA);
// Compute tangent force
float32 vt = b2Dot(dv, tangent) - vc->tangentSpeed;
float32 lambda = vcp->tangentMass * (-vt);
// b2Clamp the accumulated force
float32 maxFriction = friction * vcp->normalImpulse;
float32 newImpulse = b2Clamp(vcp->tangentImpulse + lambda, -maxFriction, maxFriction);
lambda = newImpulse - vcp->tangentImpulse;
vcp->tangentImpulse = newImpulse;
// Apply contact impulse
b2Vec2 P = lambda * tangent;
vA -= mA * P;
wA -= iA * b2Cross(vcp->rA, P);
vB += mB * P;
wB += iB * b2Cross(vcp->rB, P);
}
// Solve normal constraints
if (pointCount == 1 || g_blockSolve == false)
{
for (int32 j = 0; j < pointCount; ++j)
{
b2VelocityConstraintPoint* vcp = vc->points + j;
// Relative velocity at contact
b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA);
// Compute normal impulse
float32 vn = b2Dot(dv, normal);
float32 lambda = -vcp->normalMass * (vn - vcp->velocityBias);
// b2Clamp the accumulated impulse
float32 newImpulse = b2Max(vcp->normalImpulse + lambda, 0.0f);
lambda = newImpulse - vcp->normalImpulse;
vcp->normalImpulse = newImpulse;
// Apply contact impulse
b2Vec2 P = lambda * normal;
vA -= mA * P;
wA -= iA * b2Cross(vcp->rA, P);
vB += mB * P;
wB += iB * b2Cross(vcp->rB, P);
}
}
else
{
// Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
// Build the mini LCP for this contact patch
//
// vn = A * x + b, vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
//
// A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
// b = vn0 - velocityBias
//
// The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
// implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
// vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid
// solution that satisfies the problem is chosen.
//
// In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
// that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
//
// Substitute:
//
// x = a + d
//
// a := old total impulse
// x := new total impulse
// d := incremental impulse
//
// For the current iteration we extend the formula for the incremental impulse
// to compute the new total impulse:
//
// vn = A * d + b
// = A * (x - a) + b
// = A * x + b - A * a
// = A * x + b'
// b' = b - A * a;
b2VelocityConstraintPoint* cp1 = vc->points + 0;
b2VelocityConstraintPoint* cp2 = vc->points + 1;
b2Vec2 a(cp1->normalImpulse, cp2->normalImpulse);
b2Assert(a.x >= 0.0f && a.y >= 0.0f);
// Relative velocity at contact
b2Vec2 dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
b2Vec2 dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
// Compute normal velocity
float32 vn1 = b2Dot(dv1, normal);
float32 vn2 = b2Dot(dv2, normal);
b2Vec2 b;
b.x = vn1 - cp1->velocityBias;
b.y = vn2 - cp2->velocityBias;
// Compute b'
b -= b2Mul(vc->K, a);
const float32 k_errorTol = 1e-3f;
B2_NOT_USED(k_errorTol);
for (;;)
{
//
// Case 1: vn = 0
//
// 0 = A * x + b'
//
// Solve for x:
//
// x = - inv(A) * b'
//
b2Vec2 x = - b2Mul(vc->normalMass, b);
if (x.x >= 0.0f && x.y >= 0.0f)
{
// Get the incremental impulse
b2Vec2 d = x - a;
// Apply incremental impulse
b2Vec2 P1 = d.x * normal;
b2Vec2 P2 = d.y * normal;
vA -= mA * (P1 + P2);
wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
vB += mB * (P1 + P2);
wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
cp2->normalImpulse = x.y;
#if B2_DEBUG_SOLVER == 1
// Postconditions
dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
// Compute normal velocity
vn1 = b2Dot(dv1, normal);
vn2 = b2Dot(dv2, normal);
b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol);
b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 2: vn1 = 0 and x2 = 0
//
// 0 = a11 * x1 + a12 * 0 + b1'
// vn2 = a21 * x1 + a22 * 0 + b2'
//
x.x = - cp1->normalMass * b.x;
x.y = 0.0f;
vn1 = 0.0f;
vn2 = vc->K.ex.y * x.x + b.y;
if (x.x >= 0.0f && vn2 >= 0.0f)
{
// Get the incremental impulse
b2Vec2 d = x - a;
// Apply incremental impulse
b2Vec2 P1 = d.x * normal;
b2Vec2 P2 = d.y * normal;
vA -= mA * (P1 + P2);
wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
vB += mB * (P1 + P2);
wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
cp2->normalImpulse = x.y;
#if B2_DEBUG_SOLVER == 1
// Postconditions
dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
// Compute normal velocity
vn1 = b2Dot(dv1, normal);
b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 3: vn2 = 0 and x1 = 0
//
// vn1 = a11 * 0 + a12 * x2 + b1'
// 0 = a21 * 0 + a22 * x2 + b2'
//
x.x = 0.0f;
x.y = - cp2->normalMass * b.y;
vn1 = vc->K.ey.x * x.y + b.x;
vn2 = 0.0f;
if (x.y >= 0.0f && vn1 >= 0.0f)
{
// Resubstitute for the incremental impulse
b2Vec2 d = x - a;
// Apply incremental impulse
b2Vec2 P1 = d.x * normal;
b2Vec2 P2 = d.y * normal;
vA -= mA * (P1 + P2);
wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
vB += mB * (P1 + P2);
wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
cp2->normalImpulse = x.y;
#if B2_DEBUG_SOLVER == 1
// Postconditions
dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
// Compute normal velocity
vn2 = b2Dot(dv2, normal);
b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 4: x1 = 0 and x2 = 0
//
// vn1 = b1
// vn2 = b2;
x.x = 0.0f;
x.y = 0.0f;
vn1 = b.x;
vn2 = b.y;
if (vn1 >= 0.0f && vn2 >= 0.0f )
{
// Resubstitute for the incremental impulse
b2Vec2 d = x - a;
// Apply incremental impulse
b2Vec2 P1 = d.x * normal;
b2Vec2 P2 = d.y * normal;
vA -= mA * (P1 + P2);
wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
vB += mB * (P1 + P2);
wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
cp2->normalImpulse = x.y;
break;
}
// No solution, give up. This is hit sometimes, but it doesn't seem to matter.
break;
}
}
m_velocities[indexA].v = vA;
m_velocities[indexA].w = wA;
m_velocities[indexB].v = vB;
m_velocities[indexB].w = wB;
}
}
b2GearJoint::b2GearJoint(const b2GearJointDef* def)
: b2Joint(def)
{
m_joint1 = def->joint1;
m_joint2 = def->joint2;
m_typeA = m_joint1->GetType();
m_typeB = m_joint2->GetType();
b2Assert(m_typeA == e_revoluteJoint || m_typeA == e_prismaticJoint);
b2Assert(m_typeB == e_revoluteJoint || m_typeB == e_prismaticJoint);
float32 coordinateA, coordinateB;
// TODO_ERIN there might be some problem with the joint edges in b2Joint.
m_bodyC = m_joint1->GetBodyA();
m_bodyA = m_joint1->GetBodyB();
// Get geometry of joint1
b2Transform xfA = m_bodyA->m_xf;
float32 aA = m_bodyA->m_sweep.a;
b2Transform xfC = m_bodyC->m_xf;
float32 aC = m_bodyC->m_sweep.a;
if (m_typeA == e_revoluteJoint)
{
b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint1;
m_localAnchorC = revolute->m_localAnchorA;
m_localAnchorA = revolute->m_localAnchorB;
m_referenceAngleA = revolute->m_referenceAngle;
m_localAxisC.SetZero();
coordinateA = aA - aC - m_referenceAngleA;
}
else
{
b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint1;
m_localAnchorC = prismatic->m_localAnchorA;
m_localAnchorA = prismatic->m_localAnchorB;
m_referenceAngleA = prismatic->m_referenceAngle;
m_localAxisC = prismatic->m_localXAxisA;
b2Vec2 pC = m_localAnchorC;
b2Vec2 pA = b2MulT(xfC.q, b2Mul(xfA.q, m_localAnchorA) + (xfA.p - xfC.p));
coordinateA = b2Dot(pA - pC, m_localAxisC);
}
m_bodyD = m_joint2->GetBodyA();
m_bodyB = m_joint2->GetBodyB();
// Get geometry of joint2
b2Transform xfB = m_bodyB->m_xf;
float32 aB = m_bodyB->m_sweep.a;
b2Transform xfD = m_bodyD->m_xf;
float32 aD = m_bodyD->m_sweep.a;
if (m_typeB == e_revoluteJoint)
{
b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint2;
m_localAnchorD = revolute->m_localAnchorA;
m_localAnchorB = revolute->m_localAnchorB;
m_referenceAngleB = revolute->m_referenceAngle;
m_localAxisD.SetZero();
coordinateB = aB - aD - m_referenceAngleB;
}
else
{
b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint2;
m_localAnchorD = prismatic->m_localAnchorA;
m_localAnchorB = prismatic->m_localAnchorB;
m_referenceAngleB = prismatic->m_referenceAngle;
m_localAxisD = prismatic->m_localXAxisA;
b2Vec2 pD = m_localAnchorD;
b2Vec2 pB = b2MulT(xfD.q, b2Mul(xfB.q, m_localAnchorB) + (xfB.p - xfD.p));
coordinateB = b2Dot(pB - pD, m_localAxisD);
}
m_ratio = def->ratio;
m_constant = coordinateA + m_ratio * coordinateB;
m_impulse = 0.0f;
}
void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color& color)
{
switch (fixture->GetType())
{
case b2Shape::e_circle:
{
b2CircleShape* circle = (b2CircleShape*)fixture->GetShape();
b2Vec2 center = b2Mul(xf, circle->m_p);
float32 radius = circle->m_radius;
b2Vec2 axis = b2Mul(xf.q, b2Vec2(1.0f, 0.0f));
m_debugDraw->DrawSolidCircle(center, radius, axis, color);
}
break;
case b2Shape::e_edge:
{
b2EdgeShape* edge = (b2EdgeShape*)fixture->GetShape();
b2Vec2 v1 = b2Mul(xf, edge->m_vertex1);
b2Vec2 v2 = b2Mul(xf, edge->m_vertex2);
m_debugDraw->DrawSegment(v1, v2, color);
}
break;
case b2Shape::e_chain:
{
b2ChainShape* chain = (b2ChainShape*)fixture->GetShape();
int32 count = chain->m_count;
const b2Vec2* vertices = chain->m_vertices;
b2Vec2 v1 = b2Mul(xf, vertices[0]);
for (int32 i = 1; i < count; ++i)
{
b2Vec2 v2 = b2Mul(xf, vertices[i]);
m_debugDraw->DrawSegment(v1, v2, color);
m_debugDraw->DrawCircle(v1, 0.05f, color);
v1 = v2;
}
}
break;
case b2Shape::e_polygon:
{
b2PolygonShape* poly = (b2PolygonShape*)fixture->GetShape();
int32 vertexCount = poly->m_count;
b2Assert(vertexCount <= b2_maxPolygonVertices);
b2Vec2 vertices[b2_maxPolygonVertices];
for (int32 i = 0; i < vertexCount; ++i)
{
vertices[i] = b2Mul(xf, poly->m_vertices[i]);
}
m_debugDraw->DrawSolidPolygon(vertices, vertexCount, color);
}
break;
default:
break;
}
}
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 b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const
{
// Polygon mass, centroid, and inertia.
// Let rho be the polygon density in mass per unit area.
// Then:
// mass = rho * int(dA)
// centroid.x = (1/mass) * rho * int(x * dA)
// centroid.y = (1/mass) * rho * int(y * dA)
// I = rho * int((x*x + y*y) * dA)
//
// We can compute these integrals by summing all the integrals
// for each triangle of the polygon. To evaluate the integral
// for a single triangle, we make a change of variables to
// the (u,v) coordinates of the triangle:
// x = x0 + e1x * u + e2x * v
// y = y0 + e1y * u + e2y * v
// where 0 <= u && 0 <= v && u + v <= 1.
//
// We integrate u from [0,1-v] and then v from [0,1].
// We also need to use the Jacobian of the transformation:
// D = cross(e1, e2)
//
// Simplification: triangle centroid = (1/3) * (p1 + p2 + p3)
//
// The rest of the derivation is handled by computer algebra.
b2Assert(m_vertexCount >= 3);
b2Vec2 center; center.Set(0.0f, 0.0f);
float32 area = 0.0f;
float32 I = 0.0f;
// s is the reference point for forming triangles.
// It's location doesn't change the result (except for rounding error).
b2Vec2 s(0.0f, 0.0f);
// This code would put the reference point inside the polygon.
for (int32 i = 0; i < m_vertexCount; ++i)
{
s += m_vertices[i];
}
s *= 1.0f / m_vertexCount;
const float32 k_inv3 = 1.0f / 3.0f;
for (int32 i = 0; i < m_vertexCount; ++i)
{
// Triangle vertices.
b2Vec2 e1 = m_vertices[i] - s;
b2Vec2 e2 = i + 1 < m_vertexCount ? m_vertices[i+1] - s : m_vertices[0] - s;
float32 D = b2Cross(e1, e2);
float32 triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
center += triangleArea * k_inv3 * (e1 + e2);
float32 ex1 = e1.x, ey1 = e1.y;
float32 ex2 = e2.x, ey2 = e2.y;
float32 intx2 = ex1*ex1 + ex2*ex1 + ex2*ex2;
float32 inty2 = ey1*ey1 + ey2*ey1 + ey2*ey2;
I += (0.25f * k_inv3 * D) * (intx2 + inty2);
}
// Total mass
massData->mass = density * area;
// Center of mass
b2Assert(area > b2_epsilon);
center *= 1.0f / area;
massData->center = center + s;
// Inertia tensor relative to the local origin (point s).
massData->I = density * I;
// Shift to center of mass then to original body origin.
massData->I += massData->mass * (b2Dot(massData->center, massData->center) - b2Dot(center, center));
}
b2PolygonAndCircleContact::b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
: b2Contact(fixtureA, fixtureB)
{
b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon);
b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
}
void b2World::DestroyJoint(b2Joint* j)
{
b2Assert(IsLocked() == false);
if (IsLocked())
{
return;
}
bool collideConnected = j->m_collideConnected;
// Remove from the doubly linked list.
if (j->m_prev)
{
j->m_prev->m_next = j->m_next;
}
if (j->m_next)
{
j->m_next->m_prev = j->m_prev;
}
if (j == m_jointList)
{
m_jointList = j->m_next;
}
// Disconnect from island graph.
b2Body* bodyA = j->m_bodyA;
b2Body* bodyB = j->m_bodyB;
// Wake up connected bodies.
bodyA->SetAwake(true);
bodyB->SetAwake(true);
// Remove from body 1.
if (j->m_edgeA.prev)
{
j->m_edgeA.prev->next = j->m_edgeA.next;
}
if (j->m_edgeA.next)
{
j->m_edgeA.next->prev = j->m_edgeA.prev;
}
if (&j->m_edgeA == bodyA->m_jointList)
{
bodyA->m_jointList = j->m_edgeA.next;
}
j->m_edgeA.prev = NULL;
j->m_edgeA.next = NULL;
// Remove from body 2
if (j->m_edgeB.prev)
{
j->m_edgeB.prev->next = j->m_edgeB.next;
}
if (j->m_edgeB.next)
{
j->m_edgeB.next->prev = j->m_edgeB.prev;
}
if (&j->m_edgeB == bodyB->m_jointList)
{
bodyB->m_jointList = j->m_edgeB.next;
}
j->m_edgeB.prev = NULL;
j->m_edgeB.next = NULL;
b2Joint::Destroy(j, &m_blockAllocator);
b2Assert(m_jointCount > 0);
--m_jointCount;
// If the joint prevents collisions, then flag any contacts for filtering.
if (collideConnected == false)
{
b2ContactEdge* edge = bodyB->GetContactList();
while (edge)
{
if (edge->other == bodyA)
{
// Flag the contact for filtering at the next time step (where either
// body is awake).
edge->contact->FlagForFiltering();
}
edge = edge->next;
}
}
}
void b2PulleyJoint::InitVelocityConstraints(const b2TimeStep& step)
{
b2Body* b1 = m_body1;
b2Body* b2 = m_body2;
b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
b2Vec2 p1 = b1->m_sweep.c + r1;
b2Vec2 p2 = b2->m_sweep.c + r2;
b2Vec2 s1 = m_ground->GetXForm().position + m_groundAnchor1;
b2Vec2 s2 = m_ground->GetXForm().position + m_groundAnchor2;
// Get the pulley axes.
m_u1 = p1 - s1;
m_u2 = p2 - s2;
float32 length1 = m_u1.Length();
float32 length2 = m_u2.Length();
if (length1 > b2_linearSlop)
{
m_u1 *= 1.0f / length1;
}
else
{
m_u1.SetZero();
}
if (length2 > b2_linearSlop)
{
m_u2 *= 1.0f / length2;
}
else
{
m_u2.SetZero();
}
float32 C = m_constant - length1 - m_ratio * length2;
if (C > 0.0f)
{
m_state = e_inactiveLimit;
m_force = 0.0f;
}
else
{
m_state = e_atUpperLimit;
m_positionImpulse = 0.0f;
}
if (length1 < m_maxLength1)
{
m_limitState1 = e_inactiveLimit;
m_limitForce1 = 0.0f;
}
else
{
m_limitState1 = e_atUpperLimit;
m_limitPositionImpulse1 = 0.0f;
}
if (length2 < m_maxLength2)
{
m_limitState2 = e_inactiveLimit;
m_limitForce2 = 0.0f;
}
else
{
m_limitState2 = e_atUpperLimit;
m_limitPositionImpulse2 = 0.0f;
}
// Compute effective mass.
float32 cr1u1 = b2Cross(r1, m_u1);
float32 cr2u2 = b2Cross(r2, m_u2);
m_limitMass1 = b1->m_invMass + b1->m_invI * cr1u1 * cr1u1;
m_limitMass2 = b2->m_invMass + b2->m_invI * cr2u2 * cr2u2;
m_pulleyMass = m_limitMass1 + m_ratio * m_ratio * m_limitMass2;
b2Assert(m_limitMass1 > B2_FLT_EPSILON);
b2Assert(m_limitMass2 > B2_FLT_EPSILON);
b2Assert(m_pulleyMass > B2_FLT_EPSILON);
m_limitMass1 = 1.0f / m_limitMass1;
m_limitMass2 = 1.0f / m_limitMass2;
m_pulleyMass = 1.0f / m_pulleyMass;
if (step.warmStarting)
{
// Warm starting.
b2Vec2 P1 = B2FORCE_SCALE(step.dt) * (-m_force - m_limitForce1) * m_u1;
b2Vec2 P2 = B2FORCE_SCALE(step.dt) * (-m_ratio * m_force - m_limitForce2) * m_u2;
b1->m_linearVelocity += b1->m_invMass * P1;
b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1);
b2->m_linearVelocity += b2->m_invMass * P2;
b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2);
}
else
{
m_force = 0.0f;
m_limitForce1 = 0.0f;
m_limitForce2 = 0.0f;
}
}
// 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 = NULL;
b2_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;
}
b2_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.
b2_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);
b2_int32 indexA = c->GetChildIndexA();
b2_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 .
b2_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 == NULL || 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 (b2_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 (b2_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;
}
}
}
/// Free a b2TrackedBlock.
void b2TrackedBlock::Free(b2TrackedBlock *block)
{
b2Assert(block);
block->~b2TrackedBlock();
b2Free(block);
}