From 3be84416f7bacafe3b8d2c1bf3ba96c6a69748dd Mon Sep 17 00:00:00 2001
From: Al McElrath <hello@yrns.org>
Date: Tue, 3 May 2016 16:00:38 -0700
Subject: [PATCH 1/2] Map for shape overlaps #283

---
 src/objects/Body.js       |  5 +++++
 src/shapes/Heightfield.js | 13 +++++++++++++
 src/world/World.js        | 39 ++++++++++++++++++++++++++++-----------
 3 files changed, 46 insertions(+), 11 deletions(-)

diff --git a/src/objects/Body.js b/src/objects/Body.js
index a38054b14..33d671ef1 100644
--- a/src/objects/Body.js
+++ b/src/objects/Body.js
@@ -605,6 +605,11 @@ Body.prototype.addShape = function(shape, _offset, _orientation){
 
     shape.body = this;
 
+    // If the body is already in the world, add the shape to the map now.
+    if(this.world){
+        this.world.idToShapeMap[shape.id] = shape;
+    }
+
     return this;
 };
 
diff --git a/src/shapes/Heightfield.js b/src/shapes/Heightfield.js
index 94d6d0263..dfb06df9a 100644
--- a/src/shapes/Heightfield.js
+++ b/src/shapes/Heightfield.js
@@ -434,10 +434,23 @@ Heightfield.prototype.getConvexTrianglePillar = function(xi, yi, getUpperTriangl
         }
 
         result = new ConvexPolyhedron();
+        // Add shape to shape map. This is a special case for
+        // heightfields. Normally this happens in
+        // addShape/addBody. world may be null for some tests.
+        var world = this.body && this.body.world
+        if(world){
+            world.idToShapeMap[result.id] = result;
+        }
         offsetResult = new Vec3();
 
         this.pillarConvex = result;
         this.pillarOffset = offsetResult;
+    } else {
+        // If the cache is not used, make the pillar map to our shape.
+        if(!result.body){
+            this.body.world.idToShapeMap[result.id] = this;
+            result.body = this.body;
+        }
     }
 
     var data = this.data;
diff --git a/src/world/World.js b/src/world/World.js
index 2a0d4a21e..653c31dae 100644
--- a/src/world/World.js
+++ b/src/world/World.js
@@ -231,8 +231,18 @@ function World(options){
         body : null
     };
 
+    /**
+     * @property idToBodyMap
+     * @type {Object} Map of ids to bodies for bodyOverlapKeeper.
+     */
     this.idToBodyMap = {};
 
+    /**
+     * @property idToShapeMap
+     * @type {Object} Map of ids to shapes for shapeOverlapKeeper.
+     */
+    this.idToShapeMap = {};
+
     this.broadphase.setWorld(this);
 }
 World.prototype = new EventTarget();
@@ -302,6 +312,18 @@ World.prototype.add = World.prototype.addBody = function(body){
 	this.collisionMatrix.setNumObjects(this.bodies.length);
     this.addBodyEvent.body = body;
     this.idToBodyMap[body.id] = body;
+    // Add all shapes to the shape map.
+    var shapes = body.shapes,
+        n = shapes.length;
+    for(var i=0;i!==n; i++){
+        var s = shapes[i];
+        this.idToShapeMap[s.id] = s;
+        // Special case for Box. Use the creating shape.
+        var c = s.convexPolyhedronRepresentation;
+        if (c) {
+            this.idToShapeMap[c.id] = s;
+        }
+    }
     this.dispatchEvent(this.addBodyEvent);
 };
 
@@ -433,6 +455,11 @@ World.prototype.remove = function(body){
         this.collisionMatrix.setNumObjects(n);
         this.removeBodyEvent.body = body;
         delete this.idToBodyMap[body.id];
+        var shapes = this.shapes,
+            sn = shapes && shapes.length || 0;
+        for(i=0; i!==sn; i++){
+            delete this.idToShapeMap[shapes[i].id];
+        }
         this.dispatchEvent(this.removeBodyEvent);
     }
 };
@@ -448,18 +475,8 @@ World.prototype.getBodyById = function(id){
     return this.idToBodyMap[id];
 };
 
-// TODO Make a faster map
 World.prototype.getShapeById = function(id){
-    var bodies = this.bodies;
-    for(var i=0, bl = bodies.length; i<bl; i++){
-        var shapes = bodies[i].shapes;
-        for (var j = 0, sl = shapes.length; j < sl; j++) {
-            var shape = shapes[j];
-            if(shape.id === id){
-                return shape;
-            }
-        }
-    }
+    return this.idToShapeMap[id];
 };
 
 /**

From 9904665f82ea8465fd4dcbbe1c096b655c9f250b Mon Sep 17 00:00:00 2001
From: Al McElrath <hello@yrns.org>
Date: Mon, 16 May 2016 19:44:36 -0700
Subject: [PATCH 2/2] Assign body to heightfield pillars

---
 src/shapes/Heightfield.js | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/shapes/Heightfield.js b/src/shapes/Heightfield.js
index dfb06df9a..877155167 100644
--- a/src/shapes/Heightfield.js
+++ b/src/shapes/Heightfield.js
@@ -441,6 +441,8 @@ Heightfield.prototype.getConvexTrianglePillar = function(xi, yi, getUpperTriangl
         if(world){
             world.idToShapeMap[result.id] = result;
         }
+        // Assign body so the shape overlap events will have it.
+        result.body = this.body;
         offsetResult = new Vec3();
 
         this.pillarConvex = result;