planck-js/lib/Contact.js

2D physics engine for JavaScript/HTML5 game development

/*
 * Copyright (c) 2016      Ali Shakiba http://shakiba.me/planck.js
 * Copyright (c) 2006-2011 Erin Catto  http://www.box2d.org
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
 * arising from the use of this software.
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 * 1. The origin of this software must not be misrepresented; you must not
 * claim that you wrote the original software. If you use this software
 * in a product, an acknowledgment in the product documentation would be
 * appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 * misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */

var DEBUG_SOLVER = false;

var Settings = require('./Settings');
var Manifold = require('./Manifold');

var Math = require('./common/Math');
var Vec2 = require('./common/Vec2');
var Transform = require('./common/Transform');
var Mat22 = require('./common/Mat22');
var Rot = require('./common/Rot');

module.exports = Contact;

/**
 * A contact edge is used to connect bodies and contacts together in a contact
 * graph where each body is a node and each contact is an edge. A contact edge
 * belongs to a doubly linked list maintained in each attached body. Each
 * contact has two contact nodes, one for each attached body.
 * 
 * @prop {Contact} contact The contact
 * @prop {ContactEdge} prev The previous contact edge in the body's contact list
 * @prop {ContactEdge} next The next contact edge in the body's contact list
 * @prop {Body} other Provides quick access to the other body attached.
 */
function ContactEdge(contact) {
  this.contact = contact;
  this.prev;
  this.next;
  this.other;
};

/**
 * @function Contact~Evaluate
 * 
 * @param manifold
 * @param xfA
 * @param fixtureA
 * @param indexA
 * @param xfB
 * @param fixtureB
 * @param indexB
 */

/**
 * The class manages contact between two shapes. A contact exists for each
 * overlapping AABB in the broad-phase (except if filtered). Therefore a contact
 * object may exist that has no contact points.
 * 
 * @param {Fixture} fA
 * @param {int} indexA
 * @param {Fixture} fB
 * @param {int} indexB
 * @param {Contact~Evaluate} evaluateFcn
 */
function Contact(fA, indexA, fB, indexB, evaluateFcn) {

  // Nodes for connecting bodies.
  this.m_nodeA = new ContactEdge(this);
  this.m_nodeB = new ContactEdge(this);

  this.m_fixtureA = fA;
  this.m_fixtureB = fB;

  this.m_indexA = indexA;
  this.m_indexB = indexB;

  this.m_evaluateFcn = evaluateFcn;

  this.m_manifold = new Manifold();

  this.m_prev = null;
  this.m_next = null;

  this.m_toi = 1.0;
  this.m_toiCount = 0;

  this.m_friction = MixFriction(this.m_fixtureA.m_friction,
      this.m_fixtureB.m_friction);
  this.m_restitution = MixRestitution(this.m_fixtureA.m_restitution,
      this.m_fixtureB.m_restitution);

  this.m_tangentSpeed = 0.0;

  // This contact can be disabled (by user)
  this.m_enabledFlag = true;

  // Used when crawling contact graph when forming islands.
  this.m_islandFlag = false;

  // Set when the shapes are touching.
  this.m_touchingFlag = false;

  // This contact needs filtering because a fixture filter was changed.
  this.m_filterFlag = false;

  // This bullet contact had a TOI event
  this.m_bulletHitFlag = false;

  // This contact has a valid TOI in m_toi
  this.m_toiFlag = false;

  // VelocityConstraint
  this.v_points = []; // VelocityConstraintPoint[maxManifoldPoints]
  this.v_K = new Mat22();
  this.v_normalMass = new Mat22();
  this.v_normal = Vec2();
};

/**
 * Get the contact manifold. Do not modify the manifold unless you understand
 * the internals of the library.
 */
Contact.prototype.GetManifold = function() {
  return this.m_manifold;
}

/**
 * Get the world manifold.
 * 
 * @param {WorldManifold} [worldManifold]
 */
Contact.prototype.GetWorldManifold = function(worldManifold) {
  var bodyA = this.m_fixtureA.GetBody();
  var bodyB = this.m_fixtureB.GetBody();
  var shapeA = this.m_fixtureA.GetShape();
  var shapeB = this.m_fixtureB.GetShape();

  return this.m_manifold.GetWorldManifold(worldManifold, bodyA.GetTransform(),
      shapeA.m_radius, bodyB.GetTransform(), shapeB.m_radius);
}

/**
 * Enable/disable this contact. This can be used inside the pre-solve contact
 * listener. The contact is only disabled for the current time step (or sub-step
 * in continuous collisions).
 */
Contact.prototype.SetEnabled = function(flag) {
  this.m_enabledFlag = !!flag;
}

/**
 * Has this contact been disabled?
 */
Contact.prototype.IsEnabled = function() {
  return this.m_enabledFlag;
}

/**
 * Is this contact touching?
 */
Contact.prototype.IsTouching = function() {
  return this.m_touchingFlag;
}

/**
 * Get the next contact in the world's contact list.
 */
Contact.prototype.GetNext = function() {
  return this.m_next;
}

/**
 * Get fixture A in this contact.
 */
Contact.prototype.GetFixtureA = function() {
  return this.m_fixtureA;
}

/**
 * Get fixture B in this contact.
 */
Contact.prototype.GetFixtureB = function() {
  return this.m_fixtureB;
}

/**
 * Get the child primitive index for fixture A.
 */
Contact.prototype.GetChildIndexA = function() {
  return this.m_indexA;
}

/**
 * Get the child primitive index for fixture B.
 */
Contact.prototype.GetChildIndexB = function() {
  return this.m_indexB;
}

/**
 * Flag this contact for filtering. Filtering will occur the next time step.
 */
Contact.prototype.FlagForFiltering = function() {
  this.m_filterFlag = true;
}

/**
 * Override the default friction mixture. You can call this in
 * ContactListener.PreSolve. This value persists until set or reset.
 */
Contact.prototype.SetFriction = function(friction) {
  this.m_friction = friction;
}

/**
 * Get the friction.
 */
Contact.prototype.GetFriction = function() {
  return this.m_friction;
}

/**
 * Reset the friction mixture to the default value.
 */
Contact.prototype.ResetFriction = function() {
  this.m_friction = MixFriction(this.m_fixtureA.m_friction,
      this.m_fixtureB.m_friction);
}

/**
 * Override the default restitution mixture. You can call this in
 * ContactListener.PreSolve. The value persists until you set or reset.
 */
Contact.prototype.SetRestitution = function(restitution) {
  this.m_restitution = restitution;
}

/**
 * Get the restitution.
 */
Contact.prototype.GetRestitution = function() {
  return this.m_restitution;
}

/**
 * Reset the restitution to the default value.
 */
Contact.prototype.ResetRestitution = function() {
  this.m_restitution = MixRestitution(this.m_fixtureA.m_restitution,
      this.m_fixtureB.m_restitution);
}

/**
 * Set the desired tangent speed for a conveyor belt behavior. In meters per
 * second.
 */
Contact.prototype.SetTangentSpeed = function(speed) {
  this.m_tangentSpeed = speed;
}

/**
 * Get the desired tangent speed. In meters per second.
 */
Contact.prototype.GetTangentSpeed = function() {
  return this.m_tangentSpeed;
}

/**
 * Called by Update method, and implemented by subclasses.
 */
Contact.prototype.Evaluate = function(manifold, xfA, xfB) {
  this.m_evaluateFcn(manifold, xfA, this.m_fixtureA, this.m_indexA, xfB,
      this.m_fixtureB, this.m_indexB);
};

/**
 * Updates the contact manifold and touching status.
 * 
 * Note: do not assume the fixture AABBs are overlapping or are valid.
 * 
 * @param {function} listener.BeginContact
 * @param {function} listener.EndContact
 * @param {function} listener.PreSolve
 */
Contact.prototype.Update = function(listener) {
  var oldManifold = this.m_manifold;

  // Re-enable this contact.
  this.m_enabledFlag = true;

  var touching = false;
  var wasTouching = this.m_touchingFlag;

  var sensorA = this.m_fixtureA.IsSensor();
  var sensorB = this.m_fixtureB.IsSensor();
  var sensor = sensorA || sensorB;

  var bodyA = this.m_fixtureA.GetBody();
  var bodyB = this.m_fixtureB.GetBody();
  var xfA = bodyA.GetTransform();
  var xfB = bodyB.GetTransform();

  // Is this contact a sensor?
  if (sensor) {
    var shapeA = this.m_fixtureA.GetShape();
    var shapeB = this.m_fixtureB.GetShape();
    touching = Distance.TestOverlap(shapeA, this.m_indexA, shapeB, this.m_indexB,
        xfA, xfB);

    // Sensors don't generate manifolds.
    this.m_manifold.pointCount = 0;
  } else {

    this.Evaluate(this.m_manifold, xfA, xfB);
    touching = this.m_manifold.pointCount > 0;

    // Match old contact ids to new contact ids and copy the
    // stored impulses to warm start the solver.
    for (var i = 0; i < this.m_manifold.pointCount; ++i) {
      var mp2 = this.m_manifold.points[i];
      mp2.normalImpulse = 0.0;
      mp2.tangentImpulse = 0.0;
      var id2 = mp2.id; // ContactID

      for (var j = 0; j < oldManifold.pointCount; ++j) {
        var mp1 = oldManifold.points[j];
        if (mp1.id.key == id2.key) {
          mp2.normalImpulse = mp1.normalImpulse;
          mp2.tangentImpulse = mp1.tangentImpulse;
          break;
        }
      }
    }

    if (touching != wasTouching) {
      bodyA.SetAwake(true);
      bodyB.SetAwake(true);
    }
  }

  this.m_touchingFlag = touching;

  if (wasTouching == false && touching == true && listener) {
    listener.BeginContact(this);
  }

  if (wasTouching == true && touching == false && listener) {
    listener.EndContact(this);
  }

  if (sensor == false && touching && listener) {
    listener.PreSolve(this, oldManifold);
  }
}

Contact.prototype.SolvePositionConstraint = function(step) {
  return this._SolvePositionConstraint(step, false);
}

Contact.prototype.SolvePositionConstraintTOI = function(step, toiA, toiB) {
  return this._SolvePositionConstraint(step, true, toiA, toiB);
}

Contact.prototype._SolvePositionConstraint = function(step, toi, toiA, toiB) {
  // Assert(manifold.pointCount > 0);

  var fixtureA = this.m_fixtureA;
  var fixtureB = this.m_fixtureB;

  var shapeA = fixtureA.GetShape();
  var shapeB = fixtureB.GetShape();

  var bodyA = fixtureA.GetBody();
  var bodyB = fixtureB.GetBody();

  var invMassA = bodyA.m_invMass;
  var invMassB = bodyB.m_invMass;

  var invIA = bodyA.m_invI;
  var invIB = bodyB.m_invI;

  var localCenterA = bodyA.m_sweep.localCenter;
  var localCenterB = bodyB.m_sweep.localCenter;

  var radiusA = shapeA.m_radius;
  var radiusB = shapeB.m_radius;

  var manifold = this.m_manifold;
  var type = manifold.type;
  var localNormal = manifold.localNormal;

  var localPointA = manifold.localPoint;

  var mA = 0.0;
  var iA = 0.0;
  if (!toi || bodyA == toiA || bodyA == toiB) {
    mA = invMassA;
    iA = invIA;
  }

  var mB = 0.0;
  var iB = 0.0;
  if (!toi || bodyB == toiA || bodyB == toiB) {
    mB = invMassB;
    iB = invIB;
  }

  var cA = bodyA.c_position.c;
  var aA = bodyA.c_position.a;

  var cB = bodyB.c_position.c;
  var aB = bodyB.c_position.a;

  var minSeparation = 0.0;

  // Solve normal constraints
  for (var j = 0; j < manifold.pointCount; ++j) {
    var localPointB = manifold.points[j].localPoint;

    var xfA = new Transform();
    var xfB = new Transform();

    bodyA.c_position.GetTransform(xfA, localCenterA);
    bodyB.c_position.GetTransform(xfB, localCenterB);

    var normal, point, separation;

    // TODO: improve
    switch (type) {
    case Manifold.e_circles:
      var pointA = Transform.Mul(xfA, localPointA);
      var pointB = Transform.Mul(xfB, localPointB);
      normal = Vec2.Sub(pointB, pointA);
      var length = normal.Normalize();
      point = Vec2.Mid(pointA, pointB);
      separation = length - radiusA - radiusB;
      break;

    case Manifold.e_faceA:
      normal = Rot.Mul(xfA.q, localNormal);
      var planePoint = Transform.Mul(xfA, localPointA);

      var clipPoint = Transform.Mul(xfB, localPointB);
      separation = Vec2.Dot(clipPoint, normal) - Vec2.Dot(planePoint, normal)
          - radiusA - radiusB;
      point = clipPoint;
      break;

    case Manifold.e_faceB:
      normal = Rot.Mul(xfB.q, localNormal);
      var planePoint = Transform.Mul(xfB, localPointA);

      var clipPoint = Transform.Mul(xfA, localPointB);
      separation = Vec2.Dot(clipPoint, normal) - Vec2.Dot(planePoint, normal)
          - radiusA - radiusB;
      point = clipPoint;

      // Ensure normal points from A to B
      normal.Mul(-1);
      break;
    }

    var rA = Vec2.Sub(point, cA);
    var rB = Vec2.Sub(point, cB);

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

    var baumgarte = toi ? Settings.toiBaugarte : Settings.baumgarte;
    var linearSlop = Settings.linearSlop;

    // Prevent large corrections and allow slop.
    var C = Math.clamp(baumgarte * (separation + linearSlop),
        -Settings.maxLinearCorrection, 0.0);

    // Compute the effective mass.
    var rnA = Vec2.Cross(rA, normal);
    var rnB = Vec2.Cross(rB, normal);
    var K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;

    // Compute normal impulse
    var impulse = K > 0.0 ? -C / K : 0.0;

    var P = Vec2.Mul(impulse, normal);

    cA.WSub(mA, P);
    aA -= iA * Vec2.Cross(rA, P);

    cB.WAdd(mB, P);
    aB += iB * Vec2.Cross(rB, P);
  }

  bodyA.c_position.c = cA;
  bodyA.c_position.a = aA;

  bodyB.c_position.c = cB;
  bodyB.c_position.a = aB;

  return minSeparation;
}

// TODO merge with ManifoldPoint
function VelocityConstraintPoint() {
  this.rA = Vec2();
  this.rB = Vec2();
  this.normalImpulse = 0;
  this.tangentImpulse = 0;
  this.normalMass = 0;
  this.tangentMass = 0;
  this.velocityBias = 0;
}

Contact.prototype.InitVelocityConstraint = function(step) {
  // Assert(manifold.pointCount > 0);
  var fixtureA = this.m_fixtureA;
  var fixtureB = this.m_fixtureB;

  var bodyA = fixtureA.GetBody();
  var bodyB = fixtureB.GetBody();

  var restitution = this.m_restitution;

  var mA = bodyA.m_invMass;
  var mB = bodyB.m_invMass;

  var iA = bodyA.m_invI;
  var iB = bodyB.m_invI;

  var shapeA = fixtureA.GetShape();
  var shapeB = fixtureB.GetShape();

  var radiusA = shapeA.m_radius;
  var radiusB = shapeB.m_radius;

  var localCenterA = bodyA.m_sweep.localCenter;
  var localCenterB = bodyB.m_sweep.localCenter;

  var cA = bodyA.c_position.c;
  var aA = bodyA.c_position.a;
  var vA = bodyA.c_velocity.v;
  var wA = bodyA.c_velocity.w;

  var cB = bodyB.c_position.c;
  var aB = bodyB.c_position.a;
  var vB = bodyB.c_velocity.v;
  var wB = bodyB.c_velocity.w;

  var xfA = new Transform();
  var xfB = new Transform();

  bodyA.c_position.GetTransform(xfA, localCenterA);
  bodyB.c_position.GetTransform(xfB, localCenterB);

  var manifold = this.m_manifold;
  this.v_pointCount = manifold.pointCount;

  var worldManifold = manifold.GetWorldManifold(null, xfA, radiusA, xfB,
      radiusB);

  this.v_normal.Set(worldManifold.normal);

  for (var j = 0; j < this.v_pointCount; ++j) {
    var cp = manifold.points[j]; // ManifoldPoint
    var vcp = this.v_points[j] = new VelocityConstraintPoint();

    vcp.rA.Set(Vec2.Sub(worldManifold.points[j], cA));
    vcp.rB.Set(Vec2.Sub(worldManifold.points[j], cB));

    var rnA = Vec2.Cross(vcp.rA, this.v_normal);
    var rnB = Vec2.Cross(vcp.rB, this.v_normal);

    var kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;

    vcp.normalMass = kNormal > 0.0 ? 1.0 / kNormal : 0.0;

    var tangent = Vec2.Cross(this.v_normal, 1.0);

    var rtA = Vec2.Cross(vcp.rA, tangent);
    var rtB = Vec2.Cross(vcp.rB, tangent);

    var kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;

    vcp.tangentMass = kTangent > 0.0 ? 1.0 / kTangent : 0.0;

    // Setup a velocity bias for restitution.
    vcp.velocityBias = 0.0;
    var vbaseB = Vec2.Add(vB, Vec2.Cross(wB, vcp.rB));
    var vbaseA = Vec2.Add(vA, Vec2.Cross(wA, vcp.rA));
    var vRel = Vec2.Dot(this.v_normal, Vec2.Sub(vbaseB, vbaseA));
    if (vRel < -Settings.velocityThreshold) {
      vcp.velocityBias = -restitution * vRel;
    }
  }

  // If we have two points, then prepare the block solver.
  if (this.v_pointCount == 2 && step.blockSolve) {
    var vcp1 = this.v_points[0];
    var vcp2 = this.v_points[1];

    var rn1A = Vec2.Cross(vcp1.rA, this.v_normal);
    var rn1B = Vec2.Cross(vcp1.rB, this.v_normal);
    var rn2A = Vec2.Cross(vcp2.rA, this.v_normal);
    var rn2B = Vec2.Cross(vcp2.rB, this.v_normal);

    var k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
    var k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
    var k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B;

    // Ensure a reasonable condition number.
    var k_maxConditionNumber = 1000.0;
    if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) {
      // K is safe to invert.
      this.v_K.ex.Set(k11, k12);
      this.v_K.ey.Set(k12, k22);
      this.v_normalMass.Set(this.v_K.GetInverse());
    } else {
      // The constraints are redundant, just use one.
      // TODO_ERIN use deepest?
      this.v_pointCount = 1;
    }
  }

};

Contact.prototype.WarmStartConstraint = function(step) {
  return; // TODO
  var fixtureA = this.m_fixtureA;
  var fixtureB = this.m_fixtureB;

  var bodyA = fixtureA.GetBody();
  var bodyB = fixtureB.GetBody();

  var mA = bodyA.m_invMass;
  var mB = bodyB.m_invMass;

  var iA = bodyA.m_invI;
  var iB = bodyB.m_invI;

  var vA = bodyA.c_velocity.v;
  var wA = bodyA.c_velocity.w;
  var vB = bodyB.c_velocity.v;
  var wB = bodyB.c_velocity.w;

  var manifold = this.m_manifold;

  var pointCount = this.v_pointCount;
  var normal = this.v_normal;
  var tangent = Vec2.Cross(normal, 1.0);

  for (var j = 0; j < pointCount; ++j) {
    var cp = manifold.points[j]; // ManifoldPoint
    var vcp = this.v_points[j];
    vcp.normalImpulse = step.dtRatio * cp.normalImpulse;
    vcp.tangentImpulse = step.dtRatio * cp.tangentImpulse;

    var P = vcp.normalImpulse * normal + vcp.tangentImpulse * tangent; // Vec2
    wA -= iA * Vec2.Cross(vcp.rA, P);
    vA -= mA * P;
    wB += iB * Vec2.Cross(vcp.rB, P);
    vB += mB * P;
  }

  bodyA.c_velocity.v = vA;
  bodyA.c_velocity.w = wA;
  bodyB.c_velocity.v = vB;
  bodyB.c_velocity.w = wB;
};

Contact.prototype.StoreConstraintImpulses = function(step) {
  return; // TODO
  var manifold = this.m_manifold;
  for (var j = 0; j < this.v_pointCount; ++j) {
    manifold.points[j].normalImpulse = vc.points[j].normalImpulse;
    manifold.points[j].tangentImpulse = vc.points[j].tangentImpulse;
  }
};

Contact.prototype.SolveVelocityConstraint = function(step) {
  var fixtureA = this.m_fixtureA;
  var fixtureB = this.m_fixtureB;

  var bodyA = fixtureA.GetBody();
  var bodyB = fixtureB.GetBody();

  var friction = this.m_friction;
  var tangentSpeed = this.m_tangentSpeed;

  var mA = bodyA.m_invMass;
  var mB = bodyB.m_invMass;

  var iA = bodyA.m_invI;
  var iB = bodyB.m_invI;

  var vA = bodyA.c_velocity.v;
  var wA = bodyA.c_velocity.w;

  var vB = bodyB.c_velocity.v;
  var wB = bodyB.c_velocity.w;

  var normal = this.v_normal;
  var tangent = Vec2.Cross(normal, 1.0);

  var pointCount = this.v_pointCount;
  Assert(pointCount == 1 || pointCount == 2);

  // Solve tangent constraints first because non-penetration is more important
  // than friction.
  for (var j = 0; j < pointCount; ++j) {
    var vcp = this.v_points[j];

    // Relative velocity at contact
    var dvB = Vec2.Add(vB, Vec2.Cross(wB, vcp.rB))
    var dvA = Vec2.Add(vA, Vec2.Cross(wA, vcp.rA));
    var dv = Vec2.Sub(dvB, dvA);

    // Compute tangent force
    var vt = Vec2.Dot(dv, tangent) - tangentSpeed;
    var lambda = vcp.tangentMass * (-vt);

    // Clamp the accumulated force
    var maxFriction = friction * vcp.normalImpulse;
    var newImpulse = Math.clamp(vcp.tangentImpulse + lambda, -maxFriction,
        maxFriction);
    lambda = newImpulse - vcp.tangentImpulse;
    vcp.tangentImpulse = newImpulse;

    // Apply contact impulse
    var P = Vec2.Mul(lambda, tangent);

    vA.WSub(mA, P);
    wA -= iA * Vec2.Cross(vcp.rA, P);

    vB.WAdd(mB, P);
    wB += iB * Vec2.Cross(vcp.rB, P);
  }

  // Solve normal constraints
  if (pointCount == 1 || step.blockSolve == false) {
    for (var i = 0; i < pointCount; ++i) {
      var vcp = this.v_points[i];

      // Relative velocity at contact
      var dvB = Vec2.Add(vB, Vec2.Cross(wB, vcp.rB));
      var dvA = Vec2.Add(vA, Vec2.Cross(wA, vcp.rA));
      var dv = Vec2.Sub(dvB, dvA);

      // Compute normal impulse
      var vn = Vec2.Dot(dv, normal);
      var lambda = -vcp.normalMass * (vn - vcp.velocityBias);

      // Clamp the accumulated impulse
      var newImpulse = Math.max(vcp.normalImpulse + lambda, 0.0);
      lambda = newImpulse - vcp.normalImpulse;
      vcp.normalImpulse = newImpulse;

      // Apply contact impulse
      var P = Vec2.Mul(lambda, normal);

      vA.WSub(mA, P);
      wA -= iA * Vec2.Cross(vcp.rA, P);

      vB.WAdd(mB, P);
      wB += iB * Vec2.Cross(vcp.rB, P);
    }
  } 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 = vn0 - 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;

    var cp1 = this.v_points[0];
    var cp2 = this.v_points[1];

    var a = Vec2(cp1.normalImpulse, cp2.normalImpulse);
    Assert(a.x >= 0.0 && a.y >= 0.0);

    // Relative velocity at contact
    var dv1B = Vec2.Add(vB, Vec2.Cross(wB, cp1.rB));
    var dv1A = Vec2.Add(vA, Vec2.Cross(wA, cp1.rA));
    var dv1 = Vec2.Sub(dv1B, dv1A);

    var dv2B = Vec2.Add(vB, Vec2.Cross(wB, cp2.rB));
    var dv2A = Vec2.Add(vA, Vec2.Cross(wA, cp2.rA));
    var dv2 = Vec2.Sub(dv2B, dv2A);

    // Compute normal velocity
    var vn1 = Vec2.Dot(dv1, normal);
    var vn2 = Vec2.Dot(dv2, normal);

    var b = Vec2(vn1 - cp1.velocityBias, vn2 - cp2.velocityBias);

    // Compute b'
    b.Sub(Mat22.Mul(this.v_K, a));

    var k_errorTol = 1e-3;
    // NOT_USED(k_errorTol);

    for (;;) {
      //
      // Case 1: vn = 0
      //
      // 0 = A * x + b'
      //
      // Solve for x:
      //
      // x = - inv(A) * b'
      //
      var x = Vec2.Neg(Mat22.Mul(this.v_normalMass, b));

      if (x.x >= 0.0 && x.y >= 0.0) {
        // Get the incremental impulse
        var d = Vec2.Sub(x, a);

        // Apply incremental impulse
        var P1 = Vec2.Mul(d.x, normal);
        var P2 = Vec2.Mul(d.y, normal);

        vA.WSub(mA, P1, mA, P2);
        wA -= iA * (Vec2.Cross(cp1.rA, P1) + Vec2.Cross(cp2.rA, P2));

        vB.WAdd(mB, P1, mB, P2);
        wB += iB * (Vec2.Cross(cp1.rB, P1) + Vec2.Cross(cp2.rB, P2));

        // Accumulate
        cp1.normalImpulse = x.x;
        cp2.normalImpulse = x.y;

        if (DEBUG_SOLVER) {
          // Postconditions
          dv1 = vB + Vec2.Cross(wB, cp1.rB) - vA - Vec2.Cross(wA, cp1.rA);
          dv2 = vB + Vec2.Cross(wB, cp2.rB) - vA - Vec2.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);
        }
        break;
      }

      //
      // Case 2: vn1 = 0 and x2 = 0
      //
      // 0 = a11 * x1 + a12 * 0 + b1'
      // vn2 = a21 * x1 + a22 * 0 + b2'
      //
      x.x = -cp1.normalMass * b.x;
      x.y = 0.0;
      vn1 = 0.0;
      vn2 = this.v_K.ex.y * x.x + b.y;

      if (x.x >= 0.0 && vn2 >= 0.0) {
        // Get the incremental impulse
        var d = Vec2.Sub(x, a);

        // Apply incremental impulse
        var P1 = Vec2.Mul(d.x, normal);
        var P2 = Vec2.Mul(d.y, normal);
        vA.WSub(mA, P1, mA, P2);
        wA -= iA * (Vec2.Cross(cp1.rA, P1) + Vec2.Cross(cp2.rA, P2));

        vB.WAdd(mB, P1, mB, P2);
        wB += iB * (Vec2.Cross(cp1.rB, P1) + Vec2.Cross(cp2.rB, P2));

        // Accumulate
        cp1.normalImpulse = x.x;
        cp2.normalImpulse = x.y;

        if (DEBUG_SOLVER) {
          // Postconditions
          var dv1B = Vec2.Add(vB, Vec2.Cross(wB, cp1.rB));
          var dv1A = Vec2.Add(vA, Vec2.Cross(wA, cp1.rA));
          var dv1 = Vec2.Sub(dv1B, dv1A);

          // Compute normal velocity
          vn1 = Vec2.Dot(dv1, normal);

          Assert(Math.abs(vn1 - cp1.velocityBias) < k_errorTol);
        }
        break;
      }

      //
      // Case 3: vn2 = 0 and x1 = 0
      //
      // vn1 = a11 * 0 + a12 * x2 + b1'
      // 0 = a21 * 0 + a22 * x2 + b2'
      //
      x.x = 0.0;
      x.y = -cp2.normalMass * b.y;
      vn1 = this.v_K.ey.x * x.y + b.x;
      vn2 = 0.0;

      if (x.y >= 0.0 && vn1 >= 0.0) {
        // Resubstitute for the incremental impulse
        var d = Vec2.Sub(x, a);

        // Apply incremental impulse
        var P1 = Vec2.Mul(d.x, normal);
        var P2 = Vec2.Mul(d.y, normal);
        vA.WSub(mA, P1, mA, P2);
        wA -= iA * (Vec2.Cross(cp1.rA, P1) + Vec2.Cross(cp2.rA, P2));

        vB.WAdd(mB, P1, mB, P2);
        wB += iB * (Vec2.Cross(cp1.rB, P1) + Vec2.Cross(cp2.rB, P2));

        // Accumulate
        cp1.normalImpulse = x.x;
        cp2.normalImpulse = x.y;

        if (DEBUG_SOLVER) {
          // Postconditions
          var dv2B = Vec2.Add(vB, Vec2.Cross(wB, cp2.rB));
          var dv2A = Vec2.Add(vA, Vec2.Cross(wA, cp2.rA));
          var dv1 = Vec2.Sub(dv2B, dv2A);

          // Compute normal velocity
          vn2 = Vec2.Dot(dv2, normal);

          Assert(Math.abs(vn2 - cp2.velocityBias) < k_errorTol);
        }
        break;
      }

      //
      // Case 4: x1 = 0 and x2 = 0
      // 
      // vn1 = b1
      // vn2 = b2;
      //
      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
        var d = Vec2.Sub(x, a);

        // Apply incremental impulse
        var P1 = Vec2.Mul(d.x, normal);
        var P2 = Vec2.Mul(d.y, normal);
        vA.WSub(mA, P1, mA, P2);
        wA -= iA * (Vec2.Cross(cp1.rA, P1) + Vec2.Cross(cp2.rA, P2));

        vB.WAdd(mB, P1, mB, P2);
        wB += iB * (Vec2.Cross(cp1.rB, P1) + Vec2.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.c_velocity.v.Set(vA);
  bodyA.c_velocity.w = wA;
  bodyB.c_velocity.v.Set(vB);
  bodyB.c_velocity.w = wB;
};

/**
 * Friction mixing law. The idea is to allow either fixture to drive the
 * restitution to zero. For example, anything slides on ice.
 */
function MixFriction(friction1, friction2) {
  return Math.sqrt(friction1 * friction2);
}

/**
 * Restitution mixing law. The idea is allow for anything to bounce off an
 * inelastic surface. For example, a superball bounces on anything.
 */
function MixRestitution(restitution1, restitution2) {
  return restitution1 > restitution2 ? restitution1 : restitution2;
}

var s_registers = [];

/**
 * @param fn function(fixtureA, indexA, fixtureB, indexB) Contact
 */
Contact.AddType = function(type1, type2, callback) {

  s_registers[type1] = s_registers[type1] || {};
  s_registers[type1][type2] = callback;
}

Contact.Create = function(fixtureA, indexA, fixtureB, indexB) {
  var typeA = fixtureA.GetType(); // Shape.Type
  var typeB = fixtureB.GetType(); // Shape.Type

  // TODO: pool contacts
  var contact, evaluateFcn;
  if (evaluateFcn = s_registers[typeA] && s_registers[typeA][typeB]) {
    contact = new Contact(fixtureA, indexA, fixtureB, indexB, evaluateFcn);
  } else if (evaluateFcn = s_registers[typeB] && s_registers[typeB][typeA]) {
    contact = new Contact(fixtureB, indexB, fixtureA, indexA, evaluateFcn);
  } else {
    return null;
  }

  // Contact creation may swap fixtures.
  fixtureA = contact.GetFixtureA();
  fixtureB = contact.GetFixtureB();
  indexA = contact.GetChildIndexA();
  indexB = contact.GetChildIndexB();
  var bodyA = fixtureA.GetBody();
  var bodyB = fixtureB.GetBody();

  // Connect to body A
  contact.m_nodeA.contact = contact;
  contact.m_nodeA.other = bodyB;

  contact.m_nodeA.prev = null;
  contact.m_nodeA.next = bodyA.m_contactList;
  if (bodyA.m_contactList != null) {
    bodyA.m_contactList.prev = contact.m_nodeA;
  }
  bodyA.m_contactList = contact.m_nodeA;

  // Connect to body B
  contact.m_nodeB.contact = contact;
  contact.m_nodeB.other = bodyA;

  contact.m_nodeB.prev = null;
  contact.m_nodeB.next = bodyB.m_contactList;
  if (bodyB.m_contactList != null) {
    bodyB.m_contactList.prev = contact.m_nodeB;
  }
  bodyB.m_contactList = contact.m_nodeB;

  // Wake up the bodies
  if (fixtureA.IsSensor() == false && fixtureB.IsSensor() == false) {
    bodyA.SetAwake(true);
    bodyB.SetAwake(true);
  }

  return contact;
}

Contact.Destroy = function(contact, callback) {
  var fixtureA = contact.m_fixtureA;
  var fixtureB = contact.m_fixtureB;

  var bodyA = fixtureA.GetBody();
  var bodyB = fixtureB.GetBody();

  if (contact.IsTouching()) {
    callback.EndContact(contact);
  }

  // Remove from body 1
  if (contact.m_nodeA.prev) {
    contact.m_nodeA.prev.next = contact.m_nodeA.next;
  }

  if (contact.m_nodeA.next) {
    contact.m_nodeA.next.prev = contact.m_nodeA.prev;
  }

  if (contact.m_nodeA == bodyA.m_contactList) {
    bodyA.m_contactList = contact.m_nodeA.next;
  }

  // Remove from body 2
  if (contact.m_nodeB.prev) {
    contact.m_nodeB.prev.next = contact.m_nodeB.next;
  }

  if (contact.m_nodeB.next) {
    contact.m_nodeB.next.prev = contact.m_nodeB.prev;
  }

  if (contact.m_nodeB == bodyB.m_contactList) {
    bodyB.m_contactList = contact.m_nodeB.next;
  }

  if (contact.m_manifold.pointCount > 0 && fixtureA.IsSensor() == false
      && fixtureB.IsSensor() == false) {
    bodyA.SetAwake(true);
    bodyB.SetAwake(true);
  }

  var typeA = fixtureA.GetType(); // Shape.Type
  var typeB = fixtureB.GetType(); // Shape.Type

  var destroyFcn = s_registers[typeA][typeB].destroyFcn;
  if (typeof destroyFcn === 'function') {
    destroyFcn(contact);
  }
}