// 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; } } } }