Having problems with EPA Collision Resolution

Questions
, , ,
1

Hi All,

I’ve been following a tutorial for GJK and EPA collisions here: https://winter.dev/articles/epa-algorithm. My GJK algorithm is working perfectly in testing, however my EPA algorithm is not working. The original article is in c++, which I’m not experienced in, but when looking at them side by side, i can’t see anywhere they differ. Whilst im not sure whether this is the source of the problems, my errors seem to be stemming from the fact that my uniqueEdges array, which is used to rebuild the polytope after expansion, is empty after the first pass, causing some errors along the line when i try to access an index from said array. I’ve added my javascript code below:

const epsilon = 0.001;

function EPA (polytope, colliderA, colliderB) {
    let faces = [
        0, 1, 2,
	0, 3, 1,
	0, 2, 3,
	1, 3, 2
    ];

    let [normals, minFace] = GetFaceNormals(polytope, faces);
    let minNormal = new THREE.Vector3();
    let minDistance = Number.MAX_VALUE;

    while (minDistance == Number.MAX_VALUE) {
        minNormal = new THREE.Vector3().copy(normals[minFace]);
        minDistance = normals[minFace].clone().w;
        console.log('minNormal:', minNormal);
        console.log('minDistance:', minDistance);

        let support = Support(colliderA, colliderB, minNormal);
        let sDistance = minNormal.dot(support);

        console.log('support:', support);
        console.log('sDistance:', sDistance);
        console.log('minDistance:', sDistance);

        if (Math.abs(sDistance - minDistance) > epsilon) {

            minDistance = Number.MAX_VALUE;
            let uniqueEdges = new Array();

            for (let i = 0; i < normals.length; i++) {
                if ( normals[i].dot(support) > normals[i].dot(polytope[faces[i * 3]])) {
                    let f = i * 3;

                    AddIfUniqueEdge(uniqueEdges, faces, f, f + 1);
                    AddIfUniqueEdge(uniqueEdges, faces, f + 1, f + 2);
                    AddIfUniqueEdge(uniqueEdges, faces, f + 2, f);

                    faces[f + 2] = faces.pop();
                    faces[f + 1] = faces.pop();
                    faces[f] = faces.pop();

                    normals[i] = normals.pop();

                    i--;
                }
            }

            console.log('uniqueEdges:', uniqueEdges);

            let newFaces = new Array();

            for (let [edgeIndex1, edgeIndex2] of uniqueEdges) {
                newFaces.push(edgeIndex1);
                newFaces.push(edgeIndex2);
                newFaces.push(polytope.length);
            }

            console.log('newFaces:', newFaces);

            polytope.push(support);

            let [newNormals, newMinFace] = GetFaceNormals(polytope, faces);
            let oldMinDistance = Number.MAX_VALUE;

            for (let i = 0; i < normals.length; i++) {
                if (normals[i].w < oldMinDistance) {
                    oldMinDistance = normals[i].clone().w;
                    minFace = i;
                }
            }

            console.log('newNormals:', newNormals);
            console.log('newMinFace:', newMinFace);

            if (newNormals[newMinFace].w < oldMinDistance) {
                minFace = newMinFace + normals.length;
            }

            console.log('minFace:', minFace);

            faces = faces.concat(newFaces);
            normals = normals.concat(newNormals);
        }
    }

    console.log('normal: ' + minNormal);
    console.log('penetrationDepth: ' + minDistance);

    return {
        'normal': minNormal,
        'penetrationDepth': minDistance + epsilon
    };
}

function GetFaceNormals(polytope, faces) {
    let normals = new Array();
    let minTriangle = 0;
    let minDistance = Number.MAX_VALUE;

    for (let i = 0; i < faces.length; i += 3) {
        let a = polytope[faces[i]].clone();
        let b = polytope[faces[i + 1]].clone();
        let c = polytope[faces[i + 2]].clone();

        let normal = new THREE.Vector3().crossVectors(b.clone().sub(a), c.clone().sub(a)).normalize();
        let distance = normal.dot(a);

        if (distance < 0) {
            normal.negate();
            distance *= -1;
        }

        normals.push(new THREE.Vector4(normal.x, normal.y, normal.z, distance));

        if (distance < minDistance) {
            minTriangle = i / 3;
            minDistance = distance;
        }
    }

    return [ normals, minTriangle ];
}

function AddIfUniqueEdge(edges, faces, a, b) {
    let reverse = edges.findIndex(([edgeA, edgeB]) => edgeB == faces[a] && edgeA == faces[b]);

    if (reverse != -1) {
        edges.splice(reverse, 1);
    } else {
        edges.push([faces[a], faces[b]]);
    }
}

I’ve been working on this the past week, and am still unable to solve the problem, so any help would be greatly appreciated.