// djm pooling from above
public void solveVelocityConstraints()
{
for (int i = 0; i < m_count; ++i)
{
ContactVelocityConstraint vc = m_velocityConstraints[i];
int indexA = vc.indexA;
int indexB = vc.indexB;
double mA = vc.invMassA;
double mB = vc.invMassB;
double iA = vc.invIA;
double iB = vc.invIB;
int pointCount = vc.pointCount;
Vec2 vA = m_velocities[indexA].v;
double wA = m_velocities[indexA].w;
Vec2 vB = m_velocities[indexB].v;
double wB = m_velocities[indexB].w;
Vec2 normal = vc.normal;
tangent.x = 1.0d * vc.normal.y;
tangent.y = -1.0d * vc.normal.x;
double friction = vc.friction;
// Solve tangent constraints
for (int j = 0; j < pointCount; ++j)
{
VelocityConstraintPoint vcp = vc.points[j];
Vec2 a = vcp.rA;
double dvx = -wB * vcp.rB.y + vB.x - vA.x + wA * a.y;
double dvy = wB * vcp.rB.x + vB.y - vA.y - wA * a.x;
// Compute tangent force
double vt = dvx * tangent.x + dvy * tangent.y - vc.tangentSpeed;
double lambda = vcp.tangentMass * (-vt);
// Clamp the accumulated force
double maxFriction = friction * vcp.normalImpulse;
double newImpulse =
MathUtils.clamp(vcp.tangentImpulse + lambda, -maxFriction, maxFriction);
lambda = newImpulse - vcp.tangentImpulse;
vcp.tangentImpulse = newImpulse;
// Apply contact impulse
// Vec2 P = lambda * tangent;
double Px = tangent.x * lambda;
double 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);
}
// Solve normal constraints
if (vc.pointCount == 1)
{
VelocityConstraintPoint vcp = vc.points[0];
// Relative velocity at contact
// Vec2 dv = vB + Cross(wB, vcp.rB) - vA - Cross(wA, vcp.rA);
double dvx = -wB * vcp.rB.y + vB.x - vA.x + wA * vcp.rA.y;
double dvy = wB * vcp.rB.x + vB.y - vA.y - wA * vcp.rA.x;
// Compute normal impulse
double vn = dvx * normal.x + dvy * normal.y;
double lambda = -vcp.normalMass * (vn - vcp.velocityBias);
// Clamp the accumulated impulse
double a = vcp.normalImpulse + lambda;
double newImpulse = (a > 0.0d ? a : 0.0d);
lambda = newImpulse - vcp.normalImpulse;
vcp.normalImpulse = newImpulse;
// Apply contact impulse
double Px = normal.x * lambda;
double 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);
// 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;
VelocityConstraintPoint cp1 = vc.points[0];
VelocityConstraintPoint cp2 = vc.points[1];
a.x = cp1.normalImpulse;
a.y = cp2.normalImpulse;
// Relative velocity at contact
// Vec2 dv1 = vB + Cross(wB, cp1.rB) - vA - Cross(wA, cp1.rA);
dv1.x = -wB * cp1.rB.y + vB.x - vA.x + wA * cp1.rA.y;
dv1.y = wB * cp1.rB.x + vB.y - vA.y - wA * cp1.rA.x;
// Vec2 dv2 = vB + Cross(wB, cp2.rB) - vA - Cross(wA, cp2.rA);
dv2.x = -wB * cp2.rB.y + vB.x - vA.x + wA * cp2.rA.y;
dv2.y = wB * cp2.rB.x + vB.y - vA.y - wA * cp2.rA.x;
// Compute normal velocity
double vn1 = dv1.x * normal.x + dv1.y * normal.y;
double vn2 = dv2.x * normal.x + dv2.y * normal.y;
b.x = vn1 - cp1.velocityBias;
b.y = vn2 - cp2.velocityBias;
// System.out.println("b is " + b.x + "," + b.y);
// Compute b'
Mat22 R = vc.K;
b.x -= R.ex.x * a.x + R.ey.x * a.y;
b.y -= R.ex.y * a.x + R.ey.y * a.y;
// System.out.println("b' is " + b.x + "," + b.y);
// double k_errorTol = 1e-3d;
// B2_NOT_USED(k_errorTol);
for (;;)
{
//
// Case 1: vn = 0
//
// 0 = A * x' + b'
//
// Solve for x':
//
// x' = - inv(A) * b'
//
// Vec2 x = - Mul(c.normalMass, b);
Mat22.mulToOutUnsafe(vc.normalMass, b, x);
x.x *= -1;
x.y *= -1;
if (x.x >= 0.0d && x.y >= 0.0d)
{
// System.out.println("case 1");
// Get the incremental impulse
// Vec2 d = x - a;
d.set(x).subLocal(a);
// Apply incremental impulse
// Vec2 P1 = d.x * normal;
// Vec2 P2 = d.y * normal;
P1.set(normal).mulLocal(d.x);
P2.set(normal).mulLocal(d.y);
/*
* vA -= invMassA * (P1 + P2); wA -= invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2));
*
* vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2));
*/
temp1.set(P1).addLocal(P2);
temp2.set(temp1).mulLocal(mA);
vA.subLocal(temp2);
temp2.set(temp1).mulLocal(mB);
vB.addLocal(temp2);
wA -= iA * (Vec2.cross(cp1.rA, P1) + Vec2.cross(cp2.rA, P2));
wB += iB * (Vec2.cross(cp1.rB, P1) + Vec2.cross(cp2.rB, P2));
// Accumulate
cp1.normalImpulse = x.x;
cp2.normalImpulse = x.y;
/*
* #if B2_DEBUG_SOLVER == 1 // Postconditions dv1 = vB + Cross(wB, cp1.rB) - vA -
* Cross(wA, cp1.rA); dv2 = vB + Cross(wB, cp2.rB) - vA - Cross(wA, cp2.rA);
*
* // Compute normal velocity vn1 = Dot(dv1, normal); vn2 = Dot(dv2, normal);
*
* assert(Abs(vn1 - cp1.velocityBias) < k_errorTol); assert(Abs(vn2 - cp2.velocityBias)
* < k_errorTol); #endif
*/
if (DEBUG_SOLVER)
{
// Postconditions
Vec2 dv1c =
vB.add(Vec2.cross(wB, cp1.rB).subLocal(vA).subLocal(Vec2.cross(wA, cp1.rA)));
Vec2 dv2c =
vB.add(Vec2.cross(wB, cp2.rB).subLocal(vA).subLocal(Vec2.cross(wA, cp2.rA)));
// Compute normal velocity
vn1 = Vec2.dot(dv1c, normal);
vn2 = Vec2.dot(dv2c, normal);
}
break;
}
//
// Case 2: vn1 = 0 and x2 = 0
//
// 0 = a11 * x1' + a12 * 0 + b1'
// vn2 = a21 * x1' + a22 * 0 + '
//
x.x = -cp1.normalMass * b.x;
x.y = 0.0d;
vn1 = 0.0d;
vn2 = vc.K.ex.y * x.x + b.y;
if (x.x >= 0.0d && vn2 >= 0.0d)
{
// System.out.println("case 2");
// Get the incremental impulse
d.set(x).subLocal(a);
// Apply incremental impulse
// Vec2 P1 = d.x * normal;
// Vec2 P2 = d.y * normal;
P1.set(normal).mulLocal(d.x);
P2.set(normal).mulLocal(d.y);
/*
* Vec2 P1 = d.x * normal; Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -=
* invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2));
*
* vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2));
*/
temp1.set(P1).addLocal(P2);
temp2.set(temp1).mulLocal(mA);
vA.subLocal(temp2);
temp2.set(temp1).mulLocal(mB);
vB.addLocal(temp2);
wA -= iA * (Vec2.cross(cp1.rA, P1) + Vec2.cross(cp2.rA, P2));
wB += iB * (Vec2.cross(cp1.rB, P1) + Vec2.cross(cp2.rB, P2));
// Accumulate
cp1.normalImpulse = x.x;
cp2.normalImpulse = x.y;
/*
* #if B2_DEBUG_SOLVER == 1 // Postconditions dv1 = vB + Cross(wB, cp1.rB) - vA -
* Cross(wA, cp1.rA);
*
* // Compute normal velocity vn1 = Dot(dv1, normal);
*
* assert(Abs(vn1 - cp1.velocityBias) < k_errorTol); #endif
*/
if (DEBUG_SOLVER)
{
// Postconditions
Vec2 dv1c =
vB.add(Vec2.cross(wB, cp1.rB).subLocal(vA).subLocal(Vec2.cross(wA, cp1.rA)));
// Compute normal velocity
vn1 = Vec2.dot(dv1c, normal);
}
break;
}
//
// Case 3: wB = 0 and x1 = 0
//
// vn1 = a11 * 0 + a12 * x2' + b1'
// 0 = a21 * 0 + a22 * x2' + '
//
x.x = 0.0d;
x.y = -cp2.normalMass * b.y;
vn1 = vc.K.ey.x * x.y + b.x;
vn2 = 0.0d;
if (x.y >= 0.0d && vn1 >= 0.0d)
{
// System.out.println("case 3");
// Resubstitute for the incremental impulse
d.set(x).subLocal(a);
// Apply incremental impulse
/*
* Vec2 P1 = d.x * normal; Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -=
* invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2));
*
* vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2));
*/
P1.set(normal).mulLocal(d.x);
P2.set(normal).mulLocal(d.y);
temp1.set(P1).addLocal(P2);
temp2.set(temp1).mulLocal(mA);
vA.subLocal(temp2);
temp2.set(temp1).mulLocal(mB);
vB.addLocal(temp2);
wA -= iA * (Vec2.cross(cp1.rA, P1) + Vec2.cross(cp2.rA, P2));
wB += iB * (Vec2.cross(cp1.rB, P1) + Vec2.cross(cp2.rB, P2));
// Accumulate
cp1.normalImpulse = x.x;
cp2.normalImpulse = x.y;
/*
* #if B2_DEBUG_SOLVER == 1 // Postconditions dv2 = vB + Cross(wB, cp2.rB) - vA -
* Cross(wA, cp2.rA);
*
* // Compute normal velocity vn2 = Dot(dv2, normal);
*
* assert(Abs(vn2 - cp2.velocityBias) < k_errorTol); #endif
*/
if (DEBUG_SOLVER)
{
// Postconditions
Vec2 dv2c =
vB.add(Vec2.cross(wB, cp2.rB).subLocal(vA).subLocal(Vec2.cross(wA, cp2.rA)));
// Compute normal velocity
vn2 = Vec2.dot(dv2c, normal);
}
break;
}
//
// Case 4: x1 = 0 and x2 = 0
//
// vn1 = b1
// vn2 = ;
x.x = 0.0d;
x.y = 0.0d;
vn1 = b.x;
vn2 = b.y;
if (vn1 >= 0.0d && vn2 >= 0.0d)
{
// System.out.println("case 4");
// Resubstitute for the incremental impulse
d.set(x).subLocal(a);
// Apply incremental impulse
/*
* Vec2 P1 = d.x * normal; Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -=
* invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2));
*
* vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2));
*/
P1.set(normal).mulLocal(d.x);
P2.set(normal).mulLocal(d.y);
temp1.set(P1).addLocal(P2);
temp2.set(temp1).mulLocal(mA);
vA.subLocal(temp2);
temp2.set(temp1).mulLocal(mB);
vB.addLocal(temp2);
wA -= iA * (Vec2.cross(cp1.rA, P1) + Vec2.cross(cp2.rA, P2));
wB += iB * (Vec2.cross(cp1.rB, P1) + Vec2.cross(cp2.rB, P2));
// Accumulate
cp1.normalImpulse = x.x;
cp2.normalImpulse = x.y;
}
// No solution, give up. This is hit sometimes, but it doesn't seem to matter.
break;
}
}
// m_velocities[indexA].v.set(vA);
m_velocities[indexA].w = wA;
// m_velocities[indexB].v.set(vB);
m_velocities[indexB].w = wB;
}
}
// djm pooling
public void init(ContactSolverDef def)
{
// System.out.println("Initializing contact solver");
m_step = def.step;
m_count = def.count;
if (m_positionConstraints.Length < m_count)
{
ContactPositionConstraint[] old = m_positionConstraints;
m_positionConstraints = new ContactPositionConstraint[MathUtils.max(old.Length * 2, m_count)];
ArrayHelper.Copy(old, 0, m_positionConstraints, 0, old.Length);
for (int i = old.Length; i < m_positionConstraints.Length; i++)
{
m_positionConstraints[i] = new ContactPositionConstraint();
}
}
if (m_velocityConstraints.Length < m_count)
{
ContactVelocityConstraint[] old = m_velocityConstraints;
m_velocityConstraints = new ContactVelocityConstraint[MathUtils.max(old.Length * 2, m_count)];
ArrayHelper.Copy(old, 0, m_velocityConstraints, 0, old.Length);
for (int i = old.Length; i < m_velocityConstraints.Length; i++)
{
m_velocityConstraints[i] = new ContactVelocityConstraint();
}
}
m_positions = def.positions;
m_velocities = def.velocities;
m_contacts = def.contacts;
for (int i = 0; i < m_count; ++i)
{
// System.out.println("contacts: " + m_count);
Contact contact = m_contacts[i];
Fixture fixtureA = contact.m_fixtureA;
Fixture fixtureB = contact.m_fixtureB;
Shape shapeA = fixtureA.getShape();
Shape shapeB = fixtureB.getShape();
double radiusA = shapeA.m_radius;
double radiusB = shapeB.m_radius;
Body bodyA = fixtureA.getBody();
Body bodyB = fixtureB.getBody();
Manifold manifold = contact.getManifold();
int pointCount = manifold.pointCount;
ContactVelocityConstraint vc = m_velocityConstraints[i];
vc.friction = contact.m_friction;
vc.restitution = contact.m_restitution;
vc.tangentSpeed = contact.m_tangentSpeed;
vc.indexA = bodyA.m_islandIndex;
vc.indexB = bodyB.m_islandIndex;
vc.invMassA = bodyA.m_invMass;
vc.invMassB = bodyB.m_invMass;
vc.invIA = bodyA.m_invI;
vc.invIB = bodyB.m_invI;
vc.contactIndex = i;
vc.pointCount = pointCount;
vc.K.setZero();
vc.normalMass.setZero();
ContactPositionConstraint pc = m_positionConstraints[i];
pc.indexA = bodyA.m_islandIndex;
pc.indexB = bodyB.m_islandIndex;
pc.invMassA = bodyA.m_invMass;
pc.invMassB = bodyB.m_invMass;
pc.localCenterA.set(bodyA.m_sweep.localCenter);
pc.localCenterB.set(bodyB.m_sweep.localCenter);
pc.invIA = bodyA.m_invI;
pc.invIB = bodyB.m_invI;
pc.localNormal.set(manifold.localNormal);
pc.localPoint.set(manifold.localPoint);
pc.pointCount = pointCount;
pc.radiusA = radiusA;
pc.radiusB = radiusB;
pc.type = manifold.type;
// System.out.println("contact point count: " + pointCount);
for (int j = 0; j < pointCount; j++)
{
ManifoldPoint cp = manifold.points[j];
VelocityConstraintPoint vcp = vc.points[j];
if (m_step.warmStarting)
{
// assert(cp.normalImpulse == 0);
// System.out.println("contact normal impulse: " + cp.normalImpulse);
vcp.normalImpulse = m_step.dtRatio * cp.normalImpulse;
vcp.tangentImpulse = m_step.dtRatio * cp.tangentImpulse;
}
else
{
vcp.normalImpulse = 0;
vcp.tangentImpulse = 0;
}
vcp.rA.setZero();
vcp.rB.setZero();
vcp.normalMass = 0;
vcp.tangentMass = 0;
vcp.velocityBias = 0;
pc.localPoints[j].x = cp.localPoint.x;
pc.localPoints[j].y = cp.localPoint.y;
}
}
}
// djm pooling, and from above
public void initializeVelocityConstraints()
{
// Warm start.
for (int i = 0; i < m_count; ++i)
{
ContactVelocityConstraint vc = m_velocityConstraints[i];
ContactPositionConstraint pc = m_positionConstraints[i];
double radiusA = pc.radiusA;
double radiusB = pc.radiusB;
Manifold manifold = m_contacts[vc.contactIndex].getManifold();
int indexA = vc.indexA;
int indexB = vc.indexB;
double mA = vc.invMassA;
double mB = vc.invMassB;
double iA = vc.invIA;
double iB = vc.invIB;
Vec2 localCenterA = pc.localCenterA;
Vec2 localCenterB = pc.localCenterB;
Vec2 cA = m_positions[indexA].c;
double aA = m_positions[indexA].a;
Vec2 vA = m_velocities[indexA].v;
double wA = m_velocities[indexA].w;
Vec2 cB = m_positions[indexB].c;
double aB = m_positions[indexB].a;
Vec2 vB = m_velocities[indexB].v;
double wB = m_velocities[indexB].w;
xfA.q.set(aA);
xfB.q.set(aB);
xfA.p.x = cA.x - (xfA.q.c * localCenterA.x - xfA.q.s * localCenterA.y);
xfA.p.y = cA.y - (xfA.q.s * localCenterA.x + xfA.q.c * localCenterA.y);
xfB.p.x = cB.x - (xfB.q.c * localCenterB.x - xfB.q.s * localCenterB.y);
xfB.p.y = cB.y - (xfB.q.s * localCenterB.x + xfB.q.c * localCenterB.y);
worldManifold.initialize(manifold, xfA, radiusA, xfB, radiusB);
vc.normal.set(worldManifold.normal);
int pointCount = vc.pointCount;
for (int j = 0; j < pointCount; ++j)
{
VelocityConstraintPoint vcp = vc.points[j];
vcp.rA.set(worldManifold.points[j]).subLocal(cA);
vcp.rB.set(worldManifold.points[j]).subLocal(cB);
double rnA = vcp.rA.x * vc.normal.y - vcp.rA.y * vc.normal.x;
double rnB = vcp.rB.x * vc.normal.y - vcp.rB.y * vc.normal.x;
double kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
vcp.normalMass = kNormal > 0.0d ? 1.0d / kNormal : 0.0d;
double tangentx = 1.0d * vc.normal.y;
double tangenty = -1.0d * vc.normal.x;
double rtA = vcp.rA.x * tangenty - vcp.rA.y * tangentx;
double rtB = vcp.rB.x * tangenty - vcp.rB.y * tangentx;
double kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;
vcp.tangentMass = kTangent > 0.0d ? 1.0d / kTangent : 0.0d;
// Setup a velocity bias for restitution.
vcp.velocityBias = 0.0d;
double tempx = vB.x + -wB * vcp.rB.y - vA.x - (-wA * vcp.rA.y);
double tempy = vB.y + wB * vcp.rB.x - vA.y - (wA * vcp.rA.x);
double vRel = vc.normal.x * tempx + vc.normal.y * tempy;
if (vRel < -Settings.velocityThreshold)
{
vcp.velocityBias = -vc.restitution * vRel;
}
}
// If we have two points, then prepare the block solver.
if (vc.pointCount == 2)
{
VelocityConstraintPoint vcp1 = vc.points[0];
VelocityConstraintPoint vcp2 = vc.points[1];
double rn1A = Vec2.cross(vcp1.rA, vc.normal);
double rn1B = Vec2.cross(vcp1.rB, vc.normal);
double rn2A = Vec2.cross(vcp2.rA, vc.normal);
double rn2B = Vec2.cross(vcp2.rB, vc.normal);
double k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
double k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
double k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B;
if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
{
// K is safe to invert.
vc.K.ex.set(k11, k12);
vc.K.ey.set(k12, k22);
vc.K.invertToOut(vc.normalMass);
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
vc.pointCount = 1;
}
}
}
}