fixed various problems with constraints

src/constraint/Constraint.js

@@ -8,14 +8,6 @@
 * @class Constraint
 */
 
-// TODO: fix instability issues with torque
-// TODO: linked constraints
-// TODO: breakable constraints
-// TODO: collision constraints
-// TODO: allow constrained bodies to sleep
-// TODO: handle 0 length constraints properly
-// TODO: impulse caching and warming
-
 var Constraint = {};
 
 module.exports = Constraint;
@@ -29,8 +21,10 @@ var Common = require('../core/Common');
 
 (function() {
 
-    var _minLength = 0.000001,
+    var _zeroVector = { x: 0, y: 0 };
-        _minDifference = 0.001;
+
+    Constraint._warming = 0.4;
+    Constraint._minLength = 0.000001;
 
     /**
      * Creates a new constraint.
@@ -54,7 +48,7 @@ var Common = require('../core/Common');
             initialPointB = constraint.bodyB ? Vector.add(constraint.bodyB.position, constraint.pointB) : constraint.pointB,
             length = Vector.magnitude(Vector.sub(initialPointA, initialPointB));
     
-        constraint.length = constraint.length || length || _minLength;
+        constraint.length = typeof constraint.length !== 'undefined' ? constraint.length : length;
 
         // render
         var render = {
@@ -106,13 +100,13 @@ var Common = require('../core/Common');
 
         // update reference angle
         if (bodyA && !bodyA.isStatic) {
-            constraint.pointA = Vector.rotate(pointA, bodyA.angle - constraint.angleA);
+            Vector.rotate(pointA, bodyA.angle - constraint.angleA, pointA);
             constraint.angleA = bodyA.angle;
         }
         
         // update reference angle
         if (bodyB && !bodyB.isStatic) {
-            constraint.pointB = Vector.rotate(pointB, bodyB.angle - constraint.angleB);
+            Vector.rotate(pointB, bodyB.angle - constraint.angleB, pointB);
             constraint.angleB = bodyB.angle;
         }
 
@@ -125,111 +119,91 @@ var Common = require('../core/Common');
         if (!pointAWorld || !pointBWorld)
             return;
 
-        var delta = Vector.sub(pointAWorld, pointBWorld),
+        var velocityPointA = _zeroVector,
-            currentLength = Vector.magnitude(delta);
+            velocityPointB = _zeroVector;
-
-        // prevent singularity
-        if (currentLength === 0)
-            currentLength = _minLength;
-
-        // solve distance constraint with Gauss-Siedel method
-        var difference = (currentLength - constraint.length) / currentLength,
-            normal = Vector.div(delta, currentLength),
-            force = Vector.mult(delta, difference * 0.5 * constraint.stiffness * timeScale * timeScale);
-        
-        // if difference is very small, we can skip
-        if (Math.abs(1 - (currentLength / constraint.length)) < _minDifference * timeScale)
-            return;
-
-        var velocityPointA,
-            velocityPointB,
-            offsetA,
-            offsetB,
-            oAn,
-            oBn,
-            bodyADenom,
-            bodyBDenom;
     
         if (bodyA && !bodyA.isStatic) {
-            // point body offset
-            offsetA = { 
-                x: pointAWorld.x - bodyA.position.x + force.x, 
-                y: pointAWorld.y - bodyA.position.y + force.y
-            };
-            
             // update velocity
             bodyA.velocity.x = bodyA.position.x - bodyA.positionPrev.x;
             bodyA.velocity.y = bodyA.position.y - bodyA.positionPrev.y;
             bodyA.angularVelocity = bodyA.angle - bodyA.anglePrev;
             
             // find point velocity and body mass
-            velocityPointA = Vector.add(bodyA.velocity, Vector.mult(Vector.perp(offsetA), bodyA.angularVelocity));
+            velocityPointA = Vector.add(bodyA.velocity, Vector.mult(Vector.perp(pointA), bodyA.angularVelocity));
-            oAn = Vector.dot(offsetA, normal);
-            bodyADenom = bodyA.inverseMass + bodyA.inverseInertia * oAn * oAn;
-        } else {
-            velocityPointA = { x: 0, y: 0 };
-            bodyADenom = bodyA ? bodyA.inverseMass : 0;
         }
             
         if (bodyB && !bodyB.isStatic) {
-            // point body offset
-            offsetB = { 
-                x: pointBWorld.x - bodyB.position.x - force.x, 
-                y: pointBWorld.y - bodyB.position.y - force.y 
-            };
-            
             // update velocity
             bodyB.velocity.x = bodyB.position.x - bodyB.positionPrev.x;
             bodyB.velocity.y = bodyB.position.y - bodyB.positionPrev.y;
             bodyB.angularVelocity = bodyB.angle - bodyB.anglePrev;
 
             // find point velocity and body mass
-            velocityPointB = Vector.add(bodyB.velocity, Vector.mult(Vector.perp(offsetB), bodyB.angularVelocity));
+            velocityPointB = Vector.add(bodyB.velocity, Vector.mult(Vector.perp(pointB), bodyB.angularVelocity));
-            oBn = Vector.dot(offsetB, normal);
-            bodyBDenom = bodyB.inverseMass + bodyB.inverseInertia * oBn * oBn;
-        } else {
-            velocityPointB = { x: 0, y: 0 };
-            bodyBDenom = bodyB ? bodyB.inverseMass : 0;
         }
 
-        var relativeVelocity = Vector.sub(velocityPointB, velocityPointA),
+        var delta = Vector.sub(pointAWorld, pointBWorld),
-            normalImpulse = Vector.dot(normal, relativeVelocity) / (bodyADenom + bodyBDenom);
+            currentLength = Vector.magnitude(delta);
 
-        if (normalImpulse > 0) normalImpulse = 0;
+        // prevent singularity
+        if (currentLength === 0) {
+            currentLength = Constraint._minLength;
+            delta.x = Constraint._minLength;
+        }
 
-        var normalVelocity = {
+        // solve distance constraint with Gauss-Siedel method
-            x: normal.x * normalImpulse, 
+        var difference = (currentLength - constraint.length) / currentLength,
-            y: normal.y * normalImpulse
+            normal = Vector.div(delta, currentLength),
-        };
+            force = Vector.mult(delta, difference * constraint.stiffness * timeScale * timeScale),
+            relativeVelocity = Vector.sub(velocityPointB, velocityPointA),
+            massTotal = (bodyA ? bodyA.inverseMass : 0) + (bodyB ? bodyB.inverseMass : 0),
+            inertiaTotal = (bodyA ? bodyA.inverseInertia : 0) + (bodyB ? bodyB.inverseInertia : 0),
+            normalImpulse = Vector.dot(normal, relativeVelocity) / (massTotal + inertiaTotal),
+            normalVelocity,
+            torque,
+            share;
 
-        var torque;
+        if (normalImpulse < 0) {
+            normalVelocity = {
+                x: normal.x * normalImpulse, 
+                y: normal.y * normalImpulse
+            };
+        }
 
         if (bodyA && !bodyA.isStatic) {
-            torque = Vector.cross(offsetA, normalVelocity) * bodyA.inverseInertia * (1 - constraint.angularStiffness);
+            share = bodyA.inverseMass / massTotal;
 
             // keep track of applied impulses for post solving
-            bodyA.constraintImpulse.x -= force.x;
+            bodyA.constraintImpulse.x -= force.x * share;
-            bodyA.constraintImpulse.y -= force.y;
+            bodyA.constraintImpulse.y -= force.y * share;
-            bodyA.constraintImpulse.angle += torque;
 
             // apply forces
-            bodyA.position.x -= force.x;
+            bodyA.position.x -= force.x * share;
-            bodyA.position.y -= force.y;
+            bodyA.position.y -= force.y * share;
-            bodyA.angle += torque;
+
+            if (normalVelocity) {
+                torque = Vector.cross(pointA, normalVelocity) * bodyA.inverseInertia * (1 - constraint.angularStiffness);
+                bodyA.constraintImpulse.angle += torque;
+                bodyA.angle += torque;
+            }
         }
 
         if (bodyB && !bodyB.isStatic) {
-            torque = Vector.cross(offsetB, normalVelocity) * bodyB.inverseInertia * (1 - constraint.angularStiffness);
+            share = bodyB.inverseMass / massTotal;
 
             // keep track of applied impulses for post solving
-            bodyB.constraintImpulse.x += force.x;
+            bodyB.constraintImpulse.x += force.x * share;
-            bodyB.constraintImpulse.y += force.y;
+            bodyB.constraintImpulse.y += force.y * share;
-            bodyB.constraintImpulse.angle -= torque;
             
             // apply forces
-            bodyB.position.x += force.x;
+            bodyB.position.x += force.x * share;
-            bodyB.position.y += force.y;
+            bodyB.position.y += force.y * share;
-            bodyB.angle -= torque;
+
+            if (normalVelocity) {
+                torque = Vector.cross(pointB, normalVelocity) * bodyB.inverseInertia * (1 - constraint.angularStiffness);
+                bodyB.constraintImpulse.angle -= torque;
+                bodyB.angle -= torque;
+            }
         }
 
     };
@@ -245,7 +219,7 @@ var Common = require('../core/Common');
             var body = bodies[i],
                 impulse = body.constraintImpulse;
 
-            if (impulse.x === 0 && impulse.y === 0 && impulse.angle === 0) {
+            if (body.isStatic || (impulse.x === 0 && impulse.y === 0 && impulse.angle === 0)) {
                 continue;
             }