c++ - GJK 算法陷入不同 Voronoi 区域案例的循环中

Question

我目前正在尝试实现 https://mollyrocket.com/849 中介绍的简化 GJK 算法进入我的 C++ 游戏。

但是，我在第二维和第三维遇到了奇怪的行为:算法有时(每秒调用多次时很常见)会卡在案例循环中。例如，调试消息一遍又一遍地将以下内容打印到 std::cout:

3ACxAB
4AB
3ABxAC
4ABD
4AD

如果您查看我的代码，您会发现这些行代表算法允许的情况。例如。 3ACxAB 表示单纯形当前是一个三角形，并且原点位于面的 voronoi 区域，在叉积 AC x AB 的方向(可以解释为如三角形“上方”或“下方”)。 4AB 表示单纯形是一个四面体，原点在边 AB 的 voronoi 区域。

A 始终是新添加的点。在代码中，A 始终是 simplex 的最大索引。 (^simplex[1]` 如果它是一条线，如果是三角形则为 2，如果是 a) 四面体则为 3。

即使经过几天的错误搜索(我找到了一些，但仍然存在一个或多个错误)，该算法仍无法正常工作。

你看到代码有什么问题了吗？因为我和我的两个 friend 都没有。

PS:我没有从 Casey 的视频中复制任何计算(例如方向 vector 的叉积)。看完之后，我自己下定决心，所以这里可能存在潜在的问题，尤其是在三维方面，凯西故意没有说。

---

我的支持功能:

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

我的单纯形函数:

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

Answer 1

在三角形的情况下:

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

应该是

simplex = {simplex[2]};

在四面体的情况下:

所有边检查都使用simplex[2] 执行点生成，但它们应该使用最新的点simplex[3]。

我认为你的第一个边缘检查在第二个条件下使用了错误的脸，所以而不是

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

应该是

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

第二个边检查的第二个条件也一样，应该是:

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

和第三次边缘检查的第二个条件:

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

我还修复了三角形检查的输出单纯形。输入单纯形的最后一点应该始终是输出单纯形的最后一点。此外，点的顺序应与计算的方向一致。

这里是完整的固定函数:

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