碰撞检测算法之GJK算法

1. 简介

参考：

• 碰撞检测算法之GJK算法 - 知乎 (zhihu.com)

• 运筹优化】凸多面体重叠判断算法：GJK 算法详解 & C++代码实现二维情形的凸多边形重叠判断_c++ 凸多边形_WSKH0929的博客-CSDN博客

• 物理引擎学习03-GJK碰撞检测算法基础gjk算法游蓝海的博客-CSDN博客

SAT 从 分离 的角度来判断物体间的碰撞，而 GJK 从 重叠 的角度来探索物体间的碰撞。

GJK是由Gilbert，Johnson，Keerthi 三位前辈发明的，用来计算两个凸多面体之间的碰撞检测，以及最近距离。GJK算法可以在O(M+N)的时间复杂度内，检测出碰撞，算法在每次迭代的过程中，都会优先选择靠近原点的方向，因此收敛速度会很快。算法的证明过程比较复杂，但是原理还是比较容易理解的。 GJK 是一种基于迭代的算法，其收敛速度取决于迭代方向

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

1.1 GJK算法原理

GJK算法的结论是：如果两个多边形相交，那么这两个多边形构成的闵可夫斯基差集(Minkowski Difference)，必然会包含原点。就像1.1节所示那样，差集的点，会分布在原点两侧。只不过这里的差集是一个多边形。

1.2 闵可夫斯基差集(Minkowski Difference)

用多边形A的所有点，减去多边形B中所有的点得到的一个点集合。

闵可夫斯基差集的意义在于，得到两个多边形顶点间的坐标分布关系，如果两个多边形相交，那么差集中点会分布在原点四周，也就是说差集会包含原点。

差集有一些特殊的性质，差集构成的多边形的形状与两个多边形之间的距离没有直接关系。两个多边形距离越大，则差集的中心位置离原点越远；反之，离原点越近。如果相交，则差集多边形会包含原点。

1.3 单形体(Simplex)

计算闵可夫斯基差集是一个非常麻烦的过程，所幸计算碰撞并不需要得到完整的闵可夫斯基差集多边形，我们仅需要计算出一个能够包含原点的差集多边形即可。对于2D空间，需要得到一个三角形；3D空间需要一个四面体。为了方便表示，我们把这样的差集多边形叫做单形体(Simplex)。

1.4 Support函数

为了方便表示，我们把单形体中的点，称作support点；把得到support点的方法称作support函数。support 函数的作用是计算多边形在给定方向上的最远点。support函数沿着某个方向，从两个多边形上找出距离最远的两个点，然后计算出差值。

• 如何寻找给定方向上的最远点呢？需要用到向量的点乘。我们可以遍历每个顶点和向量d的点乘，找到点乘值最大的顶点，它就是向量 d 方向的最远点。这个点也被称为支撑点。

• 为什么需要Support函数呢？这是因为在构建单纯形时，我们希望尽可能得到闵可夫斯基差的顶点，而不是其内部的一个点，这样产生的单纯形才能包含最大的区域，增加算法的快速收敛性。

2. GJK算法

2.1 GJK算法伪代码 & 算法步骤

伪代码：

bool GJK(Shape shapeA, Shape shapeB)
{
    // 得到初始的方向
    Vector2 direction = findFirstDirection();
    // 得到首个support点
    simplex.add(support(direction));
    // 得到第二个方向
    direction = -direction;
    while(true)
    {
        Vector2 p = support(direction);
        // 沿着dir的方向，已经找不到能够跨越原点的support点了。
        if (Vector2.Dot(p, direction) < 0)
            return false;
        
        simplex.add(p);
        // 单形体包含原点了
        if (simplex.contains(Vector2(0, 0)))
            return true;
 
        direction = findNextDirection();
    }
}

算法步骤：碰撞检测算法之GJK算法 - 知乎 (zhihu.com)

这里比较重要的是迭代如何终止，以及下一次迭代的方向选择，其他概念都比较好理解。下面用文字来解释一下算法核心步骤：

1. 随机选取一个初始方向，用support函数得到第一个support点；

2. 将初始方向取反，作为下一次的迭代方向；

3. 迭代循环开始:

1. 用support函数得到一个新的suppport点；

2. 如果新的support点，在迭代方向上的投影小于0，说明在这个方向上，已经无法找到一个能够跨越原点的support点了。也就是说，无法组成一个能够包含原点的单形体了。则两个多边形不相交，检测到此结束；

3. 如果support点达到3个，用这3点组成三角形，如果包含原点，说明发生了碰撞，检测到此结束；

4. 否则，仅保留离原点最近的support边上的两个support点；

5. 此时，将仅剩的两个support点构成一条直线，计算直线的垂线。并选垂线取朝向原点方向，作为下一次的迭代方向；

6. 跳转到步骤3。

这里比较难理解的是第b步，此时的单形体存在两种情况:

1. 首次进入循环，单形体中只有一个初始的support点。如果投影小于0，说明沿着背离初始点的方向，无法找到一个能够跨越原点的support点了。也就是说，该点和初始点都在原点的同一侧；

2. 非首次进入循环，单形体中只有两个support点了，迭代方向是由步骤8生成的，该方向是垂直于单形体中剩余两个support点构成的直线。如果投影小于0，则说明单形体中仅剩的两点，已经是最接近原点两个support点了。同时这两点构成的线段，就是闵可夫斯基差集中最接近原点的边，该边是计算两个多边形最近距离的关键，下一章中会用到这条边。
   

需要注意一个特殊情况，步骤e中，如果原点恰好就在两个support点构成的直线上，说明原点就在闵可夫斯基差集的边界上。也就是说，两个多边形刚开始发生碰撞。

2.2 算法细节

（1）判断三角形是否包含原点

判断一个点是否在三角形内部 - 知乎 (zhihu.com)

• 若点O在三角形内部，则沿着三角形的边逆时针走，点O一定保持在边的左侧。如图示，点在逆时针行走时，在AB，BC，CA的左侧。

• 如何判断点在一个边的左侧呢？可以借助向量叉乘来判断O是否在向量AB的哪一侧。通过计算向量AO与向量AB的叉乘的值为正，则表示O在AB的左侧，反之为右侧。

向量的叉乘可以用来判断点P是在向量AB的左侧还是右侧。判断一点是否在三角形内部 - 知乎 (zhihu.com)

其运算结果仍是一个向量，我们记之为向量c，它的模定义为：

其中θ为向量a和向量b的夹角，如上图所示，

• c的模：即以a和b为两条边的平行四边形的面积；

• c的方向：即a和b构成平面的法向量，c的方向定义为垂直于a和b所构成的平面，并且a，b和c构成右手螺旋定则，也就是右手四指方向从a转向b，大拇指即得到c方向。

#include <iostream>
#include <math.h>
using namespace std;
struct Point {
    double x;
    double y;
};
double product(Point p1,Point p2,Point p3) {
    //首先根据坐标计算p1p2和p1p3的向量，然后再计算叉乘
    //p1p2 向量表示为 (p2.x-p1.x,p2.y-p1.y)
    //p1p3 向量表示为 (p3.x-p1.x,p3.y-p1.y)
    return (p2.x-p1.x)*(p3.y-p1.y) - (p2.y-p1.y)*(p3.x-p1.x);
}
bool isInTriangle(Point p1,Point p2,Point p3,Point o) {
    //保证p1，p2，p3是逆时针顺序
    if(product(p1, p2, p3)<0) return isInTriangle(p1,p3,p2,o);
    if(product(p1, p2, o)>0 && product(p2, p3, o)>0 && product(p3, p1, o)>0)
        return true;
    return false;
}
int main() {
    Point p1,p2,p3,o;
    cin >> p1.x >> p1.y;
    cin >> p2.x >> p2.y;
    cin >> p3.x >> p3.y;
    cin >> o.x >> o.y;
    bool flag = isInTriangle(p1,p2,p3,o);
    if(flag) puts("Yes");
    else puts("No");
}

• 判断若p3在p1p2→的右侧！则表示输入的点的顺序是顺时针的，即A,C,B式的输入，将p2、p3调换位置即可保证顺序是逆时针。

（2）迭代结束条件

GJK 是一种 迭代算法，它需要不断地检测单纯形是否包含原点。它退出迭代的条件是

1. 单纯形包含原点

2. 单纯形最后添加的顶点与搜寻方向点乘小于0

（3）2D求垂线

3D：叉积（cross）得到的向量其实是一个垂直向量，垂直于两个进行叉积的向量构建的平面。

（4）找一条边面向原点的法向量方向

这种方法被称为矢量三重积

具体方式：

首先我们定义两个向量 AO 和 AB，如下图所示

我们可以通过 AO 和 AB 的叉乘，找到他们的所在平面的法向量方向，如下图所示

然后再将上一步的结果和 AB 做叉乘，就可以得到指向原点的目标方向。

（5）判断点是否在立方体内部

Rotated Region3: Rotated Boxes in 3D Space - Scripting Helpers

判断点在平面的哪侧：这就意味着，如果投影是负的我们就知道两个向量之间的角差大于90°这就意味着我们在平面下面。另一方面，如果投影是正的那么我们就知道角差小于90°并且我们在平面上方。最后，如果我们的投影是0，那么我们就知道给定的点实际上在平面上。

判断点是否在立方体内部：现在我们知道了如何判断一个点在平面上的哪一边，下一步就是把这个知识应用到我们旋转的区域上。概念很简单，我们用六个面法线都背离立方体中心的平面来定义一个立方体。这样，我们就可以对照所有6个平面来检查任意给定的点，如果有一个点在这6个平面某一个的“上方”，则点在区域外，如果这个点均在这6个平面的“下方”，点就在区域内。

（6）找四面体离原点最近的面

step1：根据右手螺旋定则，找到三个面的法向量（法向量方向为背离立方体中心的平面）；

step2：然后依次判断原点O在每个面的哪侧，如若在ABC面的外侧，则原点更接近ABC，用它作为新的单纯形；如果原点在所有面的内侧，则它一定在四面体内部，表明两个物体发生碰撞。

3. 3D code代码实现

code1

博客：Winter's Blog，github：IwEngine/IwEngine/include/iw/physics/impl at master · IainWinter/IwEngine (github.com)

code2

一个文件：gjktest/testopengl.cpp at main · matan45/gjktest (github.com)

code3

kevinmoran/GJK: Basic 3D collision detection implementation using the Gilbert–Johnson–Keerthi distance algorithm along with the Expanding Polytope Algorithm (github.com)

• translate()原理

使用Vec3.transformMat4转换本地坐标为世界坐标，结果为2倍的本地坐标？ - Creator 3.x - Cocos中文社区

世界坐标系和本地坐标系 - 快乐码原 | Yesmore