Dart Documentationbox2dContactSolver

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 vec2 tangent;
 final vec2 temp1;
 final vec2 temp2;
 final vec2 P;
 final vec2 dv;
 final vec2 dv1;
 final vec2 dv2;
 final vec2 x;
 final vec2 d;
 final vec2 P1;
 final vec2 P2;
 final PositionSolverManifold psolver;
 final vec2 rA;
 final vec2 rB;

 /** Constructs a new ContactSolver. */
 ContactSolver() :
   constraints = new List<ContactConstraint>(INITIAL_NUM_CONSTRAINTS),

   // Setup pool variables.
   worldManifold = new WorldManifold(),
   tangent = new vec2.zero(),
   temp1 = new vec2.zero(),
   temp2 = new vec2.zero(),
   P = new vec2.zero(),
   dv = new vec2.zero(),
   dv1 = new vec2.zero(),
   dv2 = new vec2.zero(),
   x = new vec2.zero(),
   d = new vec2.zero(),
   P1 = new vec2.zero(),
   P2 = new vec2.zero(),
   psolver = new PositionSolverManifold(),
   rA = new vec2.zero(),
   rB = new vec2.zero() {
   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);

     vec2 vA = bodyA.linearVelocity;
     vec2 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;

       vec2 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 = cross(ccp1.rA, cc.normal);
       num rn1B = cross(ccp1.rB, cc.normal);
       num rn2A = cross(ccp2.rA, cc.normal);
       num rn2B = cross(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.col0.x = k11;
         cc.K.col0.y = k12;
         cc.K.col1.x = k12;
         cc.K.col1.y = k22;
         cc.normalMass.col0.x = cc.K.col0.x;
         cc.normalMass.col0.y = cc.K.col0.y;
         cc.normalMass.col1.x = cc.K.col1.x;
         cc.normalMass.col1.y = cc.K.col1.y;
         cc.normalMass.invert();
       } 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 vec2 normal = c.normal;
     cross(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 vec2 vA = bodyA.linearVelocity;
     final vec2 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];
       vec2 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 = 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];
       vec2 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;
       vec2 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];
       vec2 a = new vec2(cp1.normalImpulse, cp2.normalImpulse);

       assert(a.x >= 0.0 && a.y >= 0.0);
       // 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
       num vn1 = dv1.x * c.normal.x + dv1.y * c.normal.y;
       num vn2 = dv2.x * c.normal.x + dv2.y * c.normal.y;

       vec2 b = new vec2(vn1 - cp1.velocityBias, vn2 - cp2.velocityBias);
       temp2.x = c.K.col0.x * a.x + c.K.col1.x * a.y;
       temp2.y = c.K.col0.y * a.x + c.K.col1.y * a.y;
       b.x -= temp2.x;
       b.y -= temp2.y;

       while (true) {
         c.normalMass.transformed(b, x);
         x.negate();

         if (x.x >= 0.0 && x.y >= 0.0){
           // Resubstitute for the incremental impulse
           //vec2 d = x - a;
           d.copyFrom(x).sub(a);

           // Apply incremental impulse
           // vec2 P1 = d.x * normal;
           // vec2 P2 = d.y * normal;
           P1.copyFrom(c.normal).scale(d.x);
           P2.copyFrom(c.normal).scale(d.y);

           temp1.copyFrom(P1).add(P2);
           temp2.copyFrom(temp1).scale(invMassA);
           vA.sub(temp2);
           temp2.copyFrom(temp1).scale(invMassB);
           vB.add(temp2);

           wA -= invIA * (cross(cp1.rA, P1) + cross(cp2.rA, P2));
           wB += invIB * (cross(cp1.rB, P1) + cross(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.col0.y * x.x + b.y;

         if (x.x >= 0.0 && vn2 >= 0.0) {
           // Resubstitute for the incremental impulse
           d.copyFrom(x).sub(a);

           // Apply incremental impulse
           P1.copyFrom(c.normal).scale(d.x);
           P2.copyFrom(c.normal).scale(d.y);

           temp1.copyFrom(P1).add(P2);
           temp2.copyFrom(temp1).scale(invMassA);
           vA.sub(temp2);
           temp2.copyFrom(temp1).scale(invMassB);
           vB.add(temp2);

           wA -= invIA * (cross(cp1.rA, P1) + cross(cp2.rA, P2));
           wB += invIB * (cross(cp1.rB, P1) + cross(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.col1.x * x.y + b.x;
         vn2 = 0.0;

         if (x.y >= 0.0 && vn1 >= 0.0) {
           // Resubstitute for the incremental impulse
           d.copyFrom(x).sub(a);

           // Apply incremental impulse
           P1.copyFrom(c.normal).scale(d.x);
           P2.copyFrom(c.normal).scale(d.y);

           temp1.copyFrom(P1).add(P2);
           temp2.copyFrom(temp1).scale(invMassA);
           vA.sub(temp2);
           temp2.copyFrom(temp1).scale(invMassB);
           vB.add(temp2);

           wA -= invIA * (cross(cp1.rA, P1) + cross(cp2.rA, P2));
           wB += invIB * (cross(cp1.rB, P1) + cross(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.copyFrom(x).sub(a);

           // Apply incremental impulse
           P1.copyFrom(c.normal).scale(d.x);
           P2.copyFrom(c.normal).scale(d.y);

           temp1.copyFrom(P1).add(P2);
           temp2.copyFrom(temp1).scale(invMassA);
           vA.sub(temp2);
           temp2.copyFrom(temp1).scale(invMassB);
           vB.add(temp2);

           wA -= invIA * (cross(cp1.rA, P1) + cross(cp2.rA, P2));
           wB += invIB * (cross(cp1.rB, P1) + cross(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.copyFrom(vA);
     bodyA.angularVelocity = wA;
     bodyB.linearVelocity.copyFrom(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);
       vec2 normal = psm.normal;

       vec2 point = psm.point;
       num separation = psm.separation;

       rA.copyFrom(point).sub(bodyA.sweep.center);
       rB.copyFrom(point).sub(bodyB.sweep.center);

       // Track max constraint error.
       minSeparation = math.min(minSeparation, separation);

       // Prevent large corrections and allow slop.
       num C = clamp(baumgarte * (separation + Settings.LINEAR_SLOP),
                     -Settings.MAX_LINEAR_CORRECTION, 0.0);

       // Compute the effective mass.
       num rnA = cross(rA, normal);
       num rnB = cross(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.copyFrom(normal).scale(impulse);

       temp1.copyFrom(P).scale(invMassA);
       bodyA.sweep.center.sub(temp1);;
       bodyA.sweep.angle -= invIA * cross(rA, P);
       bodyA.synchronizeTransform();

       temp1.copyFrom(P).scale(invMassB);
       bodyB.sweep.center.add(temp1);
       bodyB.sweep.angle += invIB * cross(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 #

INITIAL_NUM_CONSTRAINTS = 256

const num K_MAX_CONDITION_NUMBER #

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 vec2.zero(),
 temp1 = new vec2.zero(),
 temp2 = new vec2.zero(),
 P = new vec2.zero(),
 dv = new vec2.zero(),
 dv1 = new vec2.zero(),
 dv2 = new vec2.zero(),
 x = new vec2.zero(),
 d = new vec2.zero(),
 P1 = new vec2.zero(),
 P2 = new vec2.zero(),
 psolver = new PositionSolverManifold(),
 rA = new vec2.zero(),
 rB = new vec2.zero() {
 for(int i=0; i < constraints.length; i++) {
   constraints[i] = new ContactConstraint();
 }
}

Properties

int constraintCount #

constraintCount

List<ContactConstraint> constraints #

constraints

final vec2 d #

d

final vec2 dv #

dv

final vec2 dv1 #

dv1

final vec2 dv2 #

dv2

final vec2 P #

P

final vec2 P1 #

P1

final vec2 P2 #

P2

final PositionSolverManifold psolver #

psolver

final vec2 rA #

rA

final vec2 rB #

rB

final vec2 tangent #

tangent

final vec2 temp1 #

temp1

final vec2 temp2 #

temp2

final WorldManifold worldManifold #

worldManifold

final vec2 x #

x

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);

   vec2 vA = bodyA.linearVelocity;
   vec2 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;

     vec2 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 = cross(ccp1.rA, cc.normal);
     num rn1B = cross(ccp1.rB, cc.normal);
     num rn2A = cross(ccp2.rA, cc.normal);
     num rn2B = cross(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.col0.x = k11;
       cc.K.col0.y = k12;
       cc.K.col1.x = k12;
       cc.K.col1.y = k22;
       cc.normalMass.col0.x = cc.K.col0.x;
       cc.normalMass.col0.y = cc.K.col0.y;
       cc.normalMass.col1.x = cc.K.col1.x;
       cc.normalMass.col1.y = cc.K.col1.y;
       cc.normalMass.invert();
     } 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);
     vec2 normal = psm.normal;

     vec2 point = psm.point;
     num separation = psm.separation;

     rA.copyFrom(point).sub(bodyA.sweep.center);
     rB.copyFrom(point).sub(bodyB.sweep.center);

     // Track max constraint error.
     minSeparation = math.min(minSeparation, separation);

     // Prevent large corrections and allow slop.
     num C = clamp(baumgarte * (separation + Settings.LINEAR_SLOP),
                   -Settings.MAX_LINEAR_CORRECTION, 0.0);

     // Compute the effective mass.
     num rnA = cross(rA, normal);
     num rnB = cross(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.copyFrom(normal).scale(impulse);

     temp1.copyFrom(P).scale(invMassA);
     bodyA.sweep.center.sub(temp1);;
     bodyA.sweep.angle -= invIA * cross(rA, P);
     bodyA.synchronizeTransform();

     temp1.copyFrom(P).scale(invMassB);
     bodyB.sweep.center.add(temp1);
     bodyB.sweep.angle += invIB * cross(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 vec2 vA = bodyA.linearVelocity;
   final vec2 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];
     vec2 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 = 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];
     vec2 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;
     vec2 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];
     vec2 a = new vec2(cp1.normalImpulse, cp2.normalImpulse);

     assert(a.x >= 0.0 && a.y >= 0.0);
     // 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
     num vn1 = dv1.x * c.normal.x + dv1.y * c.normal.y;
     num vn2 = dv2.x * c.normal.x + dv2.y * c.normal.y;

     vec2 b = new vec2(vn1 - cp1.velocityBias, vn2 - cp2.velocityBias);
     temp2.x = c.K.col0.x * a.x + c.K.col1.x * a.y;
     temp2.y = c.K.col0.y * a.x + c.K.col1.y * a.y;
     b.x -= temp2.x;
     b.y -= temp2.y;

     while (true) {
       c.normalMass.transformed(b, x);
       x.negate();

       if (x.x >= 0.0 && x.y >= 0.0){
         // Resubstitute for the incremental impulse
         //vec2 d = x - a;
         d.copyFrom(x).sub(a);

         // Apply incremental impulse
         // vec2 P1 = d.x * normal;
         // vec2 P2 = d.y * normal;
         P1.copyFrom(c.normal).scale(d.x);
         P2.copyFrom(c.normal).scale(d.y);

         temp1.copyFrom(P1).add(P2);
         temp2.copyFrom(temp1).scale(invMassA);
         vA.sub(temp2);
         temp2.copyFrom(temp1).scale(invMassB);
         vB.add(temp2);

         wA -= invIA * (cross(cp1.rA, P1) + cross(cp2.rA, P2));
         wB += invIB * (cross(cp1.rB, P1) + cross(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.col0.y * x.x + b.y;

       if (x.x >= 0.0 && vn2 >= 0.0) {
         // Resubstitute for the incremental impulse
         d.copyFrom(x).sub(a);

         // Apply incremental impulse
         P1.copyFrom(c.normal).scale(d.x);
         P2.copyFrom(c.normal).scale(d.y);

         temp1.copyFrom(P1).add(P2);
         temp2.copyFrom(temp1).scale(invMassA);
         vA.sub(temp2);
         temp2.copyFrom(temp1).scale(invMassB);
         vB.add(temp2);

         wA -= invIA * (cross(cp1.rA, P1) + cross(cp2.rA, P2));
         wB += invIB * (cross(cp1.rB, P1) + cross(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.col1.x * x.y + b.x;
       vn2 = 0.0;

       if (x.y >= 0.0 && vn1 >= 0.0) {
         // Resubstitute for the incremental impulse
         d.copyFrom(x).sub(a);

         // Apply incremental impulse
         P1.copyFrom(c.normal).scale(d.x);
         P2.copyFrom(c.normal).scale(d.y);

         temp1.copyFrom(P1).add(P2);
         temp2.copyFrom(temp1).scale(invMassA);
         vA.sub(temp2);
         temp2.copyFrom(temp1).scale(invMassB);
         vB.add(temp2);

         wA -= invIA * (cross(cp1.rA, P1) + cross(cp2.rA, P2));
         wB += invIB * (cross(cp1.rB, P1) + cross(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.copyFrom(x).sub(a);

         // Apply incremental impulse
         P1.copyFrom(c.normal).scale(d.x);
         P2.copyFrom(c.normal).scale(d.y);

         temp1.copyFrom(P1).add(P2);
         temp2.copyFrom(temp1).scale(invMassA);
         vA.sub(temp2);
         temp2.copyFrom(temp1).scale(invMassB);
         vB.add(temp2);

         wA -= invIA * (cross(cp1.rA, P1) + cross(cp2.rA, P2));
         wB += invIB * (cross(cp1.rB, P1) + cross(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.copyFrom(vA);
   bodyA.angularVelocity = wA;
   bodyB.linearVelocity.copyFrom(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 vec2 normal = c.normal;
   cross(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;
   }
 }
}