From 2eab580b68ba29074135e00090caf973cc344a85 Mon Sep 17 00:00:00 2001
From: Brice Chevalier <brice.chevalier@blackstormlabs.com>
Date: Fri, 24 Nov 2017 17:38:24 +0900
Subject: [PATCH] Optimization of Resolver.solvePosition method

---
 src/body/Body.js          |  1 +
 src/collision/Resolver.js | 71 ++++++++++++++++++++++-----------------
 src/core/Engine.js        |  2 +-
 3 files changed, 42 insertions(+), 32 deletions(-)

diff --git a/src/body/Body.js b/src/body/Body.js
index 39ba0d3..04dfc49 100644
--- a/src/body/Body.js
+++ b/src/body/Body.js
@@ -49,6 +49,7 @@ var Axes = require('../geometry/Axes');
             force: { x: 0, y: 0 },
             torque: 0,
             positionImpulse: { x: 0, y: 0 },
+            previousPositionImpulse: { x: 0, y: 0 },
             constraintImpulse: { x: 0, y: 0, angle: 0 },
             totalContacts: 0,
             speed: 0,
diff --git a/src/collision/Resolver.js b/src/collision/Resolver.js
index c53817f..04d25a7 100644
--- a/src/collision/Resolver.js
+++ b/src/collision/Resolver.js
@@ -50,21 +50,31 @@ var Bounds = require('../geometry/Bounds');
      * @param {pair[]} pairs
      * @param {number} timeScale
      */
-    Resolver.solvePosition = function(pairs, timeScale) {
+    Resolver.solvePosition = function(pairs, bodies, timeScale) {
         var i,
+            normalX,
+            normalY,
             pair,
             collision,
             bodyA,
             bodyB,
             normal,
-            bodyBtoA,
+            separation,
+            penetration,
+            positionImpulseA,
+            positionImpulseB,
             contactShare,
+            bodyBtoAX,
+            bodyBtoAY,
             positionImpulse,
-            contactCount = {},
-            tempA = Vector._temp[0],
-            tempB = Vector._temp[1],
-            tempC = Vector._temp[2],
-            tempD = Vector._temp[3];
+            impulseCoefficient = timeScale * Resolver._positionDampen;
+
+
+        for (i = 0; i < bodies.length; i++) {
+            var body = bodies[i];
+            body.previousPositionImpulse.x = body.positionImpulse.x;
+            body.previousPositionImpulse.y = body.positionImpulse.y;
+        }
 
         // find impulses required to resolve penetration
         for (i = 0; i < pairs.length; i++) {
@@ -78,43 +88,42 @@ var Bounds = require('../geometry/Bounds');
             bodyB = collision.parentB;
             normal = collision.normal;
 
-            // get current separation between body edges involved in collision
-            bodyBtoA = Vector.sub(Vector.add(bodyB.positionImpulse, bodyB.position, tempA), 
-                                    Vector.add(bodyA.positionImpulse, 
-                                        Vector.sub(bodyB.position, collision.penetration, tempB), tempC), tempD);
+            positionImpulseA = bodyA.previousPositionImpulse;
+            positionImpulseB = bodyB.previousPositionImpulse;
 
-            pair.separation = Vector.dot(normal, bodyBtoA);
-        }
-        
-        for (i = 0; i < pairs.length; i++) {
-            pair = pairs[i];
+            penetration = collision.penetration;
 
-            if (!pair.isActive || pair.isSensor)
-                continue;
-            
-            collision = pair.collision;
-            bodyA = collision.parentA;
-            bodyB = collision.parentB;
-            normal = collision.normal;
-            positionImpulse = (pair.separation - pair.slop) * timeScale;
+            // bodyBtoA = positionImpulseB - positionImpulseA + penetration
+            bodyBtoAX = positionImpulseB.x - positionImpulseA.x + penetration.x;
+            bodyBtoAY = positionImpulseB.y - positionImpulseA.y + penetration.y;
+
+            normalX = normal.x;
+            normalY = normal.y;
+
+            // separation = dot(normal, bodyBtoA)
+            separation = normalX * bodyBtoAX + normalY * bodyBtoAY;
+            pair.separation = separation;
+
+            positionImpulse = (separation - pair.slop) * impulseCoefficient;
 
             if (bodyA.isStatic || bodyB.isStatic)
                 positionImpulse *= 2;
             
             if (!(bodyA.isStatic || bodyA.isSleeping)) {
-                contactShare = Resolver._positionDampen / bodyA.totalContacts;
-                bodyA.positionImpulse.x += normal.x * positionImpulse * contactShare;
-                bodyA.positionImpulse.y += normal.y * positionImpulse * contactShare;
+                contactShare = positionImpulse / bodyA.totalContacts;
+                bodyA.positionImpulse.x += normalX * contactShare;
+                bodyA.positionImpulse.y += normalY * contactShare;
             }
 
             if (!(bodyB.isStatic || bodyB.isSleeping)) {
-                contactShare = Resolver._positionDampen / bodyB.totalContacts;
-                bodyB.positionImpulse.x -= normal.x * positionImpulse * contactShare;
-                bodyB.positionImpulse.y -= normal.y * positionImpulse * contactShare;
+                contactShare = positionImpulse / bodyB.totalContacts;
+                bodyB.positionImpulse.x -= normalX * contactShare;
+                bodyB.positionImpulse.y -= normalY * contactShare;
             }
         }
     };
 
+
     /**
      * Apply position resolution.
      * @method postSolvePosition
@@ -341,4 +350,4 @@ var Bounds = require('../geometry/Bounds');
         }
     };
 
-})();
+})();
\ No newline at end of file
diff --git a/src/core/Engine.js b/src/core/Engine.js
index 2466fde..c14281c 100644
--- a/src/core/Engine.js
+++ b/src/core/Engine.js
@@ -195,7 +195,7 @@ var Body = require('../body/Body');
         // iteratively resolve position between collisions
         Resolver.preSolvePosition(pairs.list);
         for (i = 0; i < engine.positionIterations; i++) {
-            Resolver.solvePosition(pairs.list, timing.timeScale);
+            Resolver.solvePosition(pairs.list, allBodies, timing.timeScale);
         }
         Resolver.postSolvePosition(allBodies);