ContactSolver class
class ContactSolver { /** * For each solver, this is the initial number of constraints * in the array, which expands as needed. */ static const int INITIAL_NUM_CONSTRAINTS = 256; /** * Ensure a reasonable condition number. For the block solver */ static const num K_MAX_CONDITION_NUMBER = 100.0; List<ContactConstraint> constraints; int constraintCount; /** Pooling */ //TODO(gregbglw): What do many of these names mean? What is rA, for example? final WorldManifold worldManifold; final Vector tangent; final Vector temp1; final Vector temp2; final Vector P; final Vector dv; final Vector dv1; final Vector dv2; final Vector x; final Vector d; final Vector P1; final Vector P2; final PositionSolverManifold psolver; final Vector rA; final Vector rB; /** Constructs a new ContactSolver. */ ContactSolver() : constraints = new List<ContactConstraint>(INITIAL_NUM_CONSTRAINTS), // Setup pool variables. worldManifold = new WorldManifold(), tangent = new Vector(), temp1 = new Vector(), temp2 = new Vector(), P = new Vector(), dv = new Vector(), dv1 = new Vector(), dv2 = new Vector(), x = new Vector(), d = new Vector(), P1 = new Vector(), P2 = new Vector(), psolver = new PositionSolverManifold(), rA = new Vector(), rB = new Vector() { for(int i=0; i < constraints.length; i++) { constraints[i] = new ContactConstraint(); } } void init(List<Contact> contacts, int contactCount, num impulseRatio){ constraintCount = contactCount; // dynamic array if(constraints.length < contactCount){ List<ContactConstraint> old = constraints; int newLen = Math.max(old.length * 2, constraintCount); constraints = new List<ContactConstraint>(newLen); constraints.setRange(0, old.length, old); for(int i=old.length; i< constraints.length; i++){ constraints[i] = new ContactConstraint(); } } for (int i = 0; i < constraintCount; ++i){ Contact contact = contacts[i]; Fixture fixtureA = contact.fixtureA; Fixture fixtureB = contact.fixtureB; Shape shapeA = fixtureA.shape; Shape shapeB = fixtureB.shape; num radiusA = shapeA.radius; num radiusB = shapeB.radius; Body bodyA = fixtureA.body; Body bodyB = fixtureB.body; Manifold manifold = contact.manifold; num friction = Settings.mixFriction(fixtureA.friction, fixtureB.friction); num restitution = Settings.mixRestitution(fixtureA.restitution, fixtureB.restitution); Vector vA = bodyA.linearVelocity; Vector vB = bodyB.linearVelocity; num wA = bodyA.angularVelocity; num wB = bodyB.angularVelocity; assert(manifold.pointCount > 0); worldManifold.initialize(manifold, bodyA.originTransform, radiusA, bodyB.originTransform, radiusB); ContactConstraint cc = constraints[i]; cc.bodyA = bodyA; cc.bodyB = bodyB; cc.manifold = manifold; cc.normal.x = worldManifold.normal.x; cc.normal.y = worldManifold.normal.y; // have to set actual manifold cc.pointCount = manifold.pointCount; cc.friction = friction; cc.restitution = restitution; cc.localNormal.x = manifold.localNormal.x; cc.localNormal.y = manifold.localNormal.y; cc.localPoint.x = manifold.localPoint.x; cc.localPoint.y = manifold.localPoint.y; cc.radius = radiusA + radiusB; cc.type = manifold.type; for (int j = 0; j < cc.pointCount; ++j){ ManifoldPoint cp = manifold.points[j]; ContactConstraintPoint ccp = cc.points[j]; ccp.normalImpulse = impulseRatio * cp.normalImpulse; ccp.tangentImpulse = impulseRatio * cp.tangentImpulse; ccp.localPoint.x = cp.localPoint.x; ccp.localPoint.y = cp.localPoint.y; ccp.rA.x = worldManifold.points[j].x - bodyA.sweep.center.x; ccp.rA.y = worldManifold.points[j].y - bodyA.sweep.center.y; ccp.rB.x = worldManifold.points[j].x - bodyB.sweep.center.x; ccp.rB.y = worldManifold.points[j].y - bodyB.sweep.center.y; num rnA = ccp.rA.x * cc.normal.y - ccp.rA.y * cc.normal.x; num rnB = ccp.rB.x * cc.normal.y - ccp.rB.y * cc.normal.x; rnA *= rnA; rnB *= rnB; num kNormal = bodyA.invMass + bodyB.invMass + bodyA.invInertia * rnA + bodyB.invInertia * rnB; assert(kNormal > Settings.EPSILON); ccp.normalMass = 1.0 / kNormal; tangent.x = 1.0 * cc.normal.y; tangent.y = -1.0 * cc.normal.x; num rtA = ccp.rA.x * tangent.y - ccp.rA.y * tangent.x; num rtB = ccp.rB.x * tangent.y - ccp.rB.y * tangent.x; rtA *= rtA; rtB *= rtB; num kTangent = bodyA.invMass + bodyB.invMass + bodyA.invInertia * rtA + bodyB.invInertia * rtB; assert(kTangent > Settings.EPSILON); ccp.tangentMass = 1.0 / kTangent; // Setup a velocity bias for restitution. ccp.velocityBias = 0.0; temp2.x = -wA * ccp.rA.y; temp2.y = wA * ccp.rA.x; temp1.x = -wB * ccp.rB.y + vB.x - vA.x - temp2.x; temp1.y = wB * ccp.rB.x + vB.y - vA.y - temp2.y; Vector a = cc.normal; num vRel = a.x * temp1.x + a.y * temp1.y; if (vRel < -Settings.VELOCITY_THRESHOLD){ ccp.velocityBias = -restitution * vRel; } } // If we have two points, then prepare the block solver. if (cc.pointCount == 2){ ContactConstraintPoint ccp1 = cc.points[0]; ContactConstraintPoint ccp2 = cc.points[1]; num invMassA = bodyA.invMass; num invIA = bodyA.invInertia; num invMassB = bodyB.invMass; num invIB = bodyB.invInertia; num rn1A = Vector.crossVectors(ccp1.rA, cc.normal); num rn1B = Vector.crossVectors(ccp1.rB, cc.normal); num rn2A = Vector.crossVectors(ccp2.rA, cc.normal); num rn2B = Vector.crossVectors(ccp2.rB, cc.normal); num k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B; num k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B; num k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B; // Ensure a reasonable condition number. if (k11 * k11 < K_MAX_CONDITION_NUMBER * (k11 * k22 - k12 * k12)) { // K is safe to invert. cc.K.col1.x = k11; cc.K.col1.y = k12; cc.K.col2.x = k12; cc.K.col2.y = k22; cc.normalMass.col1.x = cc.K.col1.x; cc.normalMass.col1.y = cc.K.col1.y; cc.normalMass.col2.x = cc.K.col2.x; cc.normalMass.col2.y = cc.K.col2.y; cc.normalMass.invertLocal(); } else{ // The constraints are redundant, just use one. cc.pointCount = 1; } } } } void warmStart(){ // Warm start. for (int i = 0; i < constraintCount; ++i){ ContactConstraint c = constraints[i]; final Body bodyA = c.bodyA; final Body bodyB = c.bodyB; final num invMassA = bodyA.invMass; final num invIA = bodyA.invInertia; final num invMassB = bodyB.invMass; final num invIB = bodyB.invInertia; final Vector normal = c.normal; Vector.crossVectorAndNumToOut(normal, 1, tangent); for (int j = 0; j < c.pointCount; ++j){ ContactConstraintPoint ccp = c.points[j]; num Px = ccp.normalImpulse * normal.x + ccp.tangentImpulse * tangent.x; num Py = ccp.normalImpulse * normal.y + ccp.tangentImpulse * tangent.y; bodyA.angularVelocity -= invIA * (ccp.rA.x * Py - ccp.rA.y * Px); bodyA.linearVelocity.x -= Px * invMassA; bodyA.linearVelocity.y -= Py * invMassA; bodyB.angularVelocity += invIB * (ccp.rB.x * Py - ccp.rB.y * Px); bodyB.linearVelocity.x += Px * invMassB; bodyB.linearVelocity.y += Py * invMassB; } } } void solveVelocityConstraints(){ for (int i = 0; i < constraintCount; ++i){ final ContactConstraint c = constraints[i]; final Body bodyA = c.bodyA; final Body bodyB = c.bodyB; num wA = bodyA.angularVelocity; num wB = bodyB.angularVelocity; final Vector vA = bodyA.linearVelocity; final Vector vB = bodyB.linearVelocity; final num invMassA = bodyA.invMass; final num invIA = bodyA.invInertia; final num invMassB = bodyB.invMass; final num invIB = bodyB.invInertia; tangent.x = 1.0 * c.normal.y; tangent.y = -1.0 * c.normal.x; final num friction = c.friction; assert(c.pointCount == 1 || c.pointCount == 2); // Solve tangent constraints for (int j = 0; j < c.pointCount; ++j){ ContactConstraintPoint ccp = c.points[j]; Vector a = ccp.rA; dv.x = -wB * ccp.rB.y + vB.x - vA.x + wA * a.y; dv.y = wB * ccp.rB.x + vB.y - vA.y - wA * a.x; // Compute tangent force num vt = dv.x * tangent.x + dv.y * tangent.y; num lambda = ccp.tangentMass * (-vt); // Clamp the accumulated force num maxFriction = friction * ccp.normalImpulse; num newImpulse = MathBox.clamp(ccp.tangentImpulse + lambda, -maxFriction, maxFriction); lambda = newImpulse - ccp.tangentImpulse; // Apply contact impulse num Px = tangent.x * lambda; num Py = tangent.y * lambda; //vA -= invMassA * P; vA.x -= Px * invMassA; vA.y -= Py * invMassA; wA -= invIA * (ccp.rA.x * Py - ccp.rA.y * Px); //vB += invMassB * P; vB.x += Px * invMassB; vB.y += Py * invMassB; wB += invIB * (ccp.rB.x * Py - ccp.rB.y * Px); ccp.tangentImpulse = newImpulse; } // Solve normal constraints if (c.pointCount == 1){ ContactConstraintPoint ccp = c.points[0]; Vector a1 = ccp.rA; dv.x = -wB * ccp.rB.y + vB.x - vA.x + wA * a1.y; dv.y = wB * ccp.rB.x + vB.y - vA.y - wA * a1.x; Vector b = c.normal; // Compute normal impulse num vn = dv.x * b.x + dv.y * b.y; num lambda = -ccp.normalMass * (vn - ccp.velocityBias); // Clamp the accumulated impulse num a = ccp.normalImpulse + lambda; num newImpulse = (a > 0.0 ? a : 0.0); lambda = newImpulse - ccp.normalImpulse; // Apply contact impulse num Px = c.normal.x * lambda; num Py = c.normal.y * lambda; //vA -= invMassA * P; vA.x -= Px * invMassA; vA.y -= Py * invMassA; wA -= invIA * (ccp.rA.x * Py - ccp.rA.y * Px); //vB += invMassB * P; vB.x += Px * invMassB; vB.y += Py * invMassB; wB += invIB * (ccp.rB.x * Py - ccp.rB.y * Px); ccp.normalImpulse = newImpulse; } else { ContactConstraintPoint cp1 = c.points[0]; ContactConstraintPoint cp2 = c.points[1]; Vector a = new Vector(cp1.normalImpulse, cp2.normalImpulse); assert(a.x >= 0.0 && a.y >= 0.0); // Relative velocity at contact //Vector 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; //Vector 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 num vn1 = dv1.x * c.normal.x + dv1.y * c.normal.y; num vn2 = dv2.x * c.normal.x + dv2.y * c.normal.y; Vector b = new Vector(vn1 - cp1.velocityBias, vn2 - cp2.velocityBias); temp2.x = c.K.col1.x * a.x + c.K.col2.x * a.y; temp2.y = c.K.col1.y * a.x + c.K.col2.y * a.y; b.x -= temp2.x; b.y -= temp2.y; while (true) { Matrix22.mulMatrixAndVectorToOut(c.normalMass, b, x); x.mulLocal(-1); if (x.x >= 0.0 && x.y >= 0.0){ // Resubstitute for the incremental impulse //Vector d = x - a; d.setFrom(x).subLocal(a); // Apply incremental impulse // Vector P1 = d.x * normal; // Vector P2 = d.y * normal; P1.setFrom(c.normal).mulLocal(d.x); P2.setFrom(c.normal).mulLocal(d.y); temp1.setFrom(P1).addLocal(P2); temp2.setFrom(temp1).mulLocal(invMassA); vA.subLocal(temp2); temp2.setFrom(temp1).mulLocal(invMassB); vB.addLocal(temp2); wA -= invIA * (Vector.crossVectors(cp1.rA, P1) + Vector.crossVectors(cp2.rA, P2)); wB += invIB * (Vector.crossVectors(cp1.rB, P1) + Vector.crossVectors(cp2.rB, P2)); // Accumulate cp1.normalImpulse = x.x; cp2.normalImpulse = x.y; break; } x.x = - cp1.normalMass * b.x; x.y = 0.0; vn1 = 0.0; vn2 = c.K.col1.y * x.x + b.y; if (x.x >= 0.0 && vn2 >= 0.0) { // Resubstitute for the incremental impulse d.setFrom(x).subLocal(a); // Apply incremental impulse P1.setFrom(c.normal).mulLocal(d.x); P2.setFrom(c.normal).mulLocal(d.y); temp1.setFrom(P1).addLocal(P2); temp2.setFrom(temp1).mulLocal(invMassA); vA.subLocal(temp2); temp2.setFrom(temp1).mulLocal(invMassB); vB.addLocal(temp2); wA -= invIA * (Vector.crossVectors(cp1.rA, P1) + Vector.crossVectors(cp2.rA, P2)); wB += invIB * (Vector.crossVectors(cp1.rB, P1) + Vector.crossVectors(cp2.rB, P2)); // Accumulate cp1.normalImpulse = x.x; cp2.normalImpulse = x.y; break; } x.x = 0.0; x.y = - cp2.normalMass * b.y; vn1 = c.K.col2.x * x.y + b.x; vn2 = 0.0; if (x.y >= 0.0 && vn1 >= 0.0) { // Resubstitute for the incremental impulse d.setFrom(x).subLocal(a); // Apply incremental impulse P1.setFrom(c.normal).mulLocal(d.x); P2.setFrom(c.normal).mulLocal(d.y); temp1.setFrom(P1).addLocal(P2); temp2.setFrom(temp1).mulLocal(invMassA); vA.subLocal(temp2); temp2.setFrom(temp1).mulLocal(invMassB); vB.addLocal(temp2); wA -= invIA * (Vector.crossVectors(cp1.rA, P1) + Vector.crossVectors(cp2.rA, P2)); wB += invIB * (Vector.crossVectors(cp1.rB, P1) + Vector.crossVectors(cp2.rB, P2)); // Accumulate cp1.normalImpulse = x.x; cp2.normalImpulse = x.y; break; } x.x = 0.0; x.y = 0.0; vn1 = b.x; vn2 = b.y; if (vn1 >= 0.0 && vn2 >= 0.0 ) { // Resubstitute for the incremental impulse d.setFrom(x).subLocal(a); // Apply incremental impulse P1.setFrom(c.normal).mulLocal(d.x); P2.setFrom(c.normal).mulLocal(d.y); temp1.setFrom(P1).addLocal(P2); temp2.setFrom(temp1).mulLocal(invMassA); vA.subLocal(temp2); temp2.setFrom(temp1).mulLocal(invMassB); vB.addLocal(temp2); wA -= invIA * (Vector.crossVectors(cp1.rA, P1) + Vector.crossVectors(cp2.rA, P2)); wB += invIB * (Vector.crossVectors(cp1.rB, P1) + Vector.crossVectors(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.linearVelocity.setFrom(vA); bodyA.angularVelocity = wA; bodyB.linearVelocity.setFrom(vB); bodyB.angularVelocity = wB; } } void storeImpulses(){ for( int i=0; i<constraintCount; i++){ ContactConstraint c = constraints[i]; Manifold m = c.manifold; for(int j=0; j< c.pointCount; j++){ m.points[j].normalImpulse = c.points[j].normalImpulse; m.points[j].tangentImpulse = c.points[j].tangentImpulse; } } } /** * Sequential solver. */ bool solvePositionConstraints(num baumgarte){ num minSeparation = 0.0; for (int i = 0; i < constraintCount; ++i){ final ContactConstraint c = constraints[i]; final Body bodyA = c.bodyA; final Body bodyB = c.bodyB; final num invMassA = bodyA.mass * bodyA.invMass; final num invIA = bodyA.mass * bodyA.invInertia; final num invMassB = bodyB.mass * bodyB.invMass; final num invIB = bodyB.mass * bodyB.invInertia; // Solve normal constraints for (int j = 0; j < c.pointCount; ++j){ PositionSolverManifold psm = psolver; psm.initialize(c, j); Vector normal = psm.normal; Vector point = psm.point; num separation = psm.separation; rA.setFrom(point).subLocal(bodyA.sweep.center); rB.setFrom(point).subLocal(bodyB.sweep.center); // Track max constraint error. minSeparation = Math.min(minSeparation, separation); // Prevent large corrections and allow slop. num C = MathBox.clamp(baumgarte * (separation + Settings.LINEAR_SLOP), -Settings.MAX_LINEAR_CORRECTION, 0.0); // Compute the effective mass. num rnA = Vector.crossVectors(rA, normal); num rnB = Vector.crossVectors(rB, normal); num K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB; // Compute normal impulse num impulse = K > 0.0 ? - C / K : 0.0; P.setFrom(normal).mulLocal(impulse); temp1.setFrom(P).mulLocal(invMassA); bodyA.sweep.center.subLocal(temp1);; bodyA.sweep.angle -= invIA * Vector.crossVectors(rA, P); bodyA.synchronizeTransform(); temp1.setFrom(P).mulLocal(invMassB); bodyB.sweep.center.addLocal(temp1); bodyB.sweep.angle += invIB * Vector.crossVectors(rB, P); bodyB.synchronizeTransform(); } } // We can't expect minSpeparation >= -LINEAR_SLOP because we don't // push the separation above -LINEAR_SLOP. return minSeparation >= -1.5 * Settings.LINEAR_SLOP; } }
Static Properties
const int INITIAL_NUM_CONSTRAINTS #
For each solver, this is the initial number of constraints in the array, which expands as needed.
static const int INITIAL_NUM_CONSTRAINTS = 256;
const num K_MAX_CONDITION_NUMBER #
Ensure a reasonable condition number. For the block solver
static const num K_MAX_CONDITION_NUMBER = 100.0;
Constructors
new ContactSolver() #
Constructs a new ContactSolver.
ContactSolver() : constraints = new List<ContactConstraint>(INITIAL_NUM_CONSTRAINTS), // Setup pool variables. worldManifold = new WorldManifold(), tangent = new Vector(), temp1 = new Vector(), temp2 = new Vector(), P = new Vector(), dv = new Vector(), dv1 = new Vector(), dv2 = new Vector(), x = new Vector(), d = new Vector(), P1 = new Vector(), P2 = new Vector(), psolver = new PositionSolverManifold(), rA = new Vector(), rB = new Vector() { for(int i=0; i < constraints.length; i++) { constraints[i] = new ContactConstraint(); } }
Properties
int constraintCount #
int constraintCount;
List<ContactConstraint> constraints #
List<ContactConstraint> constraints;
final PositionSolverManifold psolver #
final PositionSolverManifold psolver;
final WorldManifold worldManifold #
Pooling
final WorldManifold worldManifold;
Methods
void init(List<Contact> contacts, int contactCount, num impulseRatio) #
void init(List<Contact> contacts, int contactCount, num impulseRatio){ constraintCount = contactCount; // dynamic array if(constraints.length < contactCount){ List<ContactConstraint> old = constraints; int newLen = Math.max(old.length * 2, constraintCount); constraints = new List<ContactConstraint>(newLen); constraints.setRange(0, old.length, old); for(int i=old.length; i< constraints.length; i++){ constraints[i] = new ContactConstraint(); } } for (int i = 0; i < constraintCount; ++i){ Contact contact = contacts[i]; Fixture fixtureA = contact.fixtureA; Fixture fixtureB = contact.fixtureB; Shape shapeA = fixtureA.shape; Shape shapeB = fixtureB.shape; num radiusA = shapeA.radius; num radiusB = shapeB.radius; Body bodyA = fixtureA.body; Body bodyB = fixtureB.body; Manifold manifold = contact.manifold; num friction = Settings.mixFriction(fixtureA.friction, fixtureB.friction); num restitution = Settings.mixRestitution(fixtureA.restitution, fixtureB.restitution); Vector vA = bodyA.linearVelocity; Vector vB = bodyB.linearVelocity; num wA = bodyA.angularVelocity; num wB = bodyB.angularVelocity; assert(manifold.pointCount > 0); worldManifold.initialize(manifold, bodyA.originTransform, radiusA, bodyB.originTransform, radiusB); ContactConstraint cc = constraints[i]; cc.bodyA = bodyA; cc.bodyB = bodyB; cc.manifold = manifold; cc.normal.x = worldManifold.normal.x; cc.normal.y = worldManifold.normal.y; // have to set actual manifold cc.pointCount = manifold.pointCount; cc.friction = friction; cc.restitution = restitution; cc.localNormal.x = manifold.localNormal.x; cc.localNormal.y = manifold.localNormal.y; cc.localPoint.x = manifold.localPoint.x; cc.localPoint.y = manifold.localPoint.y; cc.radius = radiusA + radiusB; cc.type = manifold.type; for (int j = 0; j < cc.pointCount; ++j){ ManifoldPoint cp = manifold.points[j]; ContactConstraintPoint ccp = cc.points[j]; ccp.normalImpulse = impulseRatio * cp.normalImpulse; ccp.tangentImpulse = impulseRatio * cp.tangentImpulse; ccp.localPoint.x = cp.localPoint.x; ccp.localPoint.y = cp.localPoint.y; ccp.rA.x = worldManifold.points[j].x - bodyA.sweep.center.x; ccp.rA.y = worldManifold.points[j].y - bodyA.sweep.center.y; ccp.rB.x = worldManifold.points[j].x - bodyB.sweep.center.x; ccp.rB.y = worldManifold.points[j].y - bodyB.sweep.center.y; num rnA = ccp.rA.x * cc.normal.y - ccp.rA.y * cc.normal.x; num rnB = ccp.rB.x * cc.normal.y - ccp.rB.y * cc.normal.x; rnA *= rnA; rnB *= rnB; num kNormal = bodyA.invMass + bodyB.invMass + bodyA.invInertia * rnA + bodyB.invInertia * rnB; assert(kNormal > Settings.EPSILON); ccp.normalMass = 1.0 / kNormal; tangent.x = 1.0 * cc.normal.y; tangent.y = -1.0 * cc.normal.x; num rtA = ccp.rA.x * tangent.y - ccp.rA.y * tangent.x; num rtB = ccp.rB.x * tangent.y - ccp.rB.y * tangent.x; rtA *= rtA; rtB *= rtB; num kTangent = bodyA.invMass + bodyB.invMass + bodyA.invInertia * rtA + bodyB.invInertia * rtB; assert(kTangent > Settings.EPSILON); ccp.tangentMass = 1.0 / kTangent; // Setup a velocity bias for restitution. ccp.velocityBias = 0.0; temp2.x = -wA * ccp.rA.y; temp2.y = wA * ccp.rA.x; temp1.x = -wB * ccp.rB.y + vB.x - vA.x - temp2.x; temp1.y = wB * ccp.rB.x + vB.y - vA.y - temp2.y; Vector a = cc.normal; num vRel = a.x * temp1.x + a.y * temp1.y; if (vRel < -Settings.VELOCITY_THRESHOLD){ ccp.velocityBias = -restitution * vRel; } } // If we have two points, then prepare the block solver. if (cc.pointCount == 2){ ContactConstraintPoint ccp1 = cc.points[0]; ContactConstraintPoint ccp2 = cc.points[1]; num invMassA = bodyA.invMass; num invIA = bodyA.invInertia; num invMassB = bodyB.invMass; num invIB = bodyB.invInertia; num rn1A = Vector.crossVectors(ccp1.rA, cc.normal); num rn1B = Vector.crossVectors(ccp1.rB, cc.normal); num rn2A = Vector.crossVectors(ccp2.rA, cc.normal); num rn2B = Vector.crossVectors(ccp2.rB, cc.normal); num k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B; num k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B; num k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B; // Ensure a reasonable condition number. if (k11 * k11 < K_MAX_CONDITION_NUMBER * (k11 * k22 - k12 * k12)) { // K is safe to invert. cc.K.col1.x = k11; cc.K.col1.y = k12; cc.K.col2.x = k12; cc.K.col2.y = k22; cc.normalMass.col1.x = cc.K.col1.x; cc.normalMass.col1.y = cc.K.col1.y; cc.normalMass.col2.x = cc.K.col2.x; cc.normalMass.col2.y = cc.K.col2.y; cc.normalMass.invertLocal(); } else{ // The constraints are redundant, just use one. cc.pointCount = 1; } } } }
bool solvePositionConstraints(num baumgarte) #
Sequential solver.
bool solvePositionConstraints(num baumgarte){ num minSeparation = 0.0; for (int i = 0; i < constraintCount; ++i){ final ContactConstraint c = constraints[i]; final Body bodyA = c.bodyA; final Body bodyB = c.bodyB; final num invMassA = bodyA.mass * bodyA.invMass; final num invIA = bodyA.mass * bodyA.invInertia; final num invMassB = bodyB.mass * bodyB.invMass; final num invIB = bodyB.mass * bodyB.invInertia; // Solve normal constraints for (int j = 0; j < c.pointCount; ++j){ PositionSolverManifold psm = psolver; psm.initialize(c, j); Vector normal = psm.normal; Vector point = psm.point; num separation = psm.separation; rA.setFrom(point).subLocal(bodyA.sweep.center); rB.setFrom(point).subLocal(bodyB.sweep.center); // Track max constraint error. minSeparation = Math.min(minSeparation, separation); // Prevent large corrections and allow slop. num C = MathBox.clamp(baumgarte * (separation + Settings.LINEAR_SLOP), -Settings.MAX_LINEAR_CORRECTION, 0.0); // Compute the effective mass. num rnA = Vector.crossVectors(rA, normal); num rnB = Vector.crossVectors(rB, normal); num K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB; // Compute normal impulse num impulse = K > 0.0 ? - C / K : 0.0; P.setFrom(normal).mulLocal(impulse); temp1.setFrom(P).mulLocal(invMassA); bodyA.sweep.center.subLocal(temp1);; bodyA.sweep.angle -= invIA * Vector.crossVectors(rA, P); bodyA.synchronizeTransform(); temp1.setFrom(P).mulLocal(invMassB); bodyB.sweep.center.addLocal(temp1); bodyB.sweep.angle += invIB * Vector.crossVectors(rB, P); bodyB.synchronizeTransform(); } } // We can't expect minSpeparation >= -LINEAR_SLOP because we don't // push the separation above -LINEAR_SLOP. return minSeparation >= -1.5 * Settings.LINEAR_SLOP; }
void solveVelocityConstraints() #
void solveVelocityConstraints(){ for (int i = 0; i < constraintCount; ++i){ final ContactConstraint c = constraints[i]; final Body bodyA = c.bodyA; final Body bodyB = c.bodyB; num wA = bodyA.angularVelocity; num wB = bodyB.angularVelocity; final Vector vA = bodyA.linearVelocity; final Vector vB = bodyB.linearVelocity; final num invMassA = bodyA.invMass; final num invIA = bodyA.invInertia; final num invMassB = bodyB.invMass; final num invIB = bodyB.invInertia; tangent.x = 1.0 * c.normal.y; tangent.y = -1.0 * c.normal.x; final num friction = c.friction; assert(c.pointCount == 1 || c.pointCount == 2); // Solve tangent constraints for (int j = 0; j < c.pointCount; ++j){ ContactConstraintPoint ccp = c.points[j]; Vector a = ccp.rA; dv.x = -wB * ccp.rB.y + vB.x - vA.x + wA * a.y; dv.y = wB * ccp.rB.x + vB.y - vA.y - wA * a.x; // Compute tangent force num vt = dv.x * tangent.x + dv.y * tangent.y; num lambda = ccp.tangentMass * (-vt); // Clamp the accumulated force num maxFriction = friction * ccp.normalImpulse; num newImpulse = MathBox.clamp(ccp.tangentImpulse + lambda, -maxFriction, maxFriction); lambda = newImpulse - ccp.tangentImpulse; // Apply contact impulse num Px = tangent.x * lambda; num Py = tangent.y * lambda; //vA -= invMassA * P; vA.x -= Px * invMassA; vA.y -= Py * invMassA; wA -= invIA * (ccp.rA.x * Py - ccp.rA.y * Px); //vB += invMassB * P; vB.x += Px * invMassB; vB.y += Py * invMassB; wB += invIB * (ccp.rB.x * Py - ccp.rB.y * Px); ccp.tangentImpulse = newImpulse; } // Solve normal constraints if (c.pointCount == 1){ ContactConstraintPoint ccp = c.points[0]; Vector a1 = ccp.rA; dv.x = -wB * ccp.rB.y + vB.x - vA.x + wA * a1.y; dv.y = wB * ccp.rB.x + vB.y - vA.y - wA * a1.x; Vector b = c.normal; // Compute normal impulse num vn = dv.x * b.x + dv.y * b.y; num lambda = -ccp.normalMass * (vn - ccp.velocityBias); // Clamp the accumulated impulse num a = ccp.normalImpulse + lambda; num newImpulse = (a > 0.0 ? a : 0.0); lambda = newImpulse - ccp.normalImpulse; // Apply contact impulse num Px = c.normal.x * lambda; num Py = c.normal.y * lambda; //vA -= invMassA * P; vA.x -= Px * invMassA; vA.y -= Py * invMassA; wA -= invIA * (ccp.rA.x * Py - ccp.rA.y * Px); //vB += invMassB * P; vB.x += Px * invMassB; vB.y += Py * invMassB; wB += invIB * (ccp.rB.x * Py - ccp.rB.y * Px); ccp.normalImpulse = newImpulse; } else { ContactConstraintPoint cp1 = c.points[0]; ContactConstraintPoint cp2 = c.points[1]; Vector a = new Vector(cp1.normalImpulse, cp2.normalImpulse); assert(a.x >= 0.0 && a.y >= 0.0); // Relative velocity at contact //Vector 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; //Vector 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 num vn1 = dv1.x * c.normal.x + dv1.y * c.normal.y; num vn2 = dv2.x * c.normal.x + dv2.y * c.normal.y; Vector b = new Vector(vn1 - cp1.velocityBias, vn2 - cp2.velocityBias); temp2.x = c.K.col1.x * a.x + c.K.col2.x * a.y; temp2.y = c.K.col1.y * a.x + c.K.col2.y * a.y; b.x -= temp2.x; b.y -= temp2.y; while (true) { Matrix22.mulMatrixAndVectorToOut(c.normalMass, b, x); x.mulLocal(-1); if (x.x >= 0.0 && x.y >= 0.0){ // Resubstitute for the incremental impulse //Vector d = x - a; d.setFrom(x).subLocal(a); // Apply incremental impulse // Vector P1 = d.x * normal; // Vector P2 = d.y * normal; P1.setFrom(c.normal).mulLocal(d.x); P2.setFrom(c.normal).mulLocal(d.y); temp1.setFrom(P1).addLocal(P2); temp2.setFrom(temp1).mulLocal(invMassA); vA.subLocal(temp2); temp2.setFrom(temp1).mulLocal(invMassB); vB.addLocal(temp2); wA -= invIA * (Vector.crossVectors(cp1.rA, P1) + Vector.crossVectors(cp2.rA, P2)); wB += invIB * (Vector.crossVectors(cp1.rB, P1) + Vector.crossVectors(cp2.rB, P2)); // Accumulate cp1.normalImpulse = x.x; cp2.normalImpulse = x.y; break; } x.x = - cp1.normalMass * b.x; x.y = 0.0; vn1 = 0.0; vn2 = c.K.col1.y * x.x + b.y; if (x.x >= 0.0 && vn2 >= 0.0) { // Resubstitute for the incremental impulse d.setFrom(x).subLocal(a); // Apply incremental impulse P1.setFrom(c.normal).mulLocal(d.x); P2.setFrom(c.normal).mulLocal(d.y); temp1.setFrom(P1).addLocal(P2); temp2.setFrom(temp1).mulLocal(invMassA); vA.subLocal(temp2); temp2.setFrom(temp1).mulLocal(invMassB); vB.addLocal(temp2); wA -= invIA * (Vector.crossVectors(cp1.rA, P1) + Vector.crossVectors(cp2.rA, P2)); wB += invIB * (Vector.crossVectors(cp1.rB, P1) + Vector.crossVectors(cp2.rB, P2)); // Accumulate cp1.normalImpulse = x.x; cp2.normalImpulse = x.y; break; } x.x = 0.0; x.y = - cp2.normalMass * b.y; vn1 = c.K.col2.x * x.y + b.x; vn2 = 0.0; if (x.y >= 0.0 && vn1 >= 0.0) { // Resubstitute for the incremental impulse d.setFrom(x).subLocal(a); // Apply incremental impulse P1.setFrom(c.normal).mulLocal(d.x); P2.setFrom(c.normal).mulLocal(d.y); temp1.setFrom(P1).addLocal(P2); temp2.setFrom(temp1).mulLocal(invMassA); vA.subLocal(temp2); temp2.setFrom(temp1).mulLocal(invMassB); vB.addLocal(temp2); wA -= invIA * (Vector.crossVectors(cp1.rA, P1) + Vector.crossVectors(cp2.rA, P2)); wB += invIB * (Vector.crossVectors(cp1.rB, P1) + Vector.crossVectors(cp2.rB, P2)); // Accumulate cp1.normalImpulse = x.x; cp2.normalImpulse = x.y; break; } x.x = 0.0; x.y = 0.0; vn1 = b.x; vn2 = b.y; if (vn1 >= 0.0 && vn2 >= 0.0 ) { // Resubstitute for the incremental impulse d.setFrom(x).subLocal(a); // Apply incremental impulse P1.setFrom(c.normal).mulLocal(d.x); P2.setFrom(c.normal).mulLocal(d.y); temp1.setFrom(P1).addLocal(P2); temp2.setFrom(temp1).mulLocal(invMassA); vA.subLocal(temp2); temp2.setFrom(temp1).mulLocal(invMassB); vB.addLocal(temp2); wA -= invIA * (Vector.crossVectors(cp1.rA, P1) + Vector.crossVectors(cp2.rA, P2)); wB += invIB * (Vector.crossVectors(cp1.rB, P1) + Vector.crossVectors(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.linearVelocity.setFrom(vA); bodyA.angularVelocity = wA; bodyB.linearVelocity.setFrom(vB); bodyB.angularVelocity = wB; } }
void storeImpulses() #
void storeImpulses(){ for( int i=0; i<constraintCount; i++){ ContactConstraint c = constraints[i]; Manifold m = c.manifold; for(int j=0; j< c.pointCount; j++){ m.points[j].normalImpulse = c.points[j].normalImpulse; m.points[j].tangentImpulse = c.points[j].tangentImpulse; } } }
void warmStart() #
void warmStart(){ // Warm start. for (int i = 0; i < constraintCount; ++i){ ContactConstraint c = constraints[i]; final Body bodyA = c.bodyA; final Body bodyB = c.bodyB; final num invMassA = bodyA.invMass; final num invIA = bodyA.invInertia; final num invMassB = bodyB.invMass; final num invIB = bodyB.invInertia; final Vector normal = c.normal; Vector.crossVectorAndNumToOut(normal, 1, tangent); for (int j = 0; j < c.pointCount; ++j){ ContactConstraintPoint ccp = c.points[j]; num Px = ccp.normalImpulse * normal.x + ccp.tangentImpulse * tangent.x; num Py = ccp.normalImpulse * normal.y + ccp.tangentImpulse * tangent.y; bodyA.angularVelocity -= invIA * (ccp.rA.x * Py - ccp.rA.y * Px); bodyA.linearVelocity.x -= Px * invMassA; bodyA.linearVelocity.y -= Py * invMassA; bodyB.angularVelocity += invIB * (ccp.rB.x * Py - ccp.rB.y * Px); bodyB.linearVelocity.x += Px * invMassB; bodyB.linearVelocity.y += Py * invMassB; } } }