fixed compound body stability, improved position resolver

src/body/Body.js

@@ -40,6 +40,7 @@ var Body = {};
             torque: 0,
             positionImpulse: { x: 0, y: 0 },
             constraintImpulse: { x: 0, y: 0, angle: 0 },
+            totalContacts: 0,
             speed: 0,
             angularSpeed: 0,
             velocity: { x: 0, y: 0 },

src/collision/Resolver.js

@@ -8,9 +8,32 @@ var Resolver = {};
 
 (function() {
 
-    var _restingThresh = 4,
-        _positionDampen = 0.2,
-        _positionWarming = 0.6;
+    Resolver._restingThresh = 4;
+    Resolver._positionDampen = 0.9;
+    Resolver._positionWarming = 0.8;
+
+    /**
+     * Description
+     * @method preSolvePosition
+     * @param {pair[]} pairs
+     */
+    Resolver.preSolvePosition = function(pairs) {
+        var i,
+            pair,
+            activeCount;
+
+        // find total contacts on each body
+        for (i = 0; i < pairs.length; i++) {
+            pair = pairs[i];
+            
+            if (!pair.isActive)
+                continue;
+            
+            activeCount = pair.activeContacts.length;
+            pair.collision.parentA.totalContacts += activeCount;
+            pair.collision.parentB.totalContacts += activeCount;
+        }
+    };
 
     /**
      * Description
@@ -26,6 +49,8 @@ var Resolver = {};
             bodyB,
             normal,
             bodyBtoA,
+            contactShare,
+            contactCount = {},
             tempA = Vector._temp[0],
             tempB = Vector._temp[1],
             tempC = Vector._temp[2],
@@ -53,27 +78,29 @@ var Resolver = {};
         
         for (i = 0; i < pairs.length; i++) {
             pair = pairs[i];
-            
-            if (!pair.isActive)
+
+            if (!pair.isActive || pair.separation < 0)
                 continue;
             
             collision = pair.collision;
             bodyA = collision.parentA;
             bodyB = collision.parentB;
             normal = collision.normal;
-            positionImpulse = ((pair.separation * _positionDampen) - pair.slop) * timeScale;
+            positionImpulse = (pair.separation - pair.slop) * timeScale;
         
             if (bodyA.isStatic || bodyB.isStatic)
                 positionImpulse *= 2;
             
             if (!(bodyA.isStatic || bodyA.isSleeping)) {
-                bodyA.positionImpulse.x += normal.x * positionImpulse;
-                bodyA.positionImpulse.y += normal.y * positionImpulse;
+                contactShare = Resolver._positionDampen / bodyA.totalContacts;
+                bodyA.positionImpulse.x += normal.x * positionImpulse * contactShare;
+                bodyA.positionImpulse.y += normal.y * positionImpulse * contactShare;
             }
 
             if (!(bodyB.isStatic || bodyB.isSleeping)) {
-                bodyB.positionImpulse.x -= normal.x * positionImpulse;
-                bodyB.positionImpulse.y -= normal.y * positionImpulse;
+                contactShare = Resolver._positionDampen / bodyB.totalContacts;
+                bodyB.positionImpulse.x -= normal.x * positionImpulse * contactShare;
+                bodyB.positionImpulse.y -= normal.y * positionImpulse * contactShare;
             }
         }
     };
@@ -87,6 +114,9 @@ var Resolver = {};
         for (var i = 0; i < bodies.length; i++) {
             var body = bodies[i];
 
+            // reset contact count
+            body.totalContacts = 0;
+
             if (body.positionImpulse.x !== 0 || body.positionImpulse.y !== 0) {
                 // update body geometry
                 for (var j = 0; j < body.parts.length; j++) {
@@ -100,10 +130,16 @@ var Resolver = {};
                 // move the body without changing velocity
                 body.positionPrev.x += body.positionImpulse.x;
                 body.positionPrev.y += body.positionImpulse.y;
-                
-                // dampen accumulator to warm the next step
-                body.positionImpulse.x *= _positionWarming;
-                body.positionImpulse.y *= _positionWarming;
+
+                if (Vector.dot(body.positionImpulse, body.velocity) < 0) {
+                    // reset cached impulse if the body has velocity along it
+                    body.positionImpulse.x = 0;
+                    body.positionImpulse.y = 0;
+                } else {
+                    // warm the next iteration
+                    body.positionImpulse.x *= Resolver._positionWarming;
+                    body.positionImpulse.y *= Resolver._positionWarming;
+                }
             }
         }
     };
@@ -241,7 +277,7 @@ var Resolver = {};
                 tangentImpulse *= share;
                 
                 // handle high velocity and resting collisions separately
-                if (normalVelocity < 0 && normalVelocity * normalVelocity > _restingThresh * timeScaleSquared) {
+                if (normalVelocity < 0 && normalVelocity * normalVelocity > Resolver._restingThresh * timeScaleSquared) {
                     // high velocity so clear cached contact impulse
                     contact.normalImpulse = 0;
                     contact.tangentImpulse = 0;

src/collision/SAT.js

@@ -117,7 +117,7 @@ var SAT = {};
         if (Vertices.contains(bodyA.vertices, verticesB[0]))
             supports.push(verticesB[0]);
 
-        if (verticesB.length > 1 && Vertices.contains(bodyA.vertices, verticesB[1]))
+        if (Vertices.contains(bodyA.vertices, verticesB[1]))
             supports.push(verticesB[1]);
 
         // find the supports from bodyA that are inside bodyB
@@ -127,7 +127,7 @@ var SAT = {};
             if (Vertices.contains(bodyB.vertices, verticesA[0]))
                 supports.push(verticesA[0]);
 
-            if (verticesA.length > 1 && supports.length < 2 && Vertices.contains(bodyB.vertices, verticesA[1]))
+            if (supports.length < 2 && Vertices.contains(bodyB.vertices, verticesA[1]))
                 supports.push(verticesA[1]);
         }
 
@@ -245,21 +245,15 @@ var SAT = {};
         nearestDistance = -Vector.dot(normal, vertexToBody);
         vertexB = vertex;
 
-        // if the closest vertex is internal, we can't use the next connected vertex
-        if (!vertexA.isInternal) {
-            var nextIndex = (vertexA.index + 1) % vertices.length;
-            vertex = vertices[nextIndex];
-            vertexToBody.x = vertex.x - bodyAPosition.x;
-            vertexToBody.y = vertex.y - bodyAPosition.y;
-            distance = -Vector.dot(normal, vertexToBody);
-            if (distance < nearestDistance) {
-                vertexB = vertex;
-            }
+        var nextIndex = (vertexA.index + 1) % vertices.length;
+        vertex = vertices[nextIndex];
+        vertexToBody.x = vertex.x - bodyAPosition.x;
+        vertexToBody.y = vertex.y - bodyAPosition.y;
+        distance = -Vector.dot(normal, vertexToBody);
+        if (distance < nearestDistance) {
+            vertexB = vertex;
         }
 
-        if (!vertexB)
-            return [vertexA];
-
         return [vertexA, vertexB];
     };
 

src/core/Engine.js

@@ -160,17 +160,18 @@ var Engine = {};
         if (pairs.collisionStart.length > 0)
             Events.trigger(engine, 'collisionStart', { pairs: pairs.collisionStart });
 
+        // iteratively resolve position between collisions
+        Resolver.preSolvePosition(pairs.list);
+        for (i = 0; i < engine.positionIterations; i++) {
+            Resolver.solvePosition(pairs.list, timing.timeScale);
+        }
+        Resolver.postSolvePosition(allBodies);
+
         // iteratively resolve velocity between collisions
         Resolver.preSolveVelocity(pairs.list);
         for (i = 0; i < engine.velocityIterations; i++) {
             Resolver.solveVelocity(pairs.list, timing.timeScale);
         }
-        
-        // iteratively resolve position between collisions
-        for (i = 0; i < engine.positionIterations; i++) {
-            Resolver.solvePosition(pairs.list, timing.timeScale);
-        }
-        Resolver.postSolvePosition(allBodies);
 
         // trigger collision events
         if (pairs.collisionActive.length > 0)

src/geometry/Axes.js

@@ -19,11 +19,6 @@ var Axes = {};
 
         // find the unique axes, using edge normal gradients
         for (var i = 0; i < vertices.length; i++) {
-            // skip internal edges
-            if (vertices[i].isInternal) {
-                continue;
-            }
-
             var j = (i + 1) % vertices.length, 
                 normal = Vector.normalise({ 
                     x: vertices[j].y - vertices[i].y, 
@@ -33,7 +28,6 @@ var Axes = {};
             
             // limit precision
             gradient = gradient.toFixed(3).toString();
-
             axes[gradient] = normal;
         }