// Verify that it's possible to enable / disable collisions between particles.
TEST_F(BodyContactTests, EnableDisableParticleContactsWithContactFilter)
{
m_world->SetGravity(b2Vec2_zero);
// Through two particles horizontally at each other
// (A moving right, B moving left).
b2ParticleDef pd;
pd.flags = b2_particleContactFilterParticle;
pd.position.Set(-m_particleDiameter * 2.0f, 0.0f);
pd.velocity.Set(m_particleDiameter, 0.0f);
int32 particleIndexA = m_particleSystem->CreateParticle(pd);
pd.flags = b2_particleContactFilterParticle;
pd.position.Set(m_particleDiameter * 2.0f, 0.0f);
pd.velocity.Set(-m_particleDiameter, 0.0f);
int32 particleIndexB = m_particleSystem->CreateParticle(pd);
// Leave contacts enabled.
ParticleContactDisabler contactDisabler;
m_world->SetContactFilter(&contactDisabler);
// Run the simulation and verify that particles don't pass each other.
RunStep(60.0f, 3.0f);
// WARNING: This assumes particle indicies have not been reallocated during
// the simulation.
b2Vec2* positions = m_particleSystem->GetPositionBuffer();
b2Vec2* velocities = m_particleSystem->GetVelocityBuffer();
EXPECT_LT(positions[particleIndexA].x, positions[particleIndexB].x);
EXPECT_LT(b2Abs(velocities[particleIndexA].x), m_particleDiameter);
EXPECT_LT(b2Abs(velocities[particleIndexB].x), m_particleDiameter);
// Disable particle / particle contacts.
contactDisabler.m_enableParticleParticleCollisions = false;
// Reset the positions and velocities of the particles.
positions[particleIndexA].Set(-m_particleDiameter * 2.0f, 0.0f);
velocities[particleIndexA].Set(m_particleDiameter, 0.0f);
positions[particleIndexB].Set(m_particleDiameter * 2.0f, 0.0f);
velocities[particleIndexB].Set(-m_particleDiameter, 0.0f);
// Run the simulation and verify that particles now pass each other (i.e
// they no longer collide).
RunStep(60.0f, 3.0f);
positions = m_particleSystem->GetPositionBuffer();
velocities = m_particleSystem->GetVelocityBuffer();
EXPECT_GT(positions[particleIndexA].x, positions[particleIndexB].x);
EXPECT_FLOAT_EQ(velocities[particleIndexA].x, m_particleDiameter);
EXPECT_FLOAT_EQ(velocities[particleIndexB].x, -m_particleDiameter);
}
bool b2DistanceJoint::SolvePositionConstraints()
{
if (m_frequencyHz > 0.0f)
{
return true;
}
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 d = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
float32 length = d.Normalize();
float32 C = length - m_length;
C = b2Clamp(C, -b2_maxLinearCorrection, b2_maxLinearCorrection);
float32 impulse = -m_mass * C;
m_u = d;
b2Vec2 P = impulse * m_u;
b1->m_sweep.c -= b1->m_invMass * P;
b1->m_sweep.a -= b1->m_invI * b2Cross(r1, P);
b2->m_sweep.c += b2->m_invMass * P;
b2->m_sweep.a += b2->m_invI * b2Cross(r2, P);
b1->SynchronizeTransform();
b2->SynchronizeTransform();
return b2Abs(C) < b2_linearSlop;
}
void b2PolyShape::ResetProxy(b2BroadPhase* broadPhase)
{
if (m_proxyId == b2_nullProxy)
{
return;
}
b2Proxy* proxy = broadPhase->GetProxy(m_proxyId);
broadPhase->DestroyProxy(m_proxyId);
proxy = NULL;
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;
if (broadPhase->InRange(aabb))
{
m_proxyId = broadPhase->CreateProxy(aabb, this);
}
else
{
m_proxyId = b2_nullProxy;
}
if (m_proxyId == b2_nullProxy)
{
m_body->Freeze();
}
}
static bool InPoints(const b2Vec2& w, const b2Vec2* points, int32 pointCount)
{
const float32 k_tolerance = 100.0f * B2_FLT_EPSILON;
for (int32 i = 0; i < pointCount; ++i)
{
b2Vec2 d = b2Abs(w - points[i]);
b2Vec2 m = b2Max(b2Abs(w), b2Abs(points[i]));
if (d.x < k_tolerance * (m.x + 1.0f) &&
d.y < k_tolerance * (m.y + 1.0f))
{
return true;
}
}
return false;
}
void b2PolyShape::Synchronize( const b2Vec2& position1, const b2Mat22& R1,
const b2Vec2& position2, const b2Mat22& R2)
{
// The body transform is copied for convenience.
m_R = R2;
m_position = position2 + b2Mul(R2, m_localCentroid);
if (m_proxyId == b2_nullProxy)
{
return;
}
b2AABB aabb1, aabb2;
{
b2Mat22 obbR = b2Mul(R1, m_localOBB.R);
b2Mat22 absR = b2Abs(obbR);
b2Vec2 h = b2Mul(absR, m_localOBB.extents);
b2Vec2 center = position1 + b2Mul(R1, m_localCentroid + m_localOBB.center);
aabb1.minVertex = center - h;
aabb1.maxVertex = center + h;
}
{
b2Mat22 obbR = b2Mul(R2, m_localOBB.R);
b2Mat22 absR = b2Abs(obbR);
b2Vec2 h = b2Mul(absR, m_localOBB.extents);
b2Vec2 center = position2 + b2Mul(R2, m_localCentroid + m_localOBB.center);
aabb2.minVertex = center - h;
aabb2.maxVertex = center + h;
}
b2AABB aabb;
aabb.minVertex = b2Min(aabb1.minVertex, aabb2.minVertex);
aabb.maxVertex = b2Max(aabb1.maxVertex, aabb2.maxVertex);
b2BroadPhase* broadPhase = m_body->m_world->m_broadPhase;
if (broadPhase->InRange(aabb))
{
broadPhase->MoveProxy(m_proxyId, aabb);
}
else
{
m_body->Freeze();
}
}
bool b2WheelJoint::SolvePositionConstraints(float32 baumgarte)
{
B2_NOT_USED(baumgarte);
b2Body* bA = m_bodyA;
b2Body* bB = m_bodyB;
b2Vec2 xA = bA->m_sweep.c;
float32 angleA = bA->m_sweep.a;
b2Vec2 xB = bB->m_sweep.c;
float32 angleB = bB->m_sweep.a;
b2Mat22 RA(angleA), RB(angleB);
b2Vec2 rA = b2Mul(RA, m_localAnchorA - m_localCenterA);
b2Vec2 rB = b2Mul(RB, m_localAnchorB - m_localCenterB);
b2Vec2 d = xB + rB - xA - rA;
b2Vec2 ay = b2Mul(RA, m_localYAxisA);
float32 sAy = b2Cross(d + rA, ay);
float32 sBy = b2Cross(rB, ay);
float32 C = b2Dot(d, ay);
float32 k = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy;
float32 impulse;
if (k != 0.0f)
{
impulse = - C / k;
}
else
{
impulse = 0.0f;
}
b2Vec2 P = impulse * ay;
float32 LA = impulse * sAy;
float32 LB = impulse * sBy;
xA -= m_invMassA * P;
angleA -= m_invIA * LA;
xB += m_invMassB * P;
angleB += m_invIB * LB;
// TODO_ERIN remove need for this.
bA->m_sweep.c = xA;
bA->m_sweep.a = angleA;
bB->m_sweep.c = xB;
bB->m_sweep.a = angleB;
bA->SynchronizeTransform();
bB->SynchronizeTransform();
return b2Abs(C) <= b2_linearSlop;
}
void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2XForm& xf) const
{
b2Mat22 R = b2Mul(xf.R, m_obb.R);
b2Mat22 absR = b2Abs(R);
b2Vec2 h = b2Mul(absR, m_obb.extents);
b2Vec2 position = xf.position + b2Mul(xf.R, m_obb.center);
aabb->lowerBound = position - h;
aabb->upperBound = position + h;
}
bool b2WeldJoint::SolvePositionConstraints(float32 baumgarte)
{
B2_NOT_USED(baumgarte);
b2Body* bA = m_bodyA;
b2Body* bB = m_bodyB;
float32 mA = bA->m_invMass, mB = bB->m_invMass;
float32 iA = bA->m_invI, iB = bB->m_invI;
b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
b2Vec2 C1 = bB->m_sweep.c + rB - bA->m_sweep.c - rA;
float32 C2 = bB->m_sweep.a - bA->m_sweep.a - m_referenceAngle;
// Handle large detachment.
const float32 k_allowedStretch = 10.0f * b2_linearSlop;
float32 positionError = C1.Length();
float32 angularError = b2Abs(C2);
if (positionError > k_allowedStretch)
{
iA *= 1.0f;
iB *= 1.0f;
}
m_mass.col1.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
m_mass.col2.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
m_mass.col3.x = -rA.y * iA - rB.y * iB;
m_mass.col1.y = m_mass.col2.x;
m_mass.col2.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
m_mass.col3.y = rA.x * iA + rB.x * iB;
m_mass.col1.z = m_mass.col3.x;
m_mass.col2.z = m_mass.col3.y;
m_mass.col3.z = iA + iB;
b2Vec3 C(C1.x, C1.y, C2);
b2Vec3 impulse = m_mass.Solve33(-C);
b2Vec2 P(impulse.x, impulse.y);
bA->m_sweep.c -= mA * P;
bA->m_sweep.a -= iA * (b2Cross(rA, P) + impulse.z);
bB->m_sweep.c += mB * P;
bB->m_sweep.a += iB * (b2Cross(rB, P) + impulse.z);
bA->SynchronizeTransform();
bB->SynchronizeTransform();
return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
}
bool b2FixedJoint::SolvePositionConstraints(float32 baumgarte)
{
B2_NOT_USED(baumgarte);
// Get bodies
b2Body* b1 = m_body1;
b2Body* b2 = m_body2;
// Recalculate effective constraint mass if angle changed enough
if (b2Abs(b1->m_sweep.a - m_a1) > 1e-3f)
CalculateMC();
// Calculate C
float C[3] =
{
b2->m_sweep.c.x - b1->m_sweep.c.x - m_c * m_d.x + m_s * m_d.y,
b2->m_sweep.c.y - b1->m_sweep.c.y - m_s * m_d.x - m_c * m_d.y,
b2->m_sweep.a - m_a1 - m_a
};
// Calculate lambda
float lambda[3];
for (int r = 0; r < 3; ++r)
lambda[r] = -(m_mc[r][0] * C[0] + m_mc[r][1] * C[1] + m_mc[r][2] * C[2]);
// Apply impulse
b1->m_sweep.c.x -= b1->m_invMass * lambda[0];
b1->m_sweep.c.y -= b1->m_invMass * lambda[1];
b1->m_sweep.a -= b1->m_invI * (lambda[0]*m_Ax+lambda[1]*m_Ay+lambda[2]);
b2->m_sweep.c.x += b2->m_invMass * lambda[0];
b2->m_sweep.c.y += b2->m_invMass * lambda[1];
b2->m_sweep.a += b2->m_invI * lambda[2];
// Push the changes to the transforms
b1->SynchronizeTransform();
b2->SynchronizeTransform();
// Constraint is satisfied if all constraint equations are nearly zero
return b2Abs(C[0]) < b2_linearSlop && b2Abs(C[1]) < b2_linearSlop && b2Abs(C[2]) < b2_angularSlop;
}
static float32 calcDistanceToLine( const Vec2& pt,
const Vec2& l1, const Vec2& l2,
bool* withinLine=NULL )
{
b2Vec2 l = l2 - l1;
b2Vec2 w = pt - l1;
float32 mag = l.Normalize();
float32 dist = b2Cross( w, l );
if ( withinLine ) {
float32 dot = b2Dot( l, w );
*withinLine = ( dot >= 0.0f && dot <= mag );
}
return b2Abs( dist );
}
bool b2WheelJoint::SolvePositionConstraints(const b2SolverData& data)
{
b2Vec2 cA = data.positions[m_indexA].c;
float32 aA = data.positions[m_indexA].a;
b2Vec2 cB = data.positions[m_indexB].c;
float32 aB = data.positions[m_indexB].a;
b2Rot qA(aA), qB(aB);
b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
b2Vec2 d = (cB - cA) + rB - rA;
b2Vec2 ay = b2Mul(qA, m_localYAxisA);
float32 sAy = b2Cross(d + rA, ay);
float32 sBy = b2Cross(rB, ay);
float32 C = b2Dot(d, ay);
float32 k = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy;
float32 impulse;
if (k != 0.0f)
{
impulse = - C / k;
}
else
{
impulse = 0.0f;
}
b2Vec2 P = impulse * ay;
float32 LA = impulse * sAy;
float32 LB = impulse * sBy;
cA -= m_invMassA * P;
aA -= m_invIA * LA;
cB += m_invMassB * P;
aB += m_invIB * LB;
data.positions[m_indexA].c = cA;
data.positions[m_indexA].a = aA;
data.positions[m_indexB].c = cB;
data.positions[m_indexB].a = aB;
return b2Abs(C) <= b2_linearSlop;
}
bool b2ElasticRopeJoint::SolvePositionConstraints(const b2SolverData& data)
{
if (m_frequencyHz > 0.0f)
{
// There is no position correction for soft distance constraints.
return true;
}
b2Vec2 cA = data.positions[m_indexA].c;
float32 aA = data.positions[m_indexA].a;
b2Vec2 cB = data.positions[m_indexB].c;
float32 aB = data.positions[m_indexB].a;
b2Rot qA(aA), qB(aB);
b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
b2Vec2 u = cB + rB - cA - rA;
float32 length = u.Normalize();
float32 C;
if(length-m_length < 0) {
C = 0;//length - m_length;
} else {
C = length - m_length;
}
C = b2Clamp(C, -b2_maxLinearCorrection, b2_maxLinearCorrection);
float32 impulse = -m_mass * C;
b2Vec2 P = impulse * u;
cA -= m_invMassA * P;
aA -= m_invIA * b2Cross(rA, P);
cB += m_invMassB * P;
aB += m_invIB * b2Cross(rB, P);
data.positions[m_indexA].c = cA;
data.positions[m_indexA].a = aA;
data.positions[m_indexB].c = cB;
data.positions[m_indexB].a = aB;
return b2Abs(C) < b2_linearSlop;
}
int32 b2DynamicTree::GetMaxBalance() const
{
int32 maxBalance = 0;
for (int32 i = 0; i < m_nodeCapacity; ++i)
{
const b2TreeNode* node = m_nodes + i;
if (node->height <= 1)
{
continue;
}
b2Assert(node->IsLeaf() == false);
int32 child1 = node->child1;
int32 child2 = node->child2;
int32 balance = b2Abs(m_nodes[child2].height - m_nodes[child1].height);
maxBalance = b2Max(maxBalance, balance);
}
return maxBalance;
}
bool b2DistanceJoint::SolvePositionConstraints(float32 baumgarte)
{
B2_NOT_USED(baumgarte);
if (m_frequencyHz > 0.0f)
{
// There is no position correction for soft distance constraints.
return true;
}
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 d = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
float32 length = d.Normalize();
float32 C = length - m_length;
C = b2Clamp(C, -(settings->b2_maxLinearCorrection), settings->b2_maxLinearCorrection);
float32 impulse = -m_mass * C;
m_u = d;
b2Vec2 P = impulse * m_u;
b1->m_sweep.c -= b1->m_invMass * P;
b1->m_sweep.a -= b1->m_invI * b2Cross(r1, P);
b2->m_sweep.c += b2->m_invMass * P;
b2->m_sweep.a += b2->m_invI * b2Cross(r2, P);
b1->SynchronizeTransform();
b2->SynchronizeTransform();
return b2Abs(C) < settings->b2_linearSlop;
}
void b2LineJoint::InitVelocityConstraints(const b2TimeStep& step)
{
b2Body* b1 = m_bodyA;
b2Body* b2 = m_bodyB;
m_localCenterA = b1->GetLocalCenter();
m_localCenterB = b2->GetLocalCenter();
b2Transform xf1 = b1->GetTransform();
b2Transform xf2 = b2->GetTransform();
// Compute the effective masses.
b2Vec2 r1 = b2Mul(xf1.R, m_localAnchor1 - m_localCenterA);
b2Vec2 r2 = b2Mul(xf2.R, m_localAnchor2 - m_localCenterB);
b2Vec2 d = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
m_invMassA = b1->m_invMass;
m_invIA = b1->m_invI;
m_invMassB = b2->m_invMass;
m_invIB = b2->m_invI;
// Compute motor Jacobian and effective mass.
{
m_axis = b2Mul(xf1.R, m_localXAxis1);
m_a1 = b2Cross(d + r1, m_axis);
m_a2 = b2Cross(r2, m_axis);
m_motorMass = m_invMassA + m_invMassB + m_invIA * m_a1 * m_a1 + m_invIB * m_a2 * m_a2;
if (m_motorMass > b2_epsilon)
{
m_motorMass = 1.0f / m_motorMass;
}
else
{
m_motorMass = 0.0f;
}
}
// Prismatic constraint.
{
m_perp = b2Mul(xf1.R, m_localYAxis1);
m_s1 = b2Cross(d + r1, m_perp);
m_s2 = b2Cross(r2, m_perp);
float32 m1 = m_invMassA, m2 = m_invMassB;
float32 i1 = m_invIA, i2 = m_invIB;
float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
float32 k12 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2;
float32 k22 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2;
m_K.col1.Set(k11, k12);
m_K.col2.Set(k12, k22);
}
// Compute motor and limit terms.
if (m_enableLimit)
{
float32 jointTranslation = b2Dot(m_axis, d);
if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)
{
m_limitState = e_equalLimits;
}
else if (jointTranslation <= m_lowerTranslation)
{
if (m_limitState != e_atLowerLimit)
{
m_limitState = e_atLowerLimit;
m_impulse.y = 0.0f;
}
}
else if (jointTranslation >= m_upperTranslation)
{
if (m_limitState != e_atUpperLimit)
{
m_limitState = e_atUpperLimit;
m_impulse.y = 0.0f;
}
}
else
{
m_limitState = e_inactiveLimit;
m_impulse.y = 0.0f;
}
}
else
{
m_limitState = e_inactiveLimit;
}
if (m_enableMotor == false)
{
m_motorImpulse = 0.0f;
}
if (step.warmStarting)
{
// Account for variable time step.
m_impulse *= step.dtRatio;
m_motorImpulse *= step.dtRatio;
b2Vec2 P = m_impulse.x * m_perp + (m_motorImpulse + m_impulse.y) * m_axis;
float32 L1 = m_impulse.x * m_s1 + (m_motorImpulse + m_impulse.y) * m_a1;
float32 L2 = m_impulse.x * m_s2 + (m_motorImpulse + m_impulse.y) * m_a2;
b1->m_linearVelocity -= m_invMassA * P;
b1->m_angularVelocity -= m_invIA * L1;
b2->m_linearVelocity += m_invMassB * P;
b2->m_angularVelocity += m_invIB * L2;
}
else
{
m_impulse.SetZero();
m_motorImpulse = 0.0f;
}
}
void b2RevoluteJoint::InitVelocityConstraints(const b2SolverData& data)
{
m_indexA = m_bodyA->m_islandIndex;
m_indexB = m_bodyB->m_islandIndex;
m_localCenterA = m_bodyA->m_sweep.localCenter;
m_localCenterB = m_bodyB->m_sweep.localCenter;
m_invMassA = m_bodyA->m_invMass;
m_invMassB = m_bodyB->m_invMass;
m_invIA = m_bodyA->m_invI;
m_invIB = m_bodyB->m_invI;
b2Vec2 cA = data.positions[m_indexA].c;
float32 aA = data.positions[m_indexA].a;
b2Vec2 vA = data.velocities[m_indexA].v;
float32 wA = data.velocities[m_indexA].w;
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 qA(aA), qB(aB);
m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
float32 mA = m_invMassA, mB = m_invMassB;
float32 iA = m_invIA, iB = m_invIB;
bool fixedRotation = (iA + iB == 0.0f);
m_mass.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
m_mass.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
m_mass.ez.x = -m_rA.y * iA - m_rB.y * iB;
m_mass.ex.y = m_mass.ey.x;
m_mass.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
m_mass.ez.y = m_rA.x * iA + m_rB.x * iB;
m_mass.ex.z = m_mass.ez.x;
m_mass.ey.z = m_mass.ez.y;
m_mass.ez.z = iA + iB;
m_motorMass = iA + iB;
if (m_motorMass > 0.0f)
{
m_motorMass = 1.0f / m_motorMass;
}
if (m_enableMotor == false || fixedRotation)
{
m_motorImpulse = 0.0f;
}
if (m_enableLimit && fixedRotation == false)
{
float32 jointAngle = aB - aA - m_referenceAngle;
if (b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2_angularSlop)
{
m_limitState = e_equalLimits;
}
else if (jointAngle <= m_lowerAngle)
{
if (m_limitState != e_atLowerLimit)
{
m_impulse.z = 0.0f;
}
m_limitState = e_atLowerLimit;
}
else if (jointAngle >= m_upperAngle)
{
if (m_limitState != e_atUpperLimit)
{
m_impulse.z = 0.0f;
}
m_limitState = e_atUpperLimit;
}
else
{
m_limitState = e_inactiveLimit;
m_impulse.z = 0.0f;
}
}
else
{
m_limitState = e_inactiveLimit;
}
if (data.step.warmStarting)
{
// Scale impulses to support a variable time step.
m_impulse *= data.step.dtRatio;
m_motorImpulse *= data.step.dtRatio;
b2Vec2 P(m_impulse.x, m_impulse.y);
vA -= mA * P;
wA -= iA * (b2Cross(m_rA, P) + m_motorImpulse + m_impulse.z);
vB += mB * P;
wB += iB * (b2Cross(m_rB, P) + m_motorImpulse + m_impulse.z);
}
else
{
m_impulse.SetZero();
m_motorImpulse = 0.0f;
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
bool b2RevoluteJoint::SolvePositionConstraints(const b2SolverData& data)
{
b2Vec2 cA = data.positions[m_indexA].c;
float32 aA = data.positions[m_indexA].a;
b2Vec2 cB = data.positions[m_indexB].c;
float32 aB = data.positions[m_indexB].a;
b2Rot qA(aA), qB(aB);
float32 angularError = 0.0f;
float32 positionError = 0.0f;
bool fixedRotation = (m_invIA + m_invIB == 0.0f);
// Solve angular limit constraint.
if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false)
{
float32 angle = aB - aA - m_referenceAngle;
float32 limitImpulse = 0.0f;
if (m_limitState == e_equalLimits)
{
// Prevent large angular corrections
float32 C = b2Clamp(angle - m_lowerAngle, -b2_maxAngularCorrection, b2_maxAngularCorrection);
limitImpulse = -m_motorMass * C;
angularError = b2Abs(C);
}
else if (m_limitState == e_atLowerLimit)
{
float32 C = angle - m_lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = b2Clamp(C + b2_angularSlop, -b2_maxAngularCorrection, 0.0f);
limitImpulse = -m_motorMass * C;
}
else if (m_limitState == e_atUpperLimit)
{
float32 C = angle - m_upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = b2Clamp(C - b2_angularSlop, 0.0f, b2_maxAngularCorrection);
limitImpulse = -m_motorMass * C;
}
aA -= m_invIA * limitImpulse;
aB += m_invIB * limitImpulse;
}
// Solve point-to-point constraint.
{
qA.Set(aA);
qB.Set(aB);
b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
b2Vec2 C = cB + rB - cA - rA;
positionError = C.Length();
float32 mA = m_invMassA, mB = m_invMassB;
float32 iA = m_invIA, iB = m_invIB;
b2Mat22 K;
K.ex.x = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y;
K.ex.y = -iA * rA.x * rA.y - iB * rB.x * rB.y;
K.ey.x = K.ex.y;
K.ey.y = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x;
b2Vec2 impulse = -K.Solve(C);
cA -= mA * impulse;
aA -= iA * b2Cross(rA, impulse);
cB += mB * impulse;
aB += iB * b2Cross(rB, impulse);
}
data.positions[m_indexA].c = cA;
data.positions[m_indexA].a = aA;
data.positions[m_indexB].c = cB;
data.positions[m_indexB].a = aB;
return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
}
float32 b2TimeOfImpact(const b2TOIInput* input, const TA* shapeA, const TB* shapeB)
{
b2Sweep sweepA = input->sweepA;
b2Sweep sweepB = input->sweepB;
float32 r1 = input->sweepRadiusA;
float32 r2 = input->sweepRadiusB;
float32 tolerance = input->tolerance;
float32 radius = shapeA->m_radius + shapeB->m_radius;
b2Assert(sweepA.t0 == sweepB.t0);
b2Assert(1.0f - sweepA.t0 > B2_FLT_EPSILON);
b2Vec2 v1 = sweepA.c - sweepA.c0;
b2Vec2 v2 = sweepB.c - sweepB.c0;
float32 omega1 = sweepA.a - sweepA.a0;
float32 omega2 = sweepB.a - sweepB.a0;
float32 alpha = 0.0f;
b2DistanceInput distanceInput;
distanceInput.useRadii = false;
b2SimplexCache cache;
cache.count = 0;
b2Vec2 p1, p2;
const int32 k_maxIterations = 1000; // TODO_ERIN b2Settings
int32 iter = 0;
b2Vec2 normal = b2Vec2_zero;
float32 distance = 0.0f;
float32 targetDistance = 0.0f;
for(;;)
{
b2XForm xf1, xf2;
sweepA.GetTransform(&xf1, alpha);
sweepB.GetTransform(&xf2, alpha);
// Get the distance between shapes.
distanceInput.transformA = xf1;
distanceInput.transformB = xf2;
b2DistanceOutput distanceOutput;
b2Distance(&distanceOutput, &cache, &distanceInput, shapeA, shapeB);
distance = distanceOutput.distance;
p1 = distanceOutput.pointA;
p2 = distanceOutput.pointB;
if (iter == 0)
{
// Compute a reasonable target distance to give some breathing room
// for conservative advancement.
if (distance > radius)
{
targetDistance = b2Max(radius - tolerance, 0.75f * radius);
}
else
{
targetDistance = b2Max(distance - tolerance, 0.02f * radius);
}
}
if (distance - targetDistance < 0.5f * tolerance || iter == k_maxIterations)
{
break;
}
normal = p2 - p1;
normal.Normalize();
// Compute upper bound on remaining movement.
float32 approachVelocityBound = b2Dot(normal, v1 - v2) + b2Abs(omega1) * r1 + b2Abs(omega2) * r2;
if (b2Abs(approachVelocityBound) < B2_FLT_EPSILON)
{
alpha = 1.0f;
break;
}
// Get the conservative time increment. Don't advance all the way.
float32 dAlpha = (distance - targetDistance) / approachVelocityBound;
//float32 dt = (distance - 0.5f * b2_linearSlop) / approachVelocityBound;
float32 newAlpha = alpha + dAlpha;
// The shapes may be moving apart or a safe distance apart.
if (newAlpha < 0.0f || 1.0f < newAlpha)
{
alpha = 1.0f;
break;
}
// Ensure significant advancement.
if (newAlpha < (1.0f + 100.0f * B2_FLT_EPSILON) * alpha)
{
break;
}
alpha = newAlpha;
++iter;
}
b2_maxToiIters = b2Max(iter, b2_maxToiIters);
return alpha;
}
float32 b2TimeOfImpact(const b2TOIInput* input, const TA* shapeA, const TB* shapeB)
{
b2Sweep sweepA = input->sweepA;
b2Sweep sweepB = input->sweepB;
b2Assert(sweepA.t0 == sweepB.t0);
b2Assert(1.0f - sweepA.t0 > B2_FLT_EPSILON);
float32 radius = shapeA->m_radius + shapeB->m_radius;
float32 tolerance = input->tolerance;
float32 alpha = 0.0f;
const int32 k_maxIterations = 1000; // TODO_ERIN b2Settings
int32 iter = 0;
float32 target = 0.0f;
// Prepare input for distance query.
b2SimplexCache cache;
cache.count = 0;
b2DistanceInput distanceInput;
distanceInput.useRadii = false;
for(;;)
{
b2XForm xfA, xfB;
sweepA.GetTransform(&xfA, alpha);
sweepB.GetTransform(&xfB, alpha);
// Get the distance between shapes.
distanceInput.transformA = xfA;
distanceInput.transformB = xfB;
b2DistanceOutput distanceOutput;
b2Distance(&distanceOutput, &cache, &distanceInput, shapeA, shapeB);
if (distanceOutput.distance <= 0.0f)
{
alpha = 1.0f;
break;
}
b2SeparationFunction<TA, TB> fcn;
fcn.Initialize(&cache, shapeA, xfA, shapeB, xfB);
float32 separation = fcn.Evaluate(xfA, xfB);
if (separation <= 0.0f)
{
alpha = 1.0f;
break;
}
if (iter == 0)
{
// Compute a reasonable target distance to give some breathing room
// for conservative advancement. We take advantage of the shape radii
// to create additional clearance.
if (separation > radius)
{
target = b2Max(radius - tolerance, 0.75f * radius);
}
else
{
target = b2Max(separation - tolerance, 0.02f * radius);
}
}
if (separation - target < 0.5f * tolerance)
{
if (iter == 0)
{
alpha = 1.0f;
break;
}
break;
}
#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 1D root of: f(x) - target = 0
float32 newAlpha = alpha;
{
float32 x1 = alpha, x2 = 1.0f;
float32 f1 = separation;
sweepA.GetTransform(&xfA, x2);
sweepB.GetTransform(&xfB, x2);
float32 f2 = fcn.Evaluate(xfA, xfB);
// If intervals don't overlap at t2, then we are done.
if (f2 >= target)
{
alpha = 1.0f;
break;
}
// Determine when intervals intersect.
int32 rootIterCount = 0;
for (;;)
{
// Use a mix of the secant rule and bisection.
float32 x;
if (rootIterCount & 1)
{
// Secant rule to improve convergence.
x = x1 + (target - f1) * (x2 - x1) / (f2 - f1);
}
else
{
// Bisection to guarantee progress.
x = 0.5f * (x1 + x2);
}
sweepA.GetTransform(&xfA, x);
sweepB.GetTransform(&xfB, x);
float32 f = fcn.Evaluate(xfA, xfB);
if (b2Abs(f - target) < 0.025f * tolerance)
{
newAlpha = x;
break;
}
// Ensure we continue to bracket the root.
if (f > target)
{
x1 = x;
f1 = f;
}
else
{
x2 = x;
f2 = f;
}
++rootIterCount;
//b2Assert(rootIterCount < 50);
if (rootIterCount >= 50 )
{
break;
}
}
b2_maxToiRootIters = b2Max(b2_maxToiRootIters, rootIterCount);
}
// Ensure significant advancement.
if (newAlpha < (1.0f + 100.0f * B2_FLT_EPSILON) * alpha)
{
break;
}
alpha = newAlpha;
++iter;
if (iter == k_maxIterations)
{
break;
}
}
b2_maxToiIters = b2Max(b2_maxToiIters, iter);
return alpha;
}
bool b2PulleyJoint::SolvePositionConstraints(const b2SolverData& data)
{
b2Vec2 cA = data.positions[m_indexA].c;
float32 aA = data.positions[m_indexA].a;
b2Vec2 cB = data.positions[m_indexB].c;
float32 aB = data.positions[m_indexB].a;
b2Rot qA(aA), qB(aB);
b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
// Get the pulley axes.
b2Vec2 uA = cA + rA - m_groundAnchorA;
b2Vec2 uB = cB + rB - m_groundAnchorB;
float32 lengthA = uA.Length();
float32 lengthB = uB.Length();
if (lengthA > 10.0f * b2_linearSlop)
{
uA *= 1.0f / lengthA;
}
else
{
uA.SetZero();
}
if (lengthB > 10.0f * b2_linearSlop)
{
uB *= 1.0f / lengthB;
}
else
{
uB.SetZero();
}
// Compute effective mass.
float32 ruA = b2Cross(rA, uA);
float32 ruB = b2Cross(rB, uB);
float32 mA = m_invMassA + m_invIA * ruA * ruA;
float32 mB = m_invMassB + m_invIB * ruB * ruB;
float32 mass = mA + m_ratio * m_ratio * mB;
if (mass > 0.0f)
{
mass = 1.0f / mass;
}
float32 C = m_constant - lengthA - m_ratio * lengthB;
float32 linearError = b2Abs(C);
float32 impulse = -mass * C;
b2Vec2 PA = -impulse * uA;
b2Vec2 PB = -m_ratio * impulse * uB;
cA += m_invMassA * PA;
aA += m_invIA * b2Cross(rA, PA);
cB += m_invMassB * PB;
aB += m_invIB * b2Cross(rB, PB);
data.positions[m_indexA].c = cA;
data.positions[m_indexA].a = aA;
data.positions[m_indexB].c = cB;
data.positions[m_indexB].a = aB;
return linearError < b2_linearSlop;
}
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, , 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;
#if B2_DEBUG_SOLVER == 1
vn1 = 0.0f;
vn2 = vc->K.ex.y * x.x + b.y;
#endif
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;
#if B2_DEBUG_SOLVER == 1
vn1 = vc->K.ey.x * x.y + b.x;
vn2 = 0.0f;
#endif
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;
}
}
void b2Island::Solve(const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep)
{
// Integrate velocities and apply damping.
for (int32 i = 0; i < m_bodyCount; ++i)
{
b2Body* b = m_bodies[i];
if (b->GetType() != b2_dynamicBody)
{
continue;
}
// Integrate velocities.
b->m_linearVelocity += step.dt * (gravity + b->m_invMass * b->m_force);
b->m_angularVelocity += step.dt * b->m_invI * b->m_torque;
// Apply damping.
// ODE: dv/dt + c * v = 0
// Solution: v(t) = v0 * exp(-c * t)
// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
// v2 = exp(-c * dt) * v1
// Taylor expansion:
// v2 = (1.0f - c * dt) * v1
b->m_linearVelocity *= b2Clamp(1.0f - step.dt * b->m_linearDamping, 0.0f, 1.0f);
b->m_angularVelocity *= b2Clamp(1.0f - step.dt * b->m_angularDamping, 0.0f, 1.0f);
}
// Partition contacts so that contacts with static bodies are solved last.
int32 i1 = -1;
for (int32 i2 = 0; i2 < m_contactCount; ++i2)
{
b2Fixture* fixtureA = m_contacts[i2]->GetFixtureA();
b2Fixture* fixtureB = m_contacts[i2]->GetFixtureB();
b2Body* bodyA = fixtureA->GetBody();
b2Body* bodyB = fixtureB->GetBody();
bool nonStatic = bodyA->GetType() != b2_staticBody && bodyB->GetType() != b2_staticBody;
if (nonStatic)
{
++i1;
b2Swap(m_contacts[i1], m_contacts[i2]);
}
}
// Initialize velocity constraints.
b2ContactSolver contactSolver(m_contacts, m_contactCount, m_allocator, step.dtRatio);
contactSolver.WarmStart();
for (int32 i = 0; i < m_jointCount; ++i)
{
m_joints[i]->InitVelocityConstraints(step);
}
// Solve velocity constraints.
for (int32 i = 0; i < step.velocityIterations; ++i)
{
for (int32 j = 0; j < m_jointCount; ++j)
{
m_joints[j]->SolveVelocityConstraints(step);
}
contactSolver.SolveVelocityConstraints();
}
// Post-solve (store impulses for warm starting).
contactSolver.StoreImpulses();
// Integrate positions.
for (int32 i = 0; i < m_bodyCount; ++i)
{
b2Body* b = m_bodies[i];
if (b->GetType() == b2_staticBody)
{
continue;
}
// Check for large velocities.
b2Vec2 translation = step.dt * b->m_linearVelocity;
if (b2Dot(translation, translation) > b2_maxTranslationSquared)
{
float32 ratio = b2_maxTranslation / translation.Length();
b->m_linearVelocity *= ratio;
}
float32 rotation = step.dt * b->m_angularVelocity;
if (rotation * rotation > b2_maxRotationSquared)
{
float32 ratio = b2_maxRotation / b2Abs(rotation);
b->m_angularVelocity *= ratio;
}
// Store positions for continuous collision.
b->m_sweep.c0 = b->m_sweep.c;
b->m_sweep.a0 = b->m_sweep.a;
// Integrate
b->m_sweep.c += step.dt * b->m_linearVelocity;
b->m_sweep.a += step.dt * b->m_angularVelocity;
// Compute new transform
b->SynchronizeTransform();
// Note: shapes are synchronized later.
}
// Iterate over constraints.
for (int32 i = 0; i < step.positionIterations; ++i)
{
bool contactsOkay = contactSolver.SolvePositionConstraints(b2_contactBaumgarte);
bool jointsOkay = true;
for (int32 i = 0; i < m_jointCount; ++i)
{
bool jointOkay = m_joints[i]->SolvePositionConstraints(b2_contactBaumgarte);
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
break;
}
}
Report(contactSolver.m_constraints);
if (allowSleep)
{
float32 minSleepTime = b2_maxFloat;
const float32 linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance;
const float32 angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance;
for (int32 i = 0; i < m_bodyCount; ++i)
{
b2Body* b = m_bodies[i];
if (b->GetType() == b2_staticBody)
{
continue;
}
if ((b->m_flags & b2Body::e_autoSleepFlag) == 0)
{
b->m_sleepTime = 0.0f;
minSleepTime = 0.0f;
}
if ((b->m_flags & b2Body::e_autoSleepFlag) == 0 ||
b->m_angularVelocity * b->m_angularVelocity > angTolSqr ||
b2Dot(b->m_linearVelocity, b->m_linearVelocity) > linTolSqr)
{
b->m_sleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b->m_sleepTime += step.dt;
minSleepTime = b2Min(minSleepTime, b->m_sleepTime);
}
}
if (minSleepTime >= b2_timeToSleep)
{
for (int32 i = 0; i < m_bodyCount; ++i)
{
b2Body* b = m_bodies[i];
b->SetAwake(false);
}
}
}
}
bool b2PrismaticJoint::SolvePositionConstraints(const b2SolverData& data)
{
b2Vec2 cA = data.positions[m_indexA].c;
float32 aA = data.positions[m_indexA].a;
b2Vec2 cB = data.positions[m_indexB].c;
float32 aB = data.positions[m_indexB].a;
b2Rot qA(aA), qB(aB);
float32 mA = m_invMassA, mB = m_invMassB;
float32 iA = m_invIA, iB = m_invIB;
// Compute fresh Jacobians
b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
b2Vec2 d = cB + rB - cA - rA;
b2Vec2 axis = b2Mul(qA, m_localXAxisA);
float32 a1 = b2Cross(d + rA, axis);
float32 a2 = b2Cross(rB, axis);
b2Vec2 perp = b2Mul(qA, m_localYAxisA);
float32 s1 = b2Cross(d + rA, perp);
float32 s2 = b2Cross(rB, perp);
b2Vec3 impulse;
b2Vec2 C1;
C1.x = b2Dot(perp, d);
C1.y = aB - aA - m_referenceAngle;
float32 linearError = b2Abs(C1.x);
float32 angularError = b2Abs(C1.y);
bool active = false;
float32 C2 = 0.0f;
if (m_enableLimit)
{
float32 translation = b2Dot(axis, d);
if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)
{
// Prevent large angular corrections
C2 = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection);
linearError = b2Max(linearError, b2Abs(translation));
active = true;
}
else if (translation <= m_lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = b2Clamp(translation - m_lowerTranslation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
linearError = b2Max(linearError, m_lowerTranslation - translation);
active = true;
}
else if (translation >= m_upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = b2Clamp(translation - m_upperTranslation - b2_linearSlop, 0.0f, b2_maxLinearCorrection);
linearError = b2Max(linearError, translation - m_upperTranslation);
active = true;
}
}
if (active)
{
float32 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
float32 k12 = iA * s1 + iB * s2;
float32 k13 = iA * s1 * a1 + iB * s2 * a2;
float32 k22 = iA + iB;
if (k22 == 0.0f)
{
// For fixed rotation
k22 = 1.0f;
}
float32 k23 = iA * a1 + iB * a2;
float32 k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;
b2Mat33 K;
K.ex.Set(k11, k12, k13);
K.ey.Set(k12, k22, k23);
K.ez.Set(k13, k23, k33);
b2Vec3 C;
C.x = C1.x;
C.y = C1.y;
C.z = C2;
impulse = K.Solve33(-C);
}
else
{
float32 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
float32 k12 = iA * s1 + iB * s2;
float32 k22 = iA + iB;
if (k22 == 0.0f)
{
k22 = 1.0f;
}
b2Mat22 K;
K.ex.Set(k11, k12);
K.ey.Set(k12, k22);
b2Vec2 impulse1 = K.Solve(-C1);
impulse.x = impulse1.x;
impulse.y = impulse1.y;
impulse.z = 0.0f;
}
b2Vec2 P = impulse.x * perp + impulse.z * axis;
float32 LA = impulse.x * s1 + impulse.y + impulse.z * a1;
float32 LB = impulse.x * s2 + impulse.y + impulse.z * a2;
cA -= mA * P;
aA -= iA * LA;
cB += mB * P;
aB += iB * LB;
data.positions[m_indexA].c = cA;
data.positions[m_indexA].a = aA;
data.positions[m_indexB].c = cB;
data.positions[m_indexB].a = aB;
return linearError <= b2_linearSlop && angularError <= b2_angularSlop;
}
void b2PrismaticJoint::SolveVelocityConstraints(const b2SolverData& data)
{
b2Vec2 vA = data.velocities[m_indexA].v;
float32 wA = data.velocities[m_indexA].w;
b2Vec2 vB = data.velocities[m_indexB].v;
float32 wB = data.velocities[m_indexB].w;
float32 mA = m_invMassA, mB = m_invMassB;
float32 iA = m_invIA, iB = m_invIB;
// Solve linear motor constraint.
if (m_enableMotor && m_limitState != e_equalLimits)
{
float32 Cdot = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA;
float32 impulse = m_motorMass * (m_motorSpeed - Cdot);
float32 oldImpulse = m_motorImpulse;
float32 maxImpulse = data.step.dt * m_maxMotorForce;
m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_motorImpulse - oldImpulse;
b2Vec2 P = impulse * m_axis;
float32 LA = impulse * m_a1;
float32 LB = impulse * m_a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
b2Vec2 Cdot1;
Cdot1.x = b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA;
Cdot1.y = wB - wA;
if (m_enableLimit && m_limitState != e_inactiveLimit)
{
// Solve prismatic and limit constraint in block form.
float32 Cdot2;
Cdot2 = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA;
b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
b2Vec3 f1 = m_impulse;
b2Vec3 df = m_K.Solve33(-Cdot);
m_impulse += df;
if (m_limitState == e_atLowerLimit)
{
m_impulse.z = b2Max(m_impulse.z, 0.0f);
}
else if (m_limitState == e_atUpperLimit)
{
m_impulse.z = b2Min(m_impulse.z, 0.0f);
}
// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
b2Vec2 b = -Cdot1 - (m_impulse.z - f1.z) * b2Vec2(m_K.ez.x, m_K.ez.y);
b2Vec2 f2r = m_K.Solve22(b) + b2Vec2(f1.x, f1.y);
m_impulse.x = f2r.x;
m_impulse.y = f2r.y;
df = m_impulse - f1;
b2Vec2 P = df.x * m_perp + df.z * m_axis;
float32 LA = df.x * m_s1 + df.y + df.z * m_a1;
float32 LB = df.x * m_s2 + df.y + df.z * m_a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
else
{
// Limit is inactive, just solve the prismatic constraint in block form.
b2Vec2 df = m_K.Solve22(-Cdot1);
m_impulse.x += df.x;
m_impulse.y += df.y;
b2Vec2 P = df.x * m_perp;
float32 LA = df.x * m_s1 + df.y;
float32 LB = df.x * m_s2 + df.y;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
b2Vec2 Cdot10 = Cdot1;
Cdot1.x = b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA;
Cdot1.y = wB - wA;
if (b2Abs(Cdot1.x) > 0.01f || b2Abs(Cdot1.y) > 0.01f)
{
b2Vec2 test = b2Mul22(m_K, df);
Cdot1.x += 0.0f;
}
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
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);
}
bool b2LineJoint::SolvePositionConstraints(float32 baumgarte)
{
B2_NOT_USED(baumgarte);
b2Body* b1 = m_bodyA;
b2Body* b2 = m_bodyB;
b2Vec2 c1 = b1->m_sweep.c;
float32 a1 = b1->m_sweep.a;
b2Vec2 c2 = b2->m_sweep.c;
float32 a2 = b2->m_sweep.a;
// Solve linear limit constraint.
float32 linearError = 0.0f, angularError = 0.0f;
bool active = false;
float32 C2 = 0.0f;
b2Mat22 R1(a1), R2(a2);
b2Vec2 r1 = b2Mul(R1, m_localAnchor1 - m_localCenterA);
b2Vec2 r2 = b2Mul(R2, m_localAnchor2 - m_localCenterB);
b2Vec2 d = c2 + r2 - c1 - r1;
if (m_enableLimit)
{
m_axis = b2Mul(R1, m_localXAxis1);
m_a1 = b2Cross(d + r1, m_axis);
m_a2 = b2Cross(r2, m_axis);
float32 translation = b2Dot(m_axis, d);
if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)
{
// Prevent large angular corrections
C2 = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection);
linearError = b2Abs(translation);
active = true;
}
else if (translation <= m_lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = b2Clamp(translation - m_lowerTranslation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
linearError = m_lowerTranslation - translation;
active = true;
}
else if (translation >= m_upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = b2Clamp(translation - m_upperTranslation - b2_linearSlop, 0.0f, b2_maxLinearCorrection);
linearError = translation - m_upperTranslation;
active = true;
}
}
m_perp = b2Mul(R1, m_localYAxis1);
m_s1 = b2Cross(d + r1, m_perp);
m_s2 = b2Cross(r2, m_perp);
b2Vec2 impulse;
float32 C1;
C1 = b2Dot(m_perp, d);
linearError = b2Max(linearError, b2Abs(C1));
angularError = 0.0f;
if (active)
{
float32 m1 = m_invMassA, m2 = m_invMassB;
float32 i1 = m_invIA, i2 = m_invIB;
float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
float32 k12 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2;
float32 k22 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2;
m_K.col1.Set(k11, k12);
m_K.col2.Set(k12, k22);
b2Vec2 C;
C.x = C1;
C.y = C2;
impulse = m_K.Solve(-C);
}
else
{
float32 m1 = m_invMassA, m2 = m_invMassB;
float32 i1 = m_invIA, i2 = m_invIB;
float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
float32 impulse1;
if (k11 != 0.0f)
{
impulse1 = - C1 / k11;
}
else
{
impulse1 = 0.0f;
}
impulse.x = impulse1;
impulse.y = 0.0f;
}
b2Vec2 P = impulse.x * m_perp + impulse.y * m_axis;
float32 L1 = impulse.x * m_s1 + impulse.y * m_a1;
float32 L2 = impulse.x * m_s2 + impulse.y * m_a2;
c1 -= m_invMassA * P;
a1 -= m_invIA * L1;
c2 += m_invMassB * P;
a2 += m_invIB * L2;
// TODO_ERIN remove need for this.
b1->m_sweep.c = c1;
b1->m_sweep.a = a1;
b2->m_sweep.c = c2;
b2->m_sweep.a = a2;
b1->SynchronizeTransform();
b2->SynchronizeTransform();
return linearError <= b2_linearSlop && angularError <= b2_angularSlop;
}
void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep)
{
b2Timer timer;
float32 h = step.dt;
// Integrate velocities and apply damping. Initialize the body state.
for (int32 i = 0; i < m_bodyCount; ++i)
{
b2Body* b = m_bodies[i];
b2Vec2 c = b->m_sweep.c;
float32 a = b->m_sweep.a;
b2Vec2 v = b->m_linearVelocity;
float32 w = b->m_angularVelocity;
// Store positions for continuous collision.
b->m_sweep.c0 = b->m_sweep.c;
b->m_sweep.a0 = b->m_sweep.a;
if (b->m_type == b2_dynamicBody)
{
// Integrate velocities.
v += h * (b->m_gravityScale * gravity + b->m_invMass * b->m_force);
w += h * b->m_invI * b->m_torque;
// Apply damping.
// ODE: dv/dt + c * v = 0
// Solution: v(t) = v0 * exp(-c * t)
// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
// v2 = exp(-c * dt) * v1
// Taylor expansion:
// v2 = (1.0f - c * dt) * v1
v *= b2Clamp(1.0f - h * b->m_linearDamping, 0.0f, 1.0f);
w *= b2Clamp(1.0f - h * b->m_angularDamping, 0.0f, 1.0f);
}
m_positions[i].c = c;
m_positions[i].a = a;
m_velocities[i].v = v;
m_velocities[i].w = w;
}
timer.Reset();
// Solver data
b2SolverData solverData;
solverData.step = step;
solverData.positions = m_positions;
solverData.velocities = m_velocities;
// Initialize velocity constraints.
b2ContactSolverDef contactSolverDef;
contactSolverDef.step = step;
contactSolverDef.contacts = m_contacts;
contactSolverDef.count = m_contactCount;
contactSolverDef.positions = m_positions;
contactSolverDef.velocities = m_velocities;
contactSolverDef.allocator = m_allocator;
b2ContactSolver contactSolver(&contactSolverDef);
contactSolver.InitializeVelocityConstraints();
if (step.warmStarting)
{
contactSolver.WarmStart();
}
for (int32 i = 0; i < m_jointCount; ++i)
{
m_joints[i]->InitVelocityConstraints(solverData);
}
profile->solveInit = timer.GetMilliseconds();
// Solve velocity constraints
timer.Reset();
for (int32 i = 0; i < step.velocityIterations; ++i)
{
for (int32 j = 0; j < m_jointCount; ++j)
{
m_joints[j]->SolveVelocityConstraints(solverData);
}
contactSolver.SolveVelocityConstraints();
}
// Store impulses for warm starting
contactSolver.StoreImpulses();
profile->solveVelocity = timer.GetMilliseconds();
// Integrate positions
for (int32 i = 0; i < m_bodyCount; ++i)
{
b2Vec2 c = m_positions[i].c;
float32 a = m_positions[i].a;
b2Vec2 v = m_velocities[i].v;
float32 w = m_velocities[i].w;
// Check for large velocities
b2Vec2 translation = h * v;
if (b2Dot(translation, translation) > b2_maxTranslationSquared)
{
float32 ratio = b2_maxTranslation / translation.Length();
v *= ratio;
}
float32 rotation = h * w;
if (rotation * rotation > b2_maxRotationSquared)
{
float32 ratio = b2_maxRotation / b2Abs(rotation);
w *= ratio;
}
// Integrate
c += h * v;
a += h * w;
m_positions[i].c = c;
m_positions[i].a = a;
m_velocities[i].v = v;
m_velocities[i].w = w;
}
// Solve position constraints
timer.Reset();
bool positionSolved = false;
for (int32 i = 0; i < step.positionIterations; ++i)
{
bool contactsOkay = contactSolver.SolvePositionConstraints();
bool jointsOkay = true;
for (int32 i = 0; i < m_jointCount; ++i)
{
bool jointOkay = m_joints[i]->SolvePositionConstraints(solverData);
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
positionSolved = true;
break;
}
}
// Copy state buffers back to the bodies
for (int32 i = 0; i < m_bodyCount; ++i)
{
b2Body* body = m_bodies[i];
body->m_sweep.c = m_positions[i].c;
body->m_sweep.a = m_positions[i].a;
body->m_linearVelocity = m_velocities[i].v;
body->m_angularVelocity = m_velocities[i].w;
body->SynchronizeTransform();
}
profile->solvePosition = timer.GetMilliseconds();
Report(contactSolver.m_velocityConstraints);
if (allowSleep)
{
float32 minSleepTime = b2_maxFloat;
const float32 linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance;
const float32 angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance;
for (int32 i = 0; i < m_bodyCount; ++i)
{
b2Body* b = m_bodies[i];
if (b->GetType() == b2_staticBody)
{
continue;
}
if ((b->m_flags & b2Body::e_autoSleepFlag) == 0 ||
b->m_angularVelocity * b->m_angularVelocity > angTolSqr ||
b2Dot(b->m_linearVelocity, b->m_linearVelocity) > linTolSqr)
{
b->m_sleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b->m_sleepTime += h;
minSleepTime = b2Min(minSleepTime, b->m_sleepTime);
}
}
if (minSleepTime >= b2_timeToSleep && positionSolved)
{
for (int32 i = 0; i < m_bodyCount; ++i)
{
b2Body* b = m_bodies[i];
b->SetAwake(false);
}
}
}
}
// 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);
#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);
}
bool b2WeldJoint::SolvePositionConstraints(const b2SolverData& data)
{
b2Vec2 cA = data.positions[m_indexA].c;
float32 aA = data.positions[m_indexA].a;
b2Vec2 cB = data.positions[m_indexB].c;
float32 aB = data.positions[m_indexB].a;
b2Rot qA(aA), qB(aB);
float32 mA = m_invMassA, mB = m_invMassB;
float32 iA = m_invIA, iB = m_invIB;
b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
float32 positionError, angularError;
b2Mat33 K;
K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
K.ez.x = -rA.y * iA - rB.y * iB;
K.ex.y = K.ey.x;
K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
K.ez.y = rA.x * iA + rB.x * iB;
K.ex.z = K.ez.x;
K.ey.z = K.ez.y;
K.ez.z = iA + iB;
if (m_frequencyHz > 0.0f)
{
b2Vec2 C1 = cB + rB - cA - rA;
positionError = C1.Length();
angularError = 0.0f;
b2Vec2 P = -K.Solve22(C1);
cA -= mA * P;
aA -= iA * b2Cross(rA, P);
cB += mB * P;
aB += iB * b2Cross(rB, P);
}
else
{
b2Vec2 C1 = cB + rB - cA - rA;
float32 C2 = aB - aA - m_referenceAngle;
positionError = C1.Length();
angularError = b2Abs(C2);
b2Vec3 C(C1.x, C1.y, C2);
b2Vec3 impulse = -K.Solve33(C);
b2Vec2 P(impulse.x, impulse.y);
cA -= mA * P;
aA -= iA * (b2Cross(rA, P) + impulse.z);
cB += mB * P;
aB += iB * (b2Cross(rB, P) + impulse.z);
}
data.positions[m_indexA].c = cA;
data.positions[m_indexA].a = aA;
data.positions[m_indexB].c = cB;
data.positions[m_indexB].a = aB;
return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
}
void b2PrismaticJoint::InitVelocityConstraints(const b2SolverData& data)
{
m_indexA = m_bodyA->m_islandIndex;
m_indexB = m_bodyB->m_islandIndex;
m_localCenterA = m_bodyA->m_sweep.localCenter;
m_localCenterB = m_bodyB->m_sweep.localCenter;
m_invMassA = m_bodyA->m_invMass;
m_invMassB = m_bodyB->m_invMass;
m_invIA = m_bodyA->m_invI;
m_invIB = m_bodyB->m_invI;
b2Vec2 cA = data.positions[m_indexA].c;
float32 aA = data.positions[m_indexA].a;
b2Vec2 vA = data.velocities[m_indexA].v;
float32 wA = data.velocities[m_indexA].w;
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 qA(aA), qB(aB);
// Compute the effective masses.
b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
b2Vec2 d = (cB - cA) + rB - rA;
float32 mA = m_invMassA, mB = m_invMassB;
float32 iA = m_invIA, iB = m_invIB;
// Compute motor Jacobian and effective mass.
{
m_axis = b2Mul(qA, m_localXAxisA);
m_a1 = b2Cross(d + rA, m_axis);
m_a2 = b2Cross(rB, m_axis);
m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
if (m_motorMass > 0.0f)
{
m_motorMass = 1.0f / m_motorMass;
}
}
// Prismatic constraint.
{
m_perp = b2Mul(qA, m_localYAxisA);
m_s1 = b2Cross(d + rA, m_perp);
m_s2 = b2Cross(rB, m_perp);
float32 k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2;
float32 k12 = iA * m_s1 + iB * m_s2;
float32 k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2;
float32 k22 = iA + iB;
if (k22 == 0.0f)
{
// For bodies with fixed rotation.
k22 = 1.0f;
}
float32 k23 = iA * m_a1 + iB * m_a2;
float32 k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
m_K.ex.Set(k11, k12, k13);
m_K.ey.Set(k12, k22, k23);
m_K.ez.Set(k13, k23, k33);
}
// Compute motor and limit terms.
if (m_enableLimit)
{
float32 jointTranslation = b2Dot(m_axis, d);
if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)
{
m_limitState = e_equalLimits;
}
else if (jointTranslation <= m_lowerTranslation)
{
if (m_limitState != e_atLowerLimit)
{
m_limitState = e_atLowerLimit;
m_impulse.z = 0.0f;
}
}
else if (jointTranslation >= m_upperTranslation)
{
if (m_limitState != e_atUpperLimit)
{
m_limitState = e_atUpperLimit;
m_impulse.z = 0.0f;
}
}
else
{
m_limitState = e_inactiveLimit;
m_impulse.z = 0.0f;
}
}
else
{
m_limitState = e_inactiveLimit;
m_impulse.z = 0.0f;
}
if (m_enableMotor == false)
{
m_motorImpulse = 0.0f;
}
if (data.step.warmStarting)
{
// Account for variable time step.
m_impulse *= data.step.dtRatio;
m_motorImpulse *= data.step.dtRatio;
b2Vec2 P = m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis;
float32 LA = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1;
float32 LB = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
else
{
m_impulse.SetZero();
m_motorImpulse = 0.0f;
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}