bool b2RopeJoint::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 u = cB + rB - cA - rA;
float32 length = u.Normalize();
float32 C = length - m_maxLength;
C = b2Clamp(C, 0.0f, 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 length - m_maxLength < b2_linearSlop;
}
bool b2RopeJoint::SolvePositionConstraints(qreal baumgarte)
{
B2_NOT_USED(baumgarte);
b2Body* bA = m_bodyA;
b2Body* bB = m_bodyB;
b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
b2Vec2 u = bB->m_sweep.c + rB - bA->m_sweep.c - rA;
qreal length = u.Normalize();
qreal C = length - m_maxLength;
C = b2Clamp(C, 0.0f, b2_maxLinearCorrection);
qreal impulse = -m_mass * C;
b2Vec2 P = impulse * u;
bA->m_sweep.c -= bA->m_invMass * P;
bA->m_sweep.a -= bA->m_invI * b2Cross(rA, P);
bB->m_sweep.c += bB->m_invMass * P;
bB->m_sweep.a += bB->m_invI * b2Cross(rB, P);
bA->SynchronizeTransform();
bB->SynchronizeTransform();
return length - m_maxLength < b2_linearSlop;
}
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;
}
// Sequential solver.
bool b2ContactSolver::SolvePositionConstraints(float32 baumgarte)
{
float32 minSeparation = 0.0f;
for (int32 i = 0; i < m_constraintCount; ++i)
{
b2ContactConstraint* c = m_constraints + i;
b2Body* bodyA = c->bodyA;
b2Body* bodyB = c->bodyB;
float32 invMassA = bodyA->m_mass * bodyA->m_invMass;
float32 invIA = bodyA->m_mass * bodyA->m_invI;
float32 invMassB = bodyB->m_mass * bodyB->m_invMass;
float32 invIB = bodyB->m_mass * bodyB->m_invI;
// Solve normal constraints
for (int32 j = 0; j < c->pointCount; ++j)
{
b2PositionSolverManifold psm;
psm.Initialize(c, j);
b2Vec2 normal = psm.normal;
b2Vec2 point = psm.point;
float32 separation = psm.separation;
b2Vec2 rA = point - bodyA->m_sweep.c;
b2Vec2 rB = point - bodyB->m_sweep.c;
// Track max constraint error.
minSeparation = b2Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float32 C = b2Clamp(baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);
// Compute the effective mass.
float32 rnA = b2Cross(rA, normal);
float32 rnB = b2Cross(rB, normal);
float32 K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;
// Compute normal impulse
float32 impulse = K > 0.0f ? - C / K : 0.0f;
b2Vec2 P = impulse * normal;
bodyA->m_sweep.c -= invMassA * P;
bodyA->m_sweep.a -= invIA * b2Cross(rA, P);
bodyA->SynchronizeTransform();
bodyB->m_sweep.c += invMassB * P;
bodyB->m_sweep.a += invIB * b2Cross(rB, P);
bodyB->SynchronizeTransform();
}
}
// We can't expect minSpeparation >= -b2_linearSlop because we don't
// push the separation above -b2_linearSlop.
return minSeparation >= -1.5f * b2_linearSlop;
}
void b2MotorJoint::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;
float32 h = data.step.dt;
float32 inv_h = data.step.inv_dt;
// Solve angular friction
{
float32 Cdot = wB - wA + inv_h * m_correctionFactor * m_angularError;
float32 impulse = -m_angularMass * Cdot;
float32 oldImpulse = m_angularImpulse;
float32 maxImpulse = h * m_maxTorque;
m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_angularImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve linear friction
{
b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA) + inv_h * m_correctionFactor * m_linearError;
b2Vec2 impulse = -b2Mul(m_linearMass, Cdot);
b2Vec2 oldImpulse = m_linearImpulse;
m_linearImpulse += impulse;
float32 maxImpulse = h * m_maxForce;
if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
{
m_linearImpulse.Normalize();
m_linearImpulse *= maxImpulse;
}
impulse = m_linearImpulse - oldImpulse;
vA -= mA * impulse;
wA -= iA * b2Cross(m_rA, impulse);
vB += mB * impulse;
wB += iB * b2Cross(m_rB, impulse);
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
// Sequential solver.
bool b2ContactSolver::SolvePositionConstraints(float32 baumgarte)
{
float32 minSeparation = 0.0f;
for (int32 i = 0; i < m_constraintCount; ++i)
{
b2ContactConstraint* c = m_constraints + i;
b2Body* b1 = c->body1;
b2Body* b2 = c->body2;
float32 invMass1 = b1->m_mass * b1->m_invMass;
float32 invI1 = b1->m_mass * b1->m_invI;
float32 invMass2 = b2->m_mass * b2->m_invMass;
float32 invI2 = b2->m_mass * b2->m_invI;
b2Vec2 normal = c->normal;
// Solver normal constraints
for (int32 j = 0; j < c->pointCount; ++j)
{
b2ContactConstraintPoint* ccp = c->points + j;
b2Vec2 r1 = b2Mul(b1->GetXForm().R, ccp->localAnchor1 - b1->GetLocalCenter());
b2Vec2 r2 = b2Mul(b2->GetXForm().R, ccp->localAnchor2 - b2->GetLocalCenter());
b2Vec2 p1 = b1->m_sweep.c + r1;
b2Vec2 p2 = b2->m_sweep.c + r2;
b2Vec2 dp = p2 - p1;
// Approximate the current separation.
float32 separation = b2Dot(dp, normal) + ccp->separation;
// Track max constraint error.
minSeparation = b2Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float32 C = baumgarte * b2Clamp(separation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
// Compute normal impulse
float32 impulse = -ccp->equalizedMass * C;
b2Vec2 P = impulse * normal;
b1->m_sweep.c -= invMass1 * P;
b1->m_sweep.a -= invI1 * b2Cross(r1, P);
b1->SynchronizeTransform();
b2->m_sweep.c += invMass2 * P;
b2->m_sweep.a += invI2 * b2Cross(r2, P);
b2->SynchronizeTransform();
}
}
// We can't expect minSpeparation >= -b2_linearSlop because we don't
// push the separation above -b2_linearSlop.
return minSeparation >= -1.5f * b2_linearSlop;
}
int32 b2CalculateParticleIterations(
float32 gravity, float32 radius, float32 timeStep)
{
// In some situations you may want more particle iterations than this,
// but to avoid excessive cycle cost, don't recommend more than this.
const int32 B2_MAX_RECOMMENDED_PARTICLE_ITERATIONS = 8;
const float32 B2_RADIUS_THRESHOLD = 0.01f;
int32 iterations =
(int32) ceilf(b2Sqrt(gravity / (B2_RADIUS_THRESHOLD * radius)) * timeStep);
return b2Clamp(iterations, 1, B2_MAX_RECOMMENDED_PARTICLE_ITERATIONS);
}
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;
}
inline void MoveAABB(b2AABB* aabb)
{
b2Vec2 d;
d.x = b2Random(-0.5f, 0.5f);
d.y = b2Random(-0.5f, 0.5f);
//d.x = 2.0f;
//d.y = 0.0f;
aabb->lowerBound += d;
aabb->upperBound += d;
b2Vec2 c0 = 0.5f * (aabb->lowerBound + aabb->upperBound);
b2Vec2 min; min.Set(-k_extent, 0.0f);
b2Vec2 max; max.Set(k_extent, 2.0f * k_extent);
b2Vec2 c = b2Clamp(c0, min, max);
aabb->lowerBound += c - c0;
aabb->upperBound += c - c0;
}
void Box2DVehicleBody::update(float delta, float throtle, float desiredAngle) {
tireFrontLeft->updateFriction();
tireFrontRight->updateFriction();
tireRearLeft->updateFriction();
tireRearRight->updateFriction();
tireFrontLeft->updateDrive(throtle);
tireFrontRight->updateDrive(throtle);
tireRearLeft->updateDrive(throtle);
tireRearRight->updateDrive(throtle);
//control steering
float turnSpeedPerSec = 160 * DEGTORAD;//from lock to lock in 0.5 sec
float turnPerTimeStep = turnSpeedPerSec * delta;
float angleNow = flJoint->GetJointAngle();
float angleToTurn = desiredAngle - angleNow;
angleToTurn = b2Clamp( angleToTurn, -turnPerTimeStep, turnPerTimeStep );
float newAngle = angleNow + angleToTurn;
flJoint->SetLimits( newAngle, newAngle );
frJoint->SetLimits( newAngle, newAngle );
}
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;
}
// Sequential position solver for position constraints.
bool b2ContactSolver::SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB)
{
float32 minSeparation = 0.0f;
for (int32 i = 0; i < m_count; ++i)
{
b2ContactPositionConstraint* pc = m_positionConstraints + i;
int32 indexA = pc->indexA;
int32 indexB = pc->indexB;
b2Vec2 localCenterA = pc->localCenterA;
b2Vec2 localCenterB = pc->localCenterB;
int32 pointCount = pc->pointCount;
float32 mA = 0.0f;
float32 iA = 0.0f;
if (indexA == toiIndexA || indexA == toiIndexB)
{
mA = pc->invMassA;
iA = pc->invIA;
}
float32 mB = 0.0f;
float32 iB = 0.;
if (indexB == toiIndexA || indexB == toiIndexB)
{
mB = pc->invMassB;
iB = pc->invIB;
}
b2Vec2 cA = m_positions[indexA].c;
float32 aA = m_positions[indexA].a;
b2Vec2 cB = m_positions[indexB].c;
float32 aB = m_positions[indexB].a;
// Solve normal constraints
for (int32 j = 0; j < pointCount; ++j)
{
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);
b2PositionSolverManifold psm;
psm.Initialize(pc, xfA, xfB, j);
b2Vec2 normal = psm.normal;
b2Vec2 point = psm.point;
float32 separation = psm.separation;
b2Vec2 rA = point - cA;
b2Vec2 rB = point - cB;
// Track max constraint error.
minSeparation = b2Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float32 C = b2Clamp(b2_toiBaugarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);
// Compute the effective mass.
float32 rnA = b2Cross(rA, normal);
float32 rnB = b2Cross(rB, normal);
float32 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
// Compute normal impulse
float32 impulse = K > 0.0f ? - C / K : 0.0f;
b2Vec2 P = impulse * normal;
cA -= mA * P;
aA -= iA * b2Cross(rA, P);
cB += mB * P;
aB += iB * b2Cross(rB, P);
}
m_positions[indexA].c = cA;
m_positions[indexA].a = aA;
m_positions[indexB].c = cB;
m_positions[indexB].a = aB;
}
// We can't expect minSpeparation >= -b2_linearSlop because we don't
// push the separation above -b2_linearSlop.
return minSeparation >= -1.5f * 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);
}
}
}
}
void b2PrismaticJoint::SolveVelocityConstraints(const b2TimeStep& step)
{
b2Body* b1 = m_bodyA;
b2Body* b2 = m_bodyB;
b2Vec2 v1 = b1->m_linearVelocity;
float32 w1 = b1->m_angularVelocity;
b2Vec2 v2 = b2->m_linearVelocity;
float32 w2 = b2->m_angularVelocity;
// Solve linear motor constraint.
if (m_enableMotor && m_limitState != e_equalLimits)
{
float32 Cdot = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1;
float32 impulse = m_motorMass * (m_motorSpeed - Cdot);
float32 oldImpulse = m_motorImpulse;
float32 maxImpulse = step.dt * m_maxMotorForce;
m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_motorImpulse - oldImpulse;
b2Vec2 P = impulse * m_axis;
float32 L1 = impulse * m_a1;
float32 L2 = impulse * m_a2;
v1 -= m_invMassA * P;
w1 -= m_invIA * L1;
v2 += m_invMassB * P;
w2 += m_invIB * L2;
}
b2Vec2 Cdot1;
Cdot1.x = b2Dot(m_perp, v2 - v1) + m_s2 * w2 - m_s1 * w1;
Cdot1.y = w2 - w1;
if (m_enableLimit && m_limitState != e_inactiveLimit)
{
// Solve prismatic and limit constraint in block form.
float32 Cdot2;
Cdot2 = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1;
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.col3.x, m_K.col3.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 L1 = df.x * m_s1 + df.y + df.z * m_a1;
float32 L2 = df.x * m_s2 + df.y + df.z * m_a2;
v1 -= m_invMassA * P;
w1 -= m_invIA * L1;
v2 += m_invMassB * P;
w2 += m_invIB * L2;
}
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 L1 = df.x * m_s1 + df.y;
float32 L2 = df.x * m_s2 + df.y;
v1 -= m_invMassA * P;
w1 -= m_invIA * L1;
v2 += m_invMassB * P;
w2 += m_invIB * L2;
}
b1->m_linearVelocity = v1;
b1->m_angularVelocity = w1;
b2->m_linearVelocity = v2;
b2->m_angularVelocity = w2;
}
void b2WheelJoint::SolveVelocityConstraints(const b2TimeStep& step)
{
b2Body* bA = m_bodyA;
b2Body* bB = m_bodyB;
b2Vec2 vA = bA->m_linearVelocity;
float32 wA = bA->m_angularVelocity;
b2Vec2 vB = bB->m_linearVelocity;
float32 wB = bB->m_angularVelocity;
// Solve spring constraint
{
float32 Cdot = b2Dot(m_ax, vB - vA) + m_sBx * wB - m_sAx * wA;
float32 impulse = -m_springMass * (Cdot + m_bias + m_gamma * m_springImpulse);
m_springImpulse += impulse;
b2Vec2 P = impulse * m_ax;
float32 LA = impulse * m_sAx;
float32 LB = impulse * m_sBx;
vA -= m_invMassA * P;
wA -= m_invIA * LA;
vB += m_invMassB * P;
wB += m_invIB * LB;
}
// Solve rotational motor constraint
{
float32 Cdot = wB - wA - m_motorSpeed;
float32 impulse = -m_motorMass * Cdot;
float32 oldImpulse = m_motorImpulse;
float32 maxImpulse = step.dt * m_maxMotorTorque;
m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_motorImpulse - oldImpulse;
wA -= m_invIA * impulse;
wB += m_invIB * impulse;
}
// Solve point to line constraint
{
float32 Cdot = b2Dot(m_ay, vB - vA) + m_sBy * wB - m_sAy * wA;
float32 impulse = m_mass * (-Cdot);
m_impulse += impulse;
b2Vec2 P = impulse * m_ay;
float32 LA = impulse * m_sAy;
float32 LB = impulse * m_sBy;
vA -= m_invMassA * P;
wA -= m_invIA * LA;
vB += m_invMassB * P;
wB += m_invIB * LB;
}
bA->m_linearVelocity = vA;
bA->m_angularVelocity = wA;
bB->m_linearVelocity = vB;
bB->m_angularVelocity = wB;
}
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;
}
bool b2PulleyJoint::SolvePositionConstraints(float32 baumgarte)
{
B2_NOT_USED(baumgarte);
b2Body* b1 = m_bodyA;
b2Body* b2 = m_bodyB;
b2Vec2 s1 = m_groundAnchor1;
b2Vec2 s2 = m_groundAnchor2;
float32 linearError = 0.0f;
if (m_state == e_atUpperLimit)
{
b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
b2Vec2 p1 = b1->m_sweep.c + r1;
b2Vec2 p2 = b2->m_sweep.c + r2;
// 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;
linearError = b2Max(linearError, -C);
C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
float32 impulse = -m_pulleyMass * C;
b2Vec2 P1 = -impulse * m_u1;
b2Vec2 P2 = -m_ratio * impulse * m_u2;
b1->m_sweep.c += b1->m_invMass * P1;
b1->m_sweep.a += b1->m_invI * b2Cross(r1, P1);
b2->m_sweep.c += b2->m_invMass * P2;
b2->m_sweep.a += b2->m_invI * b2Cross(r2, P2);
b1->SynchronizeTransform();
b2->SynchronizeTransform();
}
if (m_limitState1 == e_atUpperLimit)
{
b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
b2Vec2 p1 = b1->m_sweep.c + r1;
m_u1 = p1 - s1;
float32 length1 = m_u1.Length();
if (length1 > b2_linearSlop)
{
m_u1 *= 1.0f / length1;
}
else
{
m_u1.SetZero();
}
float32 C = m_maxLength1 - length1;
linearError = b2Max(linearError, -C);
C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
float32 impulse = -m_limitMass1 * C;
b2Vec2 P1 = -impulse * m_u1;
b1->m_sweep.c += b1->m_invMass * P1;
b1->m_sweep.a += b1->m_invI * b2Cross(r1, P1);
b1->SynchronizeTransform();
}
if (m_limitState2 == e_atUpperLimit)
{
b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
b2Vec2 p2 = b2->m_sweep.c + r2;
m_u2 = p2 - s2;
float32 length2 = m_u2.Length();
if (length2 > b2_linearSlop)
{
m_u2 *= 1.0f / length2;
}
else
{
m_u2.SetZero();
}
float32 C = m_maxLength2 - length2;
linearError = b2Max(linearError, -C);
C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
float32 impulse = -m_limitMass2 * C;
b2Vec2 P2 = -impulse * m_u2;
b2->m_sweep.c += b2->m_invMass * P2;
b2->m_sweep.a += b2->m_invI * b2Cross(r2, P2);
b2->SynchronizeTransform();
}
return linearError < b2_linearSlop;
}
int main(int argc, char** argv)
{
// Place a communicator at serialport (Default COM1)
communicator = new Communicator(serialport);
// Hide joints
settings.drawJoints=0;
// Set startup view position
settings.viewCenter=b2Vec2(35,16);
simulatorPageCount = 0;
while (g_simulatorPageEntries[simulatorPageCount].createFcn != NULL)
{
++simulatorPageCount;
}
simulatorPageIndex = b2Clamp(simulatorPageIndex, 0, simulatorPageCount-1);
simulatorPageSelection = simulatorPageIndex;
entry = g_simulatorPageEntries + simulatorPageIndex;
simulatorPage = entry->createFcn(communicator);
glutInit(&argc, argv);
glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);
glutInitWindowSize(width, height);
char title[32];
sprintf(title, "Educational simulator for control-system development");
mainWindow = glutCreateWindow(title);
//glutSetOption (GLUT_ACTION_ON_WINDOW_CLOSE, GLUT_ACTION_GLUTMAINLOOP_RETURNS);
glutDisplayFunc(SimulationLoop);
GLUI_Master.set_glutReshapeFunc(Resize);
GLUI_Master.set_glutKeyboardFunc(Keyboard);
GLUI_Master.set_glutSpecialFunc(KeyboardSpecial);
GLUI_Master.set_glutMouseFunc(Mouse);
#ifdef FREEGLUT
glutMouseWheelFunc(MouseWheel);
#endif
glutMotionFunc(MouseMotion);
glutKeyboardUpFunc(KeyboardUp);
glui = GLUI_Master.create_glui_subwindow( mainWindow,
GLUI_SUBWINDOW_RIGHT );
glui->add_statictext("Simulator-page:");
GLUI_Listbox* simulatorPageList =
glui->add_listbox("", &simulatorPageSelection);
int32 simulatorPageCount = 0;
SimulatorPageEntry* e = g_simulatorPageEntries;
while (e->createFcn)
{
simulatorPageList->add_item(simulatorPageCount, e->name);
++simulatorPageCount;
++e;
}
glui->add_separator();
glui->add_edittext( "Serialport:", GLUI_EDITTEXT_TEXT, serialport );
glui->add_button("Apply", 0, (GLUI_Update_CB)SetSerialPort);
glui->add_separator();
glui->add_button("Pause", 0, Pause);
glui->add_button("Single Step", 0, SingleStep);
glui->add_button("Restart", 0, Restart);
glui->add_button("Quit", 0,(GLUI_Update_CB)Exit);
glui->add_separator();
glui->add_button("Help", 0,(GLUI_Update_CB)Help);
glui->add_button("About", 0,(GLUI_Update_CB)About);
glui->set_main_gfx_window( mainWindow );
// Use a timer to control the frame rate.
glutTimerFunc(framePeriod, Timer, 0);
glutMainLoop();
return 0;
}
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 convspeed1 = c->fixtureA->GetConveyorSpeed();
float32 convspeed2 = c->fixtureB->GetConveyorSpeed();
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);
/// Conveyor
int flip = (c->manifold->type == b2Manifold::e_faceB ? -1 : 1);
vt += convspeed1 * flip;
vt -= convspeed2 * flip;
/// END Conveyor
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: 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 = 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;
}
}
void b2WheelJoint::SolveVelocityConstraints(const b2SolverData& data) {
float32 mA = m_invMassA, mB = m_invMassB;
float32 iA = m_invIA, iB = m_invIB;
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;
// Solve spring constraint
{
float32 Cdot = b2Dot(m_ax, vB - vA) + m_sBx * wB - m_sAx * wA;
float32 impulse = -m_springMass
* (Cdot + m_bias + m_gamma * m_springImpulse);
m_springImpulse += impulse;
b2Vec2 P = impulse * m_ax;
float32 LA = impulse * m_sAx;
float32 LB = impulse * m_sBx;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
// Solve rotational motor constraint
{
float32 Cdot = wB - wA - m_motorSpeed;
float32 impulse = -m_motorMass * Cdot;
float32 oldImpulse = m_motorImpulse;
float32 maxImpulse = data.step.dt * m_maxMotorTorque;
m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse,
maxImpulse);
impulse = m_motorImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve point to line constraint
{
float32 Cdot = b2Dot(m_ay, vB - vA) + m_sBy * wB - m_sAy * wA;
float32 impulse = -m_mass * Cdot;
m_impulse += impulse;
b2Vec2 P = impulse * m_ay;
float32 LA = impulse * m_sAy;
float32 LB = impulse * m_sBy;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
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 b2ContactSolver::SolveVelocityConstraints()
{
for (int32 i = 0; i < m_constraintCount; ++i)
{
b2ContactConstraint* c = m_constraints + i;
b2Body* b1 = c->body1;
b2Body* b2 = c->body2;
float32 invMass1 = b1->m_invMass;
float32 invI1 = b1->m_invI;
float32 invMass2 = b2->m_invMass;
float32 invI2 = b2->m_invI;
b2Vec2 normal = c->normal;
b2Vec2 tangent = b2Cross(normal, 1.0f);
// Solve normal constraints
for (int32 j = 0; j < c->pointCount; ++j)
{
b2ContactConstraintPoint* ccp = c->points + j;
b2Vec2 r1 = b2Mul(b1->m_xf.R, ccp->localAnchor1 - b1->GetLocalCenter());
b2Vec2 r2 = b2Mul(b2->m_xf.R, ccp->localAnchor2 - b2->GetLocalCenter());
// Relative velocity at contact
b2Vec2 dv = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2) - b1->m_linearVelocity - b2Cross(b1->m_angularVelocity, r1);
// Compute normal force
float32 vn = b2Dot(dv, normal);
float32 lambda = - m_step.inv_dt * ccp->normalMass * (vn - ccp->velocityBias);
// b2Clamp the accumulated force
float32 newForce = b2Max(ccp->normalForce + lambda, 0.0f);
lambda = newForce - ccp->normalForce;
// Apply contact impulse
b2Vec2 P = m_step.dt * lambda * normal;
b1->m_linearVelocity -= invMass1 * P;
b1->m_angularVelocity -= invI1 * b2Cross(r1, P);
b2->m_linearVelocity += invMass2 * P;
b2->m_angularVelocity += invI2 * b2Cross(r2, P);
ccp->normalForce = newForce;
}
// Solve tangent constraints
for (int32 j = 0; j < c->pointCount; ++j)
{
b2ContactConstraintPoint* ccp = c->points + j;
b2Vec2 r1 = b2Mul(b1->m_xf.R, ccp->localAnchor1 - b1->GetLocalCenter());
b2Vec2 r2 = b2Mul(b2->m_xf.R, ccp->localAnchor2 - b2->GetLocalCenter());
// Relative velocity at contact
b2Vec2 dv = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2) - b1->m_linearVelocity - b2Cross(b1->m_angularVelocity, r1);
// Compute tangent force
float32 vt = b2Dot(dv, tangent);
float32 lambda = m_step.inv_dt * ccp->tangentMass * (-vt);
// b2Clamp the accumulated force
float32 maxFriction = c->friction * ccp->normalForce;
float32 newForce = b2Clamp(ccp->tangentForce + lambda, -maxFriction, maxFriction);
lambda = newForce - ccp->tangentForce;
// Apply contact impulse
b2Vec2 P = m_step.dt * lambda * tangent;
b1->m_linearVelocity -= invMass1 * P;
b1->m_angularVelocity -= invI1 * b2Cross(r1, P);
b2->m_linearVelocity += invMass2 * P;
b2->m_angularVelocity += invI2 * b2Cross(r2, P);
ccp->tangentForce = newForce;
}
}
}
void b2RevoluteJoint::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;
bool fixedRotation = (iA + iB == 0.0f);
// Solve motor constraint.
if (m_enableMotor && m_limitState != e_equalLimits && fixedRotation == false)
{
float32 Cdot = wB - wA - m_motorSpeed;
float32 impulse = -m_motorMass * Cdot;
float32 oldImpulse = m_motorImpulse;
float32 maxImpulse = data.step.dt * m_maxMotorTorque;
m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_motorImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve limit constraint.
if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false)
{
b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
float32 Cdot2 = wB - wA;
b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
b2Vec3 impulse = -m_mass.Solve33(Cdot);
if (m_limitState == e_equalLimits)
{
m_impulse += impulse;
}
else if (m_limitState == e_atLowerLimit)
{
float32 newImpulse = m_impulse.z + impulse.z;
if (newImpulse < 0.0f)
{
b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y);
b2Vec2 reduced = m_mass.Solve22(rhs);
impulse.x = reduced.x;
impulse.y = reduced.y;
impulse.z = -m_impulse.z;
m_impulse.x += reduced.x;
m_impulse.y += reduced.y;
m_impulse.z = 0.0f;
}
else
{
m_impulse += impulse;
}
}
else if (m_limitState == e_atUpperLimit)
{
float32 newImpulse = m_impulse.z + impulse.z;
if (newImpulse > 0.0f)
{
b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y);
b2Vec2 reduced = m_mass.Solve22(rhs);
impulse.x = reduced.x;
impulse.y = reduced.y;
impulse.z = -m_impulse.z;
m_impulse.x += reduced.x;
m_impulse.y += reduced.y;
m_impulse.z = 0.0f;
}
else
{
m_impulse += impulse;
}
}
b2Vec2 P(impulse.x, impulse.y);
vA -= mA * P;
wA -= iA * (b2Cross(m_rA, P) + impulse.z);
vB += mB * P;
wB += iB * (b2Cross(m_rB, P) + impulse.z);
}
else
{
// Solve point-to-point constraint
b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
b2Vec2 impulse = m_mass.Solve22(-Cdot);
m_impulse.x += impulse.x;
m_impulse.y += impulse.y;
vA -= mA * impulse;
wA -= iA * b2Cross(m_rA, impulse);
vB += mB * impulse;
wB += iB * b2Cross(m_rB, impulse);
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
int main(int argc, char** argv)
{
if (glfwInit() == 0)
{
fprintf(stderr, "Failed to initialize GLFW\n");
return -1;
}
char title[64];
sprintf(title, "Box2D Testbed Version %d.%d.%d", b2_version.major, b2_version.minor, b2_version.revision);
mainWindow = glfwCreateWindow(windowWidth, windowHeight, title, NULL, NULL);
if (mainWindow == NULL)
{
fprintf(stderr, "Failed to open GLFW mainWindow.\n");
glfwTerminate();
return -1;
}
glfwMakeContextCurrent(mainWindow);
glfwSetScrollCallback(mainWindow, sScrollCallback);
glfwSetWindowSizeCallback(mainWindow, sResizeWindow);
glfwSetKeyCallback(mainWindow, sKeyCallback);
glfwSetMouseButtonCallback(mainWindow, sMouseButton);
glfwSetCursorPosCallback(mainWindow, sMouseMotion);
glfwSetScrollCallback(mainWindow, sScrollCallback);
GLenum err = glewInit();
if (GLEW_OK != err)
{
fprintf(stderr, "Error: %s\n", glewGetErrorString(err));
exit(EXIT_FAILURE);
}
sCreateUI();
testCount = 0;
while (g_testEntries[testCount].createFcn != NULL)
{
++testCount;
}
testIndex = b2Clamp(testIndex, 0, testCount - 1);
testSelection = testIndex;
entry = g_testEntries + testIndex;
test = entry->createFcn();
test->SetWindow(mainWindow);
// Control the frame rate. One draw per monitor refresh.
glfwSwapInterval(1);
glClearColor(0.3f, 0.3f, 0.3f, 1.f);
// glfw scrolling
int glfwscroll = 0;
while (!glfwWindowShouldClose(mainWindow))
{
glfwGetWindowSize(mainWindow, &windowWidth, &windowHeight);
glViewport(0, 0, windowWidth, windowHeight);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
unsigned char mousebutton = 0;
int mscroll = ui.scroll;
ui.scroll = 0;
double xd, yd;
glfwGetCursorPos(mainWindow, &xd, &yd);
int mousex = int(xd);
int mousey = int(yd);
mousey = windowHeight - mousey;
int leftButton = glfwGetMouseButton(mainWindow, GLFW_MOUSE_BUTTON_LEFT);
if (leftButton == GLFW_PRESS)
mousebutton |= IMGUI_MBUT_LEFT;
imguiBeginFrame(mousex, mousey, mousebutton, mscroll);
sSimulate();
sInterface();
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glDisable(GL_DEPTH_TEST);
imguiRenderGLDraw(windowWidth, windowHeight);
glfwSwapBuffers(mainWindow);
glfwPollEvents();
}
imguiRenderGLDestroy();
glfwTerminate();
return 0;
}
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;
}
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 b2LineJoint::SolveVelocityConstraints(const b2TimeStep& step)
{
b2Body* b1 = m_bodyA;
b2Body* b2 = m_bodyB;
b2Vec2 v1 = b1->m_linearVelocity;
float32 w1 = b1->m_angularVelocity;
b2Vec2 v2 = b2->m_linearVelocity;
float32 w2 = b2->m_angularVelocity;
// Solve linear motor constraint.
if (m_enableMotor && m_limitState != e_equalLimits)
{
float32 Cdot = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1;
float32 impulse = m_motorMass * (m_motorSpeed - Cdot);
float32 oldImpulse = m_motorImpulse;
float32 maxImpulse = step.dt * m_maxMotorForce;
m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_motorImpulse - oldImpulse;
b2Vec2 P = impulse * m_axis;
float32 L1 = impulse * m_a1;
float32 L2 = impulse * m_a2;
v1 -= m_invMassA * P;
w1 -= m_invIA * L1;
v2 += m_invMassB * P;
w2 += m_invIB * L2;
}
float32 Cdot1 = b2Dot(m_perp, v2 - v1) + m_s2 * w2 - m_s1 * w1;
if (m_enableLimit && m_limitState != e_inactiveLimit)
{
// Solve prismatic and limit constraint in block form.
float32 Cdot2 = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1;
b2Vec2 Cdot(Cdot1, Cdot2);
b2Vec2 f1 = m_impulse;
b2Vec2 df = m_K.Solve(-Cdot);
m_impulse += df;
if (m_limitState == e_atLowerLimit)
{
m_impulse.y = b2Max(m_impulse.y, 0.0f);
}
else if (m_limitState == e_atUpperLimit)
{
m_impulse.y = b2Min(m_impulse.y, 0.0f);
}
// f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1)
float32 b = -Cdot1 - (m_impulse.y - f1.y) * m_K.col2.x;
float32 f2r;
if (m_K.col1.x != 0.0f)
{
f2r = b / m_K.col1.x + f1.x;
}
else
{
f2r = f1.x;
}
m_impulse.x = f2r;
df = m_impulse - f1;
b2Vec2 P = df.x * m_perp + df.y * m_axis;
float32 L1 = df.x * m_s1 + df.y * m_a1;
float32 L2 = df.x * m_s2 + df.y * m_a2;
v1 -= m_invMassA * P;
w1 -= m_invIA * L1;
v2 += m_invMassB * P;
w2 += m_invIB * L2;
}
else
{
// Limit is inactive, just solve the prismatic constraint in block form.
float32 df;
if (m_K.col1.x != 0.0f)
{
df = - Cdot1 / m_K.col1.x;
}
else
{
df = 0.0f;
}
m_impulse.x += df;
b2Vec2 P = df * m_perp;
float32 L1 = df * m_s1;
float32 L2 = df * m_s2;
v1 -= m_invMassA * P;
w1 -= m_invIA * L1;
v2 += m_invMassB * P;
w2 += m_invIB * L2;
}
b1->m_linearVelocity = v1;
b1->m_angularVelocity = w1;
b2->m_linearVelocity = v2;
b2->m_angularVelocity = w2;
}
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 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);
}
}
}
}
int main(int argc, char** argv)
{
testCount = 0;
while (g_testEntries[testCount].createFcn != NULL)
{
++testCount;
}
testIndex = b2Clamp(testIndex, 0, testCount-1);
testSelection = testIndex;
entry = g_testEntries + testIndex;
test = entry->createFcn();
glutInit(&argc, argv);
glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);
glutInitWindowSize(width, height);
char title[32];
sprintf(title, "Box2D Version %d.%d.%d", b2_version.major, b2_version.minor, b2_version.revision);
mainWindow = glutCreateWindow(title);
//glutSetOption (GLUT_ACTION_ON_WINDOW_CLOSE, GLUT_ACTION_GLUTMAINLOOP_RETURNS);
glutDisplayFunc(SimulationLoop);
GLUI_Master.set_glutReshapeFunc(Resize);
GLUI_Master.set_glutKeyboardFunc(Keyboard);
GLUI_Master.set_glutSpecialFunc(KeyboardSpecial);
GLUI_Master.set_glutMouseFunc(Mouse);
#ifdef FREEGLUT
glutMouseWheelFunc(MouseWheel);
#endif
glutMotionFunc(MouseMotion);
glui = GLUI_Master.create_glui_subwindow( mainWindow,
GLUI_SUBWINDOW_RIGHT );
glui->add_statictext("Tests");
GLUI_Listbox* testList =
glui->add_listbox("", &testSelection);
glui->add_separator();
GLUI_Spinner* velocityIterationSpinner =
glui->add_spinner("Vel Iters", GLUI_SPINNER_INT, &settings.velocityIterations);
velocityIterationSpinner->set_int_limits(1, 500);
GLUI_Spinner* positionIterationSpinner =
glui->add_spinner("Pos Iters", GLUI_SPINNER_INT, &settings.positionIterations);
positionIterationSpinner->set_int_limits(0, 100);
GLUI_Spinner* hertzSpinner =
glui->add_spinner("Hertz", GLUI_SPINNER_FLOAT, &settingsHz);
hertzSpinner->set_float_limits(5.0f, 200.0f);
glui->add_checkbox("Warm Starting", &settings.enableWarmStarting);
glui->add_checkbox("Time of Impact", &settings.enableContinuous);
//glui->add_separator();
GLUI_Panel* drawPanel = glui->add_panel("Draw");
glui->add_checkbox_to_panel(drawPanel, "Shapes", &settings.drawShapes);
glui->add_checkbox_to_panel(drawPanel, "Joints", &settings.drawJoints);
glui->add_checkbox_to_panel(drawPanel, "AABBs", &settings.drawAABBs);
glui->add_checkbox_to_panel(drawPanel, "Pairs", &settings.drawPairs);
glui->add_checkbox_to_panel(drawPanel, "Contact Points", &settings.drawContactPoints);
glui->add_checkbox_to_panel(drawPanel, "Contact Normals", &settings.drawContactNormals);
glui->add_checkbox_to_panel(drawPanel, "Contact Forces", &settings.drawContactForces);
glui->add_checkbox_to_panel(drawPanel, "Friction Forces", &settings.drawFrictionForces);
glui->add_checkbox_to_panel(drawPanel, "Center of Masses", &settings.drawCOMs);
glui->add_checkbox_to_panel(drawPanel, "Statistics", &settings.drawStats);
int32 testCount = 0;
TestEntry* e = g_testEntries;
while (e->createFcn)
{
testList->add_item(testCount, e->name);
++testCount;
++e;
}
glui->add_button("Pause", 0, Pause);
glui->add_button("Single Step", 0, SingleStep);
glui->add_button("Restart", 0, Restart);
glui->add_button("Quit", 0,(GLUI_Update_CB)exit);
glui->set_main_gfx_window( mainWindow );
// Use a timer to control the frame rate.
glutTimerFunc(framePeriod, Timer, 0);
glutMainLoop();
return 0;
}
