Алгоритм GJK застревает в цикле разных дел Воронежского края

В настоящее время я пытаюсь реализовать упрощенный алгоритм GJK, представленный в https://mollyrocket.com/849, в мою игру C++.

Тем не менее, я испытываю странное поведение во втором и третьем измерениях: иногда алгоритм (который часто встречается несколько раз в секунду) застревает в цикле случаев. Например, отладочные сообщения печатают следующее в std :: cout снова и снова:

3ACxAB
4AB
3ABxAC
4ABD
4AD

Если вы посмотрите на мой код, вы увидите, что эти строки представляют собой случаи, которые позволяет алгоритм. Например, 3ACxAB означает, что симплекс в настоящее время является треугольником и что начало координат находится в области voronoi лица, в направлении поперечного произведения AC x AB (которое может быть интерпретировано как "выше" или "ниже" треугольника). Случай 4AB означает, что симплекс представляет собой тетраэдр, а начало координат находится в области вороней края AB.

Всегда это вновь добавленная точка. В коде A всегда является наибольшим индексом simplex. (^ симплекс [1] ', если это линия, 2, если треугольник и 3 в случае а) тетраэдра.

Даже после нескольких дней поиска ошибок (я нашел некоторые, но все равно остается один или несколько), алгоритм не будет работать.

Вы видите какие-либо проблемы в коде? Потому что ни я, ни двое моих друзей.

PS: Я не копировал какие-либо вычисления (например, кросс-продукты для вектора направления) из видео Casey. Наблюдая за этим, я сам решил, поэтому здесь могут возникнуть потенциальные проблемы, особенно в третьем измерении, где Кейси намеренно не говорил.

Моя функция поддержки:

//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 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;
}

Моя симплексная функция:

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:

//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;
}