GJK algorithm gets stuck in a loop of different Voronoi Region cases

Question

I'm currently trying to implement the simplified GJK algorithm presented in https://mollyrocket.com/849 into my C++ game.

However, I'm experiencing strange behaviour in the second and third dimension: The algorithm sometimes (which is quite often when called multiple times a second) gets stuck in a loop of cases. As an example, the debug messages print the following to std::cout over and over again:

3ACxAB
4AB
3ABxAC
4ABD
4AD

If you have a look at my code, you'll see that these lines represent the cases the algorithm allows. E.g. 3ACxAB means that the simplex currently is a triangle and that the origin is in the voronoi region of the face, in direction of the cross product AC x AB (which may be interpreted as "above" or "below" the triangle). The case 4AB means that the simplex is a tetrahedron and the origin is in the voronoi region of the edge AB.

A always is the newly added point. In the code, A always is the greatest index of simplex. (^simplex[1]` if it's a line, 2 if a triangle and 3 in the case of a) tetrahedron.

Even after days of searching for mistakes (I found some, but there still is one or more left), the algorithm won't work.

Do you see any problem in the code? Because neither I nor two friends of mine do.

PS: I didn't copy any of the calculations (e.g. cross products for the direction vector) from Casey's video. After watching it, I made up my mind myself, so here potential problems may lie, especially in the third dimension, where Casey intentionally didn't speak about.

---

My support function:

//hullA/B: convex hull of A resp. B; baseA/B: location of A/B
Vector3f gjkSupport(Vector3f direction,
        std::vector<GLfloat> hullA, std::vector<GLfloat> baseA,
        std::vector<GLfloat> hullB, std::vector<GLfloat> baseB) {
    //Initialize
    GLfloat maxDotP = -std::numeric_limits<GLfloat>::max();
    Vector3f furthestPointA, furthestPointB;
    //Get furthest point in given direction out of hullA by getting the maximum dot
    //product of the direction vector and a hull vertex's position vector
    for (GLuint i = 0; i < hullA.size(); i += 3) {
        Vector3f current (hullA[i]+baseA[0], hullA[i+1]+baseA[1], hullA[i+2]+baseA[2]);
        // * = dot product
        GLfloat dotP = direction * current;
        if (dotP > maxDotP) {
            maxDotP = dotP;
            furthestPointA = current;
        }
    }
    maxDotP = -std::numeric_limits<GLfloat>::max();
    //Get furthest point in negative of the given direction out of hullB
    for (GLuint i = 0; i < hullB.size(); i += 3) {
        Vector3f current (hullB[i]+baseB[0], hullB[i+1]+baseB[1], hullB[i+2]+baseB[2]);
        GLfloat dotP = -direction * current;
        if (dotP > maxDotP) {
            maxDotP = dotP;
            furthestPointB = current;
        }
    }
    //Furthest Minkowski Difference point is difference of d*A[i]-(-d)*B[j]
    return furthestPointA - furthestPointB;
}

My simplex function:

bool gjkSimplex(std::vector<Vector3f> &simplex, Vector3f &direction) {
    GLuint simplexSize = simplex.size();
    std::cout << simplexSize;
    switch (simplexSize) {
    //If the simplex is a line segment
    case 2:
        //Point is closest feature
        if ((simplex[0]-simplex[1])*-simplex[1] < 0) {
            std::cout << "A";
            simplex = {simplex[1]};
            //direction = A0
            direction = -simplex[1];
        //Line is closest feature
        } else {
            std::cout << "AB";
            //direction = AB x (A0 x AB)
            // ^ = cross product
            direction = (simplex[0]-simplex[1]) ^ ((-simplex[1]) ^ (simplex[0]-simplex[1]));
        }
        break;
    //If the simplex is a triangle
    case 3:
        //Point is closest feature
        if ((simplex[0]-simplex[2])*(-simplex[2]) < 0 && (simplex[1]-simplex[2])*(-simplex[2]) < 0) {
            std::cout << "A";
            //direction = A0
            direction = -simplex[2];
            simplex = {simplex[1]};
        //Line to second-latest point is closest feature
        } else if ((((simplex[0]-simplex[2])^(simplex[1]-simplex[2]))^(simplex[1]-simplex[2]))*-simplex[2] > 0) {
            std::cout << "AB";
            //direction = AB x (A0 x AB)
            direction = (simplex[1]-simplex[2]) ^ ((-simplex[2]) ^ (simplex[1]-simplex[2]));
            simplex = {simplex[1], simplex[2]};
        //Line to oldest point is closest feature
        } else if (((simplex[0]-simplex[2])^((simplex[0]-simplex[2])^(simplex[1]-simplex[2])))*-simplex[2] > 0) {
            std::cout << "AC";
            //direction = AC x (A0 x AC)
            direction = (simplex[0]-simplex[2]) ^ ((-simplex[2]) ^ (simplex[0]-simplex[2]));
            simplex = {simplex[0], simplex[2]};
        //Face is closest feature
        } else {
            //Origin is in direction AC x AB
            if (((simplex[1]-simplex[2]) ^ (simplex[0]-simplex[2])) * (-simplex[2]) < 0) {
                std::cout << "ACxAB";
                //direction = AC x AB
                direction = (simplex[0]-simplex[2]) ^ (simplex[1]-simplex[2]);
            //origin is in direction AB x AC (other side of the face)
            } else {
                std::cout << "ABxAC";
                //direction = AB x AC
                direction = (simplex[1]-simplex[2]) ^ (simplex[0]-simplex[2]);
                simplex = {simplex[1], simplex[0], simplex[2]};
            }
        }
        break;
    //If the simplex is a tetrahedron
    case 4:
        //Newest point is closest feature
        if ((simplex[0]-simplex[3])*(-simplex[3]) < 0 && (simplex[1]-simplex[3])*(-simplex[3]) < 0 &&
                (simplex[2]-simplex[3])*(-simplex[3]) < 0) {
            std::cout << "A";
            //direction = A0
            direction = -simplex[3];
            simplex = {simplex[3]};
        //Edge between newest and second-newest point is closest feature
        } else if ((((simplex[2]-simplex[3]) ^ ((simplex[1]-simplex[3]) ^ (simplex[2]-simplex[3]))) * (-simplex[2]) < 0) &&
                ((((simplex[1]-simplex[3]) ^ (simplex[0]-simplex[3])) ^ (simplex[2]-simplex[3])) * (-simplex[2]) < 0)) {
            std::cout << "AB";
            //direction = AB x (A0 x AB)
            direction = (simplex[2]-simplex[3]) ^ ((-simplex[3]) ^ (simplex[2]-simplex[3]));
            simplex = {simplex[2], simplex[3]};
        //Edge between newest and third-newest vertex is closest feature
        } else if ((((simplex[1]-simplex[3]) ^ ((simplex[0]-simplex[3]) ^ (simplex[1]-simplex[3]))) * (-simplex[2]) < 0) &&
                ((((simplex[0]-simplex[3]) ^ (simplex[2]-simplex[3])) ^ (simplex[1]-simplex[3])) * (-simplex[2]) < 0)) {
            std::cout << "AC";
            //direction = AC x (A0 x AC)
            direction = (simplex[1]-simplex[3]) ^ ((-simplex[3]) ^ (simplex[1]-simplex[3]));
            simplex = {simplex[1], simplex[3]};
        //Edge between newest and oldest point is closest feature
        } else if ((((simplex[0]-simplex[3]) ^ ((simplex[2]-simplex[3]) ^ (simplex[0]-simplex[3]))) * (-simplex[2]) < 0) &&
                ((((simplex[2]-simplex[3]) ^ (simplex[1]-simplex[3])) ^ (simplex[0]-simplex[3])) * (-simplex[2]) < 0)) {
            std::cout << "AD";
            //direction = AD x (A0 x AD)
            direction = (simplex[0]-simplex[3]) ^ ((-simplex[3]) ^ (simplex[0]-simplex[3]));
            simplex = {simplex[0], simplex[3]};
        //Face between the three newest points is closest feature
        } else if (((simplex[1]-simplex[3]) ^ (simplex[2]-simplex[3])) * (-simplex[3]) > 0) {
            std::cout << "ABC";
            //direction = AC x AB (outer normal of face)
            direction = (simplex[1]-simplex[3]) ^ (simplex[2]-simplex[3]);
            simplex = {simplex[1], simplex[3], simplex[2]};
        //Face between newest, second-newest and oldest point is closest feature
        } else if (((simplex[2]-simplex[3]) ^ (simplex[0]-simplex[3])) * (-simplex[3]) > 0) {
            std::cout << "ABD";
            //direction = AB x AD
            direction = (simplex[2]-simplex[3]) ^ (simplex[0]-simplex[3]);
            simplex = {simplex[0], simplex[2], simplex[3]};
        //Face between newest, second-oldest and oldest point is closest feature
        } else if (((simplex[0]-simplex[3]) ^ (simplex[1]-simplex[3])) * (-simplex[3]) > 0) {
            std::cout << "ACD";
            //direction = AD x AC
            direction = (simplex[0]-simplex[3]) ^ (simplex[1]-simplex[3]);
            simplex = {simplex[0], simplex[3], simplex[1]};
        //Origin is encased by simplex
        } else {
            //Collision detected
            std::cout << "ABCD";
            return true;
        }
        break;
    default:
        direction = {1,1,1};
        simplex = {};
        break;
    }
    std::cout << "\n";
    return false;
};

GJK main loop:

//Narrow Phase collision function using GJK
bool SolidObject::collidesWith(SolidObject *object) {
    //Initialize by using an arbitrary direction
    Vector3f direction (1,1,1);
    std::vector<Vector3f> simplex;
    Vector3f point = gjkSupport(direction,
            this->meshes[0].getConvexHull(), this->base, object->meshes[0].getConvexHull(), object->base);
    simplex = {point};
    //Set direction to the negative of the resulting point
    direction = -point;

    bool originInSimplex = false;
    while (!originInSimplex) {
        //Get furthest point in new direction
        point = gjkSupport(direction,
                this->meshes[0].getConvexHull(), this->base, object->meshes[0].getConvexHull(), object->base);
        //The furthest point in the negative direction is not in the opposing octant
        //  => no collision
        if (point*direction < 0) {
            return false;
        }
        //Add point to the simplex
        simplex.push_back(point);
        //Update simplex and direction, and return whether the simplex contains the origin
        originInSimplex = gjkSimplex(simplex, direction);
    }
    std::cout << "\n";
    return true;
}

Answer 1

In the triangle case:

//Point is closest feature
if ((simplex[0]-simplex[2])*(-simplex[2]) < 0 && (simplex[1]-simplex[2])*(-simplex[2]) < 0) {
    std::cout << "A";
    //direction = A0
    direction = -simplex[2];
    simplex = {simplex[1]};
}

It should be

simplex = {simplex[2]};

In the tetrahedron case:

All your edge checks perform the dot prodocut withsimplex[2], but they should use the latest point simplex[3].

I think your first edge check uses the wrong face for the second condition, so instead of

if ((((simplex[2]-simplex[3]) ^ ((simplex[1]-simplex[3]) ^ (simplex[2]-simplex[3]))) * (-simplex[2]) < 0) &&
        ((((simplex[1]-simplex[3]) ^ (simplex[0]-simplex[3])) ^ (simplex[2]-simplex[3])) * (-simplex[2]) < 0)) {
    std::cout << "AB";
    //direction = AB x (A0 x AB)
    direction = (simplex[2]-simplex[3]) ^ ((-simplex[3]) ^ (simplex[2]-simplex[3]));
    simplex = {simplex[2], simplex[3]};
}

it should be

if ((((simplex[2]-simplex[3]) ^ ((simplex[1]-simplex[3]) ^ (simplex[2]-simplex[3]))) * (-simplex[3]) < 0) &&
        ((((simplex[2]-simplex[3]) ^ (simplex[0]-simplex[3])) ^ (simplex[2]-simplex[3])) * (-simplex[3]) < 0)) {
    std::cout << "AB";
    //direction = AB x (A0 x AB)
    direction = (simplex[2]-simplex[3]) ^ ((-simplex[3]) ^ (simplex[2]-simplex[3]));
    simplex = {simplex[2], simplex[3]};
}

the same holds true for the second condition of the second edge check, where it should be:

((((simplex[1]-simplex[3]) ^ (simplex[2]-simplex[3])) ^ (simplex[1]-simplex[3])) * (-simplex[3]) < 0)

and the second condition of the third edge check:

((((simplex[0]-simplex[3]) ^ (simplex[1]-simplex[3])) ^ (simplex[0]-simplex[3])) * (-simplex[3]) < 0)

I also fixed the output simplexe of the triangle checks. The last point of the input simplex should be always the last point of the output simplex. Also, the ordering of the points should be consistent and match the calculated direction.

Here is the complete fixed function:

bool gjkSimplex(std::vector<Vector3f> &simplex, Vector3f &direction) {
    GLuint simplexSize = simplex.size();
    std::cout << simplexSize;
    switch (simplexSize) {
    //If the simplex is a line segment
    case 2:
        //Point is closest feature
        if ((simplex[0]-simplex[1])*-simplex[1] < 0) {
            std::cout << "A";
            //direction = A0
            direction = -simplex[1];
            simplex = {simplex[1]};
        //Line is closest feature
        } else {
            std::cout << "AB";
            //direction = AB x (A0 x AB)
            // ^ = cross product
            direction = (simplex[0]-simplex[1]) ^ ((-simplex[1]) ^ (simplex[0]-simplex[1]));
        }
        break;
    //If the simplex is a triangle
    case 3:
        //Point is closest feature
        if ((simplex[0]-simplex[2])*(-simplex[2]) < 0 && (simplex[1]-simplex[2])*(-simplex[2]) < 0) {
            std::cout << "A";
            //direction = A0
            direction = -simplex[2];
            simplex = {simplex[2]};
        //Line to second-latest point is closest feature
        } else if ((((simplex[0]-simplex[2])^(simplex[1]-simplex[2]))^(simplex[1]-simplex[2]))*-simplex[2] > 0) {
            std::cout << "AB";
            //direction = AB x (A0 x AB)
            direction = (simplex[1]-simplex[2]) ^ ((-simplex[2]) ^ (simplex[1]-simplex[2]));
            simplex = {simplex[1], simplex[2]};
        //Line to oldest point is closest feature
        } else if (((simplex[0]-simplex[2])^((simplex[0]-simplex[2])^(simplex[1]-simplex[2])))*-simplex[2] > 0) {
            std::cout << "AC";
            //direction = AC x (A0 x AC)
            direction = (simplex[0]-simplex[2]) ^ ((-simplex[2]) ^ (simplex[0]-simplex[2]));
            simplex = {simplex[0], simplex[2]};
        //Face is closest feature
        } else {
            //Origin is in direction AC x AB
            if (((simplex[1]-simplex[2]) ^ (simplex[0]-simplex[2])) * (-simplex[2]) < 0) {
                std::cout << "ACxAB";
                //direction = AC x AB
                direction = (simplex[0]-simplex[2]) ^ (simplex[1]-simplex[2]);
            //origin is in direction AB x AC (other side of the face)
            } else {
                std::cout << "ABxAC";
                //direction = AB x AC
                direction = (simplex[1]-simplex[2]) ^ (simplex[0]-simplex[2]);
                simplex = {simplex[1], simplex[0], simplex[2]};
            }
        }
        break;
    //If the simplex is a tetrahedron
    case 4:
        //Newest point is closest feature
        if ((simplex[0]-simplex[3])*(-simplex[3]) < 0 && (simplex[1]-simplex[3])*(-simplex[3]) < 0 &&
                (simplex[2]-simplex[3])*(-simplex[3]) < 0) {
            std::cout << "A";
            //direction = A0
            direction = -simplex[3];
            simplex = {simplex[3]};
        //Edge between newest and second-newest point is closest feature
        } else if ((((simplex[2]-simplex[3]) ^ ((simplex[1]-simplex[3]) ^ (simplex[2]-simplex[3]))) * (-simplex[3]) < 0) &&
                ((((simplex[2]-simplex[3]) ^ (simplex[0]-simplex[3])) ^ (simplex[2]-simplex[3])) * (-simplex[3]) < 0)) {
            std::cout << "AB";
            //direction = AB x (A0 x AB)
            direction = (simplex[2]-simplex[3]) ^ ((-simplex[3]) ^ (simplex[2]-simplex[3]));
            simplex = {simplex[2], simplex[3]};
        //Edge between newest and third-newest vertex is closest feature
        } else if ((((simplex[1]-simplex[3]) ^ ((simplex[0]-simplex[3]) ^ (simplex[1]-simplex[3]))) * (-simplex[3]) < 0) &&
                ((((simplex[1]-simplex[3]) ^ (simplex[2]-simplex[3])) ^ (simplex[1]-simplex[3])) * (-simplex[3]) < 0)) {
            std::cout << "AC";
            //direction = AC x (A0 x AC)
            direction = (simplex[1]-simplex[3]) ^ ((-simplex[3]) ^ (simplex[1]-simplex[3]));
            simplex = {simplex[1], simplex[3]};
        //Edge between newest and oldest point is closest feature
        } else if ((((simplex[0]-simplex[3]) ^ ((simplex[2]-simplex[3]) ^ (simplex[0]-simplex[3]))) * (-simplex[3]) < 0) &&
                ((((simplex[0]-simplex[3]) ^ (simplex[1]-simplex[3])) ^ (simplex[0]-simplex[3])) * (-simplex[3]) < 0)) {
            std::cout << "AD";
            //direction = AD x (A0 x AD)
            direction = (simplex[0]-simplex[3]) ^ ((-simplex[3]) ^ (simplex[0]-simplex[3]));
            simplex = {simplex[0], simplex[3]};
        //Face between the three newest points is closest feature
        } else if (((simplex[1]-simplex[3]) ^ (simplex[2]-simplex[3])) * (-simplex[3]) > 0) {
            std::cout << "ABC";
            //direction = AC x AB (outer normal of face)
            direction = (simplex[1]-simplex[3]) ^ (simplex[2]-simplex[3]);
            simplex = {simplex[1], simplex[2], simplex[3]};
        //Face between newest, second-newest and oldest point is closest feature
        } else if (((simplex[2]-simplex[3]) ^ (simplex[0]-simplex[3])) * (-simplex[3]) > 0) {
            std::cout << "ABD";
            //direction = AB x AD
            direction = (simplex[2]-simplex[3]) ^ (simplex[0]-simplex[3]);
            simplex = {simplex[2], simplex[0], simplex[3]};
        //Face between newest, second-oldest and oldest point is closest feature
        } else if (((simplex[0]-simplex[3]) ^ (simplex[1]-simplex[3])) * (-simplex[3]) > 0) {
            std::cout << "ACD";
            //direction = AD x AC
            direction = (simplex[0]-simplex[3]) ^ (simplex[1]-simplex[3]);
            simplex = {simplex[0], simplex[1], simplex[3]};
        //Origin is encased by simplex
        } else {
            //Collision detected
            std::cout << "ABCD";
            return true;
        }
        break;
    default:
        direction = {1,1,1};
        simplex = {};
        break;
    }
    std::cout << "\n";
    return false;
};

Answer 2

Just a guess:

case 2:
    //Point is closest feature
    if ((simplex[0]-simplex[1])*-simplex[1] < 0) {
        std::cout << "A";
        simplex = {simplex[1]};
        //direction = A0
        direction = -simplex[1];
    //Line is closest feature

Should you set direction before you set simplex? Otherwise you are trying to access the 2nd element of a length-1 vector.

Also, your function gjkSupport() as written accepts std::vector objects instead of std::vector& objects. This is likely to slow performance, as the vectors are getting copied-constructed every time you call the function.