Search code examples
javascriptcollision-detection

Implementing the Expanding Polytope Algorithm in 3D Space

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.

Consider this simplex:

enter image description here

And because it's hard to see what's going on here it is animated:

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:

  • 0.45396564417079877
  • 0.9473689548609279
  • 0.3346846050014339
  • 0.09982613239032267
  • 0.09982617482390854

So those two faces on the right side of the .gif are just barley in the same direction.

Okay so the EPA algorithm says to remove those faces:

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:

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.

Here is my 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 < 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 < 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) > 0)
            norm.negate();

        if(extendPoint.clone().dot(norm) > 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 < 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 >= 0; i--) {
        simplexFaces.splice(removalFaces[i], 1);
    }

    //form new faces with the edges and new point
    for(var i = 0; i < 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 < edges.length; i++) {
        if(edges[i].a == edge.a && edges[i].b == edge.b)
            return i;
    }

    return -1;
}

var findClosestFace = function(simplex, simplexFaces) {
    var closest = {dist: Infinity};

    for(var i = 0; i < 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) > 0)
            norm.negate();

        var dist = simplex[face.a].clone().dot(norm);

        if(dist < 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 < verts.length; i++) {
        var dot = verts[i].clone().dot(dir.clone().normalize());

        if(dot > maxDot) {
            maxDot = dot;
            index = i;
        }
    }

    return verts[index];
}

I know the support function works correctly as well as findClosestFace() and edgeInEdges(). Also, it shouldn't matter but this is implemented using Three.js and Javascript. Maybe I'm just fundamentally misunderstanding how the algorithm works?

What have I done wrong and how can I fix it?

Solution

  • Many hours of debugging later I've found my problem. The faces you want to remove before extending the polytope to the new point are not found by checking if the normal of the face is in the same direction as the origin to the new point. Many articles on this topic say you want to remove the faces that the new point can "see" which I took to mean the normals are in the same direction. This is not the case because you can very well have a face normal in the same direction as the origin to the new point however the face can't be "seen" by the point so removing it would be problematic which is what I was doing. You want to essentially imagine you're camera to be exactly where the new point is, look around and any face you can see should be removed.

    To check if a given face can be "seen" by the new point you want to form a vector from a vertex of said face to the new point and check the dot product of that with the face normal. So I replaced if(extendPoint.clone().dot(norm) > 0) with if(norm.clone().dot(extendPoint.clone().sub(simplex[face.a])) > 0) in the reconstruct() function and it now works.