assert (pointCount == 1 || pointCount == 2);
// Solve tangent constraints
for (int j = 0; j < pointCount; ++j) {
final VelocityConstraintPoint vcp = vc.points[j];
// Vec2.crossToOutUnsafe(wA, vcp.rA, temp);
// Vec2.crossToOutUnsafe(wB, vcp.rB, dv);
// dv.addLocal(vB).subLocal(vA).subLocal(temp);
final Vec2 a = vcp.rA;
dv.x = -wB * vcp.rB.y + vB.x - vA.x + wA * a.y;
dv.y = wB * vcp.rB.x + vB.y - vA.y - wA * a.x;
// Compute tangent force
final float vt = dv.x * tangent.x + dv.y * tangent.y - 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 = tangent.x * lambda;
final float Py = tangent.y * 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);
// System.out.println("tangent solve velocity (point "+j+") for " + indexA + " is " + vA.x + "," + vA.y + " rot " +
// wA);
// System.out.println("tangent solve velocity (point "+j+") for " + indexB + " is " + vB.x + "," + vB.y + " rot " +
// wB);
}
// Solve normal constraints
if (vc.pointCount == 1) {
final VelocityConstraintPoint vcp = vc.points[0];
Vec2 a1 = vcp.rA;
// Relative velocity at contact
// Vec2 dv = vB + Cross(wB, vcp.rB) - vA - Cross(wA, vcp.rA);
// Vec2.crossToOut(wA, vcp.rA, temp1);
// Vec2.crossToOut(wB, vcp.rB, dv);
// dv.addLocal(vB).subLocal(vA).subLocal(temp1);
//
dv.x = -wB * vcp.rB.y + vB.x - vA.x + wA * a1.y;
dv.y = wB * vcp.rB.x + vB.y - vA.y - wA * a1.x;
// Compute normal impulse
final float vn = dv.x * normal.x + dv.y * normal.y;
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;
// assert(newImpulse == 0);
vcp.normalImpulse = newImpulse;
// Apply contact impulse
float Px = normal.x * lambda;
float Py = normal.y * lambda;
// vA -= invMassA * P;
vA.x -= Px * mA;
vA.y -= Py * mA;
wA -= iA * (vcp.rA.x * Py - vcp.rA.y * Px);
// assert(vA.x == 0);
// vB += invMassB * P;
vB.x += Px * mB;
vB.y += Py * mB;
wB += iB * (vcp.rB.x * Py - vcp.rB.y * Px);
// assert(vB.x == 0);
} 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];
a.x = cp1.normalImpulse;
a.y = cp2.normalImpulse;
assert (a.x >= 0.0f && a.y >= 0.0f);
// Relative velocity at contact