GJK(Gilbert-Johnson-Keerthi)算法

背景知识

凸多边形

• 定义：对于平面上的一个多边形，如果延长它的任意一条边，使整个多边形都位于延长线的同侧，这样的多边形为凸多边形

显然，人可以直观的判断一个多边形是否为凸多边形，那么在程序中，应该如何判断一个多边形是否为凸多边形

利用向量的叉乘，根据多边形顶点坐标，依次求出每条边的向量axb,bxc,cxd...的结果应同号，否则就为凹多边形

闵可夫斯基和

闵可夫斯基和是两个欧几里得空间点集的和。点集A与B的闵可夫斯基和用公式表示就是 \(A+B=\{a+b|a\in A,b\in B\}\)

对于减法，闵可夫斯基和的概念也成立，即为闵可夫斯基差，举个例子

下面这两个图形的闵可夫斯基差即为
\(A-B=\\\{(3-5,4-6),(3-6,4-1),(3-10,4-2),(3-9,4-7)\\(1-5,1-6),(1-6,1-1),(1-10,1-2),(1-9,1-7)\\(4-5,0-6),(4-6,0-1),(4-10,0-2),(4-9,0-7)\}\)

这是两个不相交的图形的闵可夫斯基差，下面看看若两个图形相交的闵可夫斯基差

可以看到，对于两个相交的图形，他们的闵可夫斯基差是包括原点的，且两个多边形之间的距离就是其闵可夫斯基差到原点的距离。下面解释一下为什么相交的多边形一定包含原点，很好理解，若两个多边形相交，那么他们之间一定存在一个公共点，相减即为(0,0)。

单纯形

k阶单纯形，一个k维单纯形是指包含(k+1)个节点的凸多面体。
例如0阶单纯形就是点，1阶单纯形就是线段，2阶单纯形就是三角形，3阶单纯形就是四面体。对于2维空间的多边形，最多用到2阶单纯形。
对于上面两个相交的多边形的例子，实际运用中，不需要求出完整的闵可夫斯基差，只需要在闵可夫斯基差内形成一个多边形，使其尽可能包含原点即可，这个多边形就是单纯形。即若单纯形包好原点，纳闷闵可夫斯基差也一定包含原点。

Support函数

Support函数的作用是计算多边形在给定方向上的最远点。在构建单纯形时，我们希望尽可能得到闵可夫斯基差的顶点，这样产生的单纯形才能包含最大的区域，增加算法的快速收敛性。

向量的点乘与叉乘

点乘用于判断两个向量是否同向，叉乘用来判断点在线段的左侧还是右侧。

GJK算法

核心逻辑

给定两个多边形p和q，以及一个初始方向，通过迭代的方式构建，更新单纯形，并判断单纯形是否包含原点，若包含原点则两个多边形相交，否则不相交。

算法步骤

• 选取初始方向，初始方向可以是两个多边形的中心点构成的矢量，也可以是两个多边形各自选取的一个顶点构成的矢量，还可以是给定的任意矢量
• 根据初始方向，用Support函数得到第一个Support点，并放到单纯形(simplex)中
• 将初始方向取反，作为下一次迭代方向
• 循环开始：
  • 根据迭代方向和Support函数，计算出一个新的Support点
  • 若新的Support点与迭代方向的点乘小于0，说明在这个迭代方向上，已经无法找到一个能够跨越原点的Support点了，也就是说，无法组成一个能包含原点的单纯形。即两个多边形不相交，函数退出
  • 若新的Support点能够跨越原点，则将其加到simplex中
  • NearestSimplex函数用来更新单纯形和迭代方向，并判断simplex是否包含原点。这是simplex有两个点或三个点：
    • 若为两个点，则这两个点构成一条直线，以该直线的垂线朝向原点的方向，作为下一次迭代方向
    • 若为三个点，则需要判断这三个点组成的三角形是否包含原点。若不包含，则保留离原点最近的边上的两个点，同样以这两个点的直线的垂线朝向原点的方向作为下一次迭代方向
  • 回到循环开始步骤

关于跨越原点，可以理解为过原点做一条垂直于方向的直线，若两个点在直线两侧，则为跨越原点

参考代码

#include <iostream>
#include <vector>
#include <limits>
#include <cmath>

class Vector2D {
    public:
    
    double x, y;

    Vector2D(double _x = 0, double _y = 0) : x(_x), y(_y) {}

    Vector2D operator - (const Vector2D& other) const 
    {
        return Vector2D(x - other.x, y - other.y);
    }

    double dot(const Vector2D& other) const
    {
        return x * other.x + y * other.y;
    }//向量的点乘

    Vector2D perp() const
    {
        return Vector2D(-y, x);
    }//计算法向量
    Vector2D operator-() const 
    {
        return Vector2D(-x, -y);
    }
}origin(0,0);

Vector2D support(const std::vector<Vector2D>& shape1, const std::vector<Vector2D>& shape2, const Vector2D& dir) {
    double maxDot = -std::numeric_limits<double>::infinity();//返回正无穷大
    Vector2D bestPoint;

    for (const auto& p : shape1) {
        double dotProduct = p.dot(dir);
        if (dotProduct > maxDot) {
            maxDot = dotProduct;
            bestPoint = p;
        }
    }//找出最远点

    Vector2D pointOnShape2;
    maxDot = std::numeric_limits<double>::infinity();

    for (const auto& p : shape2) {
        double dotProduct = p.dot(dir);
        if (dotProduct < maxDot) {
            maxDot = dotProduct;
            pointOnShape2 = p;
        }
    }//找出最远点

    return bestPoint - pointOnShape2;//作差
}

double calcPointToPointDistance(Vector2D p1, Vector2D p2)
{
	return sqrt(pow(p1.x - p2.x,2) + pow(p1.y - p2.y,2));
}

double calcPointToLineDistance(Vector2D p0, Vector2D p1, Vector2D p2)
{
	double k = (p2.y - p1.y) / (p2.x - p1.x);
	double A = k, B = -1, C = p1.y - k * p1.x;
	return fabs(A * p0.x + B * p0.y + C) / sqrt(A * A + B * B);
}//计算p0到p1与p2构成的边之间的距离

double calcTriangleArea(Vector2D p1, Vector2D p2, Vector2D p3)
{
	double a = calcPointToPointDistance(p1, p2);
	double b = calcPointToPointDistance(p1, p3);
    double c = calcPointToPointDistance(p2, p3);
    double p = (a + b + c) / 2.0;
    return sqrt(p * (p - a) * (p - b) * (p - c));
}//海伦公式计算面积

bool isContainOrigin(Vector2D p1, Vector2D p2, Vector2D p3)
{
    double s1 = calcTriangleArea(origin, p1, p2);
    double s2 = calcTriangleArea(origin, p1, p3);
    double s3 = calcTriangleArea(origin, p2, p3);
    double s = calcTriangleArea(p1, p2, p3);
    if (fabs(s1 + s2 + s3 - s) < 1e-6)
        return true;
    return false;
}

bool GJK(const std::vector<Vector2D>& shape1, const std::vector<Vector2D>& shape2){
	// 初始化单纯形
	std::vector<Vector2D> simplex;
    Vector2D direction = {1, 0}; // 初始方向

    simplex.push_back(support(shape1, shape2, direction));
    direction = -simplex[0];//方向取反

    while (true) {//开始循环
        simplex.push_back(support(shape1, shape2, direction));

        if (simplex.back().dot(direction) <= 0)
            return false; //点乘小于0，不可能相交

		if (simplex.size() == 3)
        {
            if (isContainOrigin(simplex[0], simplex[1], simplex[2])){
                return true;
            }// 判断当前单纯形的三个顶点组成的三角形是否包含原点
            std::cout << simplex[0].x << " 1 " << simplex[0].y << "\n";
            std::cout << simplex[1].x << " 2 " << simplex[1].y << "\n";
            std::cout << simplex[2].x << " 3 " << simplex[2].y << "\n";
            double minDistance;
            int minIndex1 = -1, minIndex2 = -1;
            for (int i = 0; i < 3; i++){
                for (int j = i + 1; j < 3; j++){
                    double distance = calcPointToLineDistance(origin, simplex[i], simplex[j]);
                    if (minIndex1 == -1 || distance < minDistance){
                        minDistance = distance, minIndex1 = i, minIndex2 = j;
                    }
                }
            }// 找到三角形离原点最近的边 

            Vector2D line = simplex[minIndex1] - simplex[minIndex2];
            direction = line.perp();
            int k = 3 - minIndex1 - minIndex2;
            simplex.erase(simplex.begin() + k);//删除剩下的那个点            
        }
        else
        {
            Vector2D line = simplex[0] - simplex[1];    
            direction = line.perp();
        }
    }
}

int main() {
    std::vector<Vector2D> shape1 = {{3, 4}, {1, 1}, {5, 1}};
    std::vector<Vector2D> shape2 = {{4, 3}, {4, 0}, {7, 3},{7,0}}; //初始化两个图形的顶点坐标

    if (GJK(shape1, shape2))
        std::cout << "Shapes intersect.\n";
    else 
        std::cout << "Shapes do not intersect.\n";

    return 0;
}

参考资料碰撞检测算法之GJK算法