assert (pointCount == 1 || pointCount == 2);
// Solve tangent constraints
for (int j = 0; j < pointCount; ++j) {
final VelocityConstraintPoint vcp = vc.points[j];
final Vec2 a = vcp.rA;
float dvx = -wB * vcp.rB.y + vB.x - vA.x + wA * a.y;
float dvy = wB * vcp.rB.x + vB.y - vA.y - wA * a.x;
// Compute tangent force
final float vt = dvx * tangentx + dvy * tangenty - vc.tangentSpeed;
float lambda = vcp.tangentMass * (-vt);
// Clamp the accumulated force
final float maxFriction = friction * vcp.normalImpulse;
final float newImpulse =
MathUtils.clamp(vcp.tangentImpulse + lambda, -maxFriction, maxFriction);
lambda = newImpulse - vcp.tangentImpulse;
vcp.tangentImpulse = newImpulse;
// Apply contact impulse
// Vec2 P = lambda * tangent;
final float Px = tangentx * lambda;
final float Py = tangenty * lambda;
// vA -= invMassA * P;
vA.x -= Px * mA;
vA.y -= Py * mA;
wA -= iA * (vcp.rA.x * Py - vcp.rA.y * Px);
// vB += invMassB * P;
vB.x += Px * mB;
vB.y += Py * mB;
wB += iB * (vcp.rB.x * Py - vcp.rB.y * Px);
}
// Solve normal constraints
if (vc.pointCount == 1) {
final VelocityConstraintPoint vcp = vc.points[0];
// Relative velocity at contact
// Vec2 dv = vB + Cross(wB, vcp.rB) - vA - Cross(wA, vcp.rA);
float dvx = -wB * vcp.rB.y + vB.x - vA.x + wA * vcp.rA.y;
float dvy = wB * vcp.rB.x + vB.y - vA.y - wA * vcp.rA.x;
// Compute normal impulse
final float vn = dvx * normalx + dvy * normaly;
float lambda = -vcp.normalMass * (vn - vcp.velocityBias);
// Clamp the accumulated impulse
float a = vcp.normalImpulse + lambda;
final float newImpulse = (a > 0.0f ? a : 0.0f);
lambda = newImpulse - vcp.normalImpulse;
vcp.normalImpulse = newImpulse;
// Apply contact impulse
float Px = normalx * lambda;
float Py = normaly * lambda;
// vA -= invMassA * P;
vA.x -= Px * mA;
vA.y -= Py * mA;
wA -= iA * (vcp.rA.x * Py - vcp.rA.y * Px);
// vB += invMassB * P;
vB.x += Px * mB;
vB.y += Py * mB;
wB += iB * (vcp.rB.x * Py - vcp.rB.y * Px);
} 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 = 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;
final VelocityConstraintPoint cp1 = vc.points[0];
final VelocityConstraintPoint cp2 = vc.points[1];
final Vec2 cp1rA = cp1.rA;
final Vec2 cp1rB = cp1.rB;
final Vec2 cp2rA = cp2.rA;
final Vec2 cp2rB = cp2.rB;
float ax = cp1.normalImpulse;