GJK-3D

在之前2D的基础上增加3D的部分。主要是增加一个四面体单纯形的判定和之前的三角形单纯形的修改。

算法

单纯形原点包含的判定和更新及搜索方向的更新

三角形

在三角形区域时，通过法线$ABC= AB \times AC$的方向和$AO$的方向，即$ABC\cdot AO>0$分为上方和下方区域。

若在上方区域，搜索方向$\hat{d}=ABC$。

若在下方区域，搜索方向$\hat{d}=-ABC$，调整三角形顶点的顺序为$\{A, C, B\}$

四面体（tetrahedron）

前面由$OA\cdot \vec{d} < 0$判定过，所以原点不可能在$BCD$面外部区域。

只需要判定原点是否在其余各面的外部区域或四面体的内部区或。

判定原点是否在某面的外部区域：判定该面的法线方向和$AO$的方向，如$ABC$面，$ABC\cdot AO > 0$。若原点在其中一个面的外部区域，则删除该面所对的顶点，剩下的进入三角形判定。

原点在四面体内当且仅当原点同时在面$ACD$、面$ABD$和面$ABC$内部区域。

支撑点的旋转平移

若几何体$A$有仿射变换$T(x)=Bx+c$，那么支撑点有：

即先对搜索方向d作逆旋转变换，然向代入支撑函数，再对计算出来的支撑点作仿射变换。

证明：

那么

所以

程序

使用了Eigen库作为基本的数学库

Geometry.hpp

/**
 * @brief Point consist of a Vector3f position.
 */
struct Point final
{
    Point(Eigen::Vector3f p = Eigen::Vector3f{0.F, 0.F, 0.F}) : position{std::move(p)}
    {
    }
    Eigen::Vector3f position{0.F, 0.F, 0.F};
};

/**
 * @brief Basic data structure of geometry.
 */
class Geometry
{
    std::vector<Point> vertices_{};
    Eigen::Vector3f center_ = {0.F, 0.F, 0.F};
    Eigen::Vector3f position_ = {0.F, 0.F, 0.F};
    Eigen::Quaternionf rotation_ = {1.F, 0.F, 0.F, 0.F};

    [[nodiscard]] Eigen::Vector3f GetCenter() const
    {
        return center_ + position_;
    };

    [[nodiscard]] Eigen::Vector3f GetSupportingPoint(const Eigen::Vector3f &dir) const;
};

Eigen::Vector3f Geometry::GetSupportingPoint(const Vector3f &dir) const
{
    float max_len = -std::numeric_limits<float>::max();
    Vector3f supporting_point;
    Vector3f dirRt = rotation_.toRotationMatrix().transpose() * dir;
    for (auto const &vertex : vertices_)
    {
        float len = dirRt.dot(vertex.position);
        if (len > max_len)
        {
            max_len = len;
            supporting_point = vertex.position;
        }
    }

    supporting_point = rotation_.toRotationMatrix() * supporting_point + position_;
    
    return supporting_point;
}

GJK.cpp

Vector3f MinkowskiDifferenceSupport(const Geometry &shape1, const Geometry &shape2, const Vector3f &dir)
{
    Vector3f a = shape1.GetSupportingPoint(dir);
    Vector3f b = shape2.GetSupportingPoint(-dir);
    return a - b;
}

bool Simplex2(vector<Vector3f> &s, Vector3f &d)
{
    Vector3f A = s[1];
    Vector3f B = s[0];
    Vector3f AB = B - A;
    Vector3f AO = -A;
    Vector3f ABOB = AB.cross(AO).cross(AB); // norm to AB toward origin

    if (ABOB.norm() == 0.f) // origin is on AB
    {
        return true;
    }
    else // origin is on voronoi region of AB
    {
        d = ABOB;
        return false;
    }
}

bool Simplex3(vector<Vector3f> &s, Vector3f &d)
{
    Vector3f A = s[2];
    Vector3f B = s[1];
    Vector3f C = s[0];
    Vector3f AB = B - A;
    Vector3f AC = C - A;
    Vector3f AO = -A;
    Vector3f ABC = AB.cross(AC);
    Vector3f ACBB = -ABC.cross(AB); // norm to AB toward far from C
    Vector3f ABCC = ABC.cross(AC); // norm to AC toward far from B

    if (ABCC.dot(AO) > 0.f) // origin is on AC voronoi
    {
        s = {C, A};
        d = ABCC;
    }
    else if (ACBB.dot(AO) > 0.f) // origin is on AB voronoi
    {
        s = {B, A};
        d = ACBB;
    }
    else // origin is on ABC voronoi
    {
        float ABCO = ABC.dot(AO);
        if (ABCO > 0.f) // above
        {
            d = ABC;
        }
        else if (ABCO < 0.f) // below
        {
            s = {B, C, A};
            d = -ABC;
        }
        else // on
        {
            return true;
        }
    }
    return false;
}

bool Simplex4(vector<Vector3f> &s, Vector3f &d)
{
    Vector3f A = s[3];
    Vector3f B = s[2];
    Vector3f C = s[1];
    Vector3f D = s[0];

    Vector3f AO = -A;
    Vector3f AB = B - A;
    Vector3f AC = C - A;
    Vector3f AD = D - A;

    Vector3f ABC = AB.cross(AC); // normal to ABC
    Vector3f ACD = AC.cross(AD); // normal to ACD
    Vector3f ADB = AD.cross(AB); // normal to ADB

    if (ABC.dot(AO) > 0)
    {
        return Simplex3(s = {C, B, A}, d);
    }
    if (ACD.dot(AO) > 0)
    {
        return Simplex3(s = {D, C, A}, d);
    }
    if (ADB.dot(AO) > 0)
    {
        return Simplex3(s = {B, D, A}, d);
    }
    return true;
}
 
bool SimplexOrigin(vector<Vector3f> &s, Vector3f &d)
{
    bool contain_origin = false;
    switch (s.size())
    {
        case 2: // line segment
            contain_origin = Simplex2(s, d);
            break;
        case 3: // triangle
            contain_origin = Simplex3(s, d);
            break;
        case 4: // tetrahedron
            contain_origin = Simplex4(s, d);
            break;
        default:
            break;
    }
    return contain_origin;
}

std::optional<vector<Vector3f>> GJKAlgorithm(const Geometry &shape1, const Geometry &shape2)
{
    // center difference as initial direction
    Vector3f dir = shape1.GetCenter() - shape2.GetCenter();
    if (dir.norm() < 0.001f)
        dir = Vector3f{1.0f, 0.f, 0.f};
    dir.normalize();

    // init
    vector<Vector3f> simplex;
    simplex.reserve(4);
    Vector3f simplex_point = MinkowskiDifferenceSupport(shape1, shape2, dir);
    simplex.push_back(simplex_point);
    dir = -simplex_point;

    // main loop
    int max_iteration = max(shape1.GetNumVertices(), shape2.GetNumVertices());
    while (max_iteration-- > 0)
    {
        simplex_point = MinkowskiDifferenceSupport(shape1, shape2, dir);
        if (simplex_point.dot(dir) < 0)
            return std::nullopt;
        
        simplex.push_back(simplex_point);

        if (SimplexOrigin(simplex, dir))
            return simplex;
    }
    return std::nullopt;
}

测试

参考

1. Linahan J P. A Geometric Interpretation of the Boolean Gilbert-Johnson-Keerthi Algorithm[M]. Villanova University, 2015.

2. Bergen G. A fast and robust GJK implementation for collision detection of convex objects[J]. Journal of graphics tools, 1999, 4(2): 7-25.