title: Implementing the Expanding Polytope Algorithm in 3D Space
body: <p>I'm trying to implement the EPA algorithm in 3D space but I seem to have found a situation when the convex simplex can turn into a concave one.</p>
  
  <p>Consider this simplex:</p>
  
  <p><a href="https://i.stack.imgur.com/mTsLp.png" rel="nofollow noreferrer"><img src="https://i.stack.imgur.com/mTsLp.png" alt="enter image description here"></a></p>
  
  <p>And because it's hard to see what's going on here it is animated:</p>
  
  <p><a href="https://i.imgur.com/bMugo9c.gif" rel="nofollow noreferrer"><img src="https://i.imgur.com/bMugo9c.gif" alt=""></a></p>
  
  <p>The origin is the red, green and blue axis helper. The white sphere with no edges connected to it represents the point where I need to expand the polytope to next. The 5 yellow arrows are the normals of the faces that should be removed since they're in the same direction as the origin to the new point. Some faces don't look to be in the same direction but I've verified that they are as the dot products with the face normal and new point are:</p>
  
  <ul>
  <li>0.45396564417079877</li>
  <li>0.9473689548609279</li>
  <li>0.3346846050014339</li>
  <li>0.09982613239032267</li>
  <li>0.09982617482390854</li>
  </ul>
  
  <p>So those two faces on the right side of the .gif are just barley in the same direction.</p>
  
  <p>Okay so the EPA algorithm says to remove those faces:</p>
  
  <p><a href="https://i.imgur.com/p9xJs3j.gif" rel="nofollow noreferrer"><img src="https://i.imgur.com/p9xJs3j.gif" alt=""></a></p>
  
  <p>Then construct new faces to the new point using the remaining edges from the faces we removed. But you can see now that the convex simplex has turned into a concave one:</p>
  
  <p><a href="https://i.imgur.com/b5xzEcS.gif" rel="nofollow noreferrer"><img src="https://i.imgur.com/b5xzEcS.gif" alt=""></a></p>
  
  <p>This is obviously not right but I'm not sure where I went wrong. It feels to me like I've removed faces I shouldn't have but those faces are in the same direction as the new point.</p>
  
  <p>Here is my code:</p>
  
  <pre><code>var EPA = function(aWorldVerts, bWorldVerts, simplex) {
      var simplexFaces = [{a: 0, b: 1, c: 2},
                          {a: 0, b: 1, c: 3},
                          {a: 0, b: 2, c: 3},
                          {a: 1, b: 2, c: 3}];
  
      var ret = null;
  
      while(true) {
          var face = findClosestFace(simplex, simplexFaces);
          var point = support(aWorldVerts, bWorldVerts, face.norm);
          var dist = point.clone().dot(face.norm);
  
          if(dist - face.dist &lt; 0.00001) {
              ret = {axis: face.norm, dist: dist};
              break;
          }
  
          simplex.push(point);
          reconstruct(simplex, simplexFaces, point);
      }
  
      return ret;
  }
  
  var reconstruct = function(simplex, simplexFaces, extendPoint) {
      //I do realize that this function can be done more efficietly
      var removalFaces = [];
      for(var i = 0; i &lt; simplexFaces.length; i++) {
          var face = simplexFaces[i];
  
          var ab = simplex[face.b].clone().sub(simplex[face.a]);
          var ac = simplex[face.c].clone().sub(simplex[face.a]);
          var norm = ab.cross(ac).normalize();
  
          var a0 = new THREE.Vector3().sub(simplex[face.a]);
          if(a0.dot(norm) &gt; 0)
              norm.negate();
  
          if(extendPoint.clone().dot(norm) &gt; 0) {
              removalFaces.push(i);
          }
      }
  
      //get the edges that are not shared between the faces that should be removed
      var edges = [];
      for(var i = 0; i &lt; removalFaces.length; i++) {
          var face = simplexFaces[removalFaces[i]];
          var edgeAB = {a: face.a, b: face.b};
          var edgeAC = {a: face.a, b: face.c};
          var edgeBC = {a: face.b, b: face.c};
  
          var k = edgeInEdges(edges, edgeAB);
          if(k != -1)
              edges.splice(k, 1);
          else
              edges.push(edgeAB);
  
          k = edgeInEdges(edges, edgeAC);
          if(k != -1)
              edges.splice(k, 1);
          else
              edges.push(edgeAC);
  
          k = edgeInEdges(edges, edgeBC);
          if(k != -1)
              edges.splice(k, 1);
          else
              edges.push(edgeBC);
      }
  
      //remove the faces from the polytope
      for(var i = removalFaces.length - 1; i &gt;= 0; i--) {
          simplexFaces.splice(removalFaces[i], 1);
      }
  
      //form new faces with the edges and new point
      for(var i = 0; i &lt; edges.length; i++) {
          simplexFaces.push({a: edges[i].a, b: edges[i].b, c: simplex.length - 1});
      }
  }
  
  var edgeInEdges = function(edges, edge) {
      for(var i = 0; i &lt; edges.length; i++) {
          if(edges[i].a == edge.a &amp;&amp; edges[i].b == edge.b)
              return i;
      }
  
      return -1;
  }
  
  var findClosestFace = function(simplex, simplexFaces) {
      var closest = {dist: Infinity};
  
      for(var i = 0; i &lt; simplexFaces.length; i++) {
          var face = simplexFaces[i];
  
          var ab = simplex[face.b].clone().sub(simplex[face.a]);
          var ac = simplex[face.c].clone().sub(simplex[face.a]);
          var norm = ab.cross(ac).normalize();
  
          var a0 = new THREE.Vector3().sub(simplex[face.a]);
          if(a0.dot(norm) &gt; 0)
              norm.negate();
  
          var dist = simplex[face.a].clone().dot(norm);
  
          if(dist &lt; closest.dist) {
              closest = {index: i, dist: dist, norm: norm, a: face.a, b: face.b, c: face.c};
          }
      }
  
      return closest;
  }
  
  var support = function(aVerts, bVerts, dir) {
      a = getFurthestPointInDirection(aVerts, dir);
      b = getFurthestPointInDirection(bVerts, dir.clone().negate());
      return a.clone().sub(b);
  }
  
  var getFurthestPointInDirection = function(verts, dir) {
      var index = 0;
      var maxDot = verts[index].clone().dot(dir.clone().normalize());
  
      for(var i = 1; i &lt; verts.length; i++) {
          var dot = verts[i].clone().dot(dir.clone().normalize());
  
          if(dot &gt; maxDot) {
              maxDot = dot;
              index = i;
          }
      }
  
      return verts[index];
  }
  </code></pre>
  
  <p>I know the support function works correctly as well as <code>findClosestFace()</code> and <code>edgeInEdges()</code>. Also, it shouldn't matter but this is implemented using Three.js and Javascript. Maybe I'm just fundamentally misunderstanding how the algorithm works?</p>
  
  <p>What have I done wrong and how can I fix it?</p>