碰撞檢測(cè)算法中廣泛使用的一種方法（實(shí)時(shí)碰撞檢測(cè)算法技術(shù)）
在自動(dòng)駕駛系統(tǒng)運(yùn)動(dòng)規(guī)劃模塊的碰撞檢測(cè)中，通常分為 粗略碰撞檢測(cè) 和 精細(xì)碰撞檢測(cè) 兩個(gè)步驟。
粗略碰撞檢測(cè)用來將兩個(gè)明顯不相交的物體快速排除，使用 外接圓的包圍形 或 軸對(duì)齊包圍矩形（Axis Aligned Bounding Box，AABB）都是比較好的方式。
外接圓
AABB 和 OBB
基于AABB的碰撞檢測(cè)判斷
精細(xì)碰撞檢測(cè)則用來準(zhǔn)確判斷兩個(gè)物體是否相交。分離軸定理（Separating Axis Theorem，SAT）是一種可用于精確判斷兩個(gè)物體是否相交的算法，不僅適用于 Box（矩形），還適用于 凸多邊形（Polygon）?；诜蛛x軸定理的算法原理簡單，即只要能找到一條可將兩個(gè)多邊形分開的直線，則這兩個(gè)多邊形不相交。
在精細(xì)碰撞檢測(cè)階段，除了 SAT 算法，另外一個(gè)就是 GJK（Gilbert–Johnson–Keerthi）算法。相比 SAT 算法，GJK 算法更加高效。
在介紹 GJK 算法之前，我們需要先了解一些背景知識(shí)。
1、凸多邊形
凸多邊形的定義：對(duì)于平面上的一個(gè)多邊形，如果延長它的任何一條邊，都使整個(gè)多邊形位于一邊延長線的同側(cè)，這樣的多邊形叫做凸多邊形。
根據(jù)上述定義，人可以直觀判斷出一個(gè)多邊形是否為凸多邊形，但在程序中，如何判斷一個(gè)多邊形是否為凸多邊形呢？
答案是利用向量的叉乘。如下圖所示，根據(jù)多邊形的頂點(diǎn)坐標(biāo)，依次求出每條邊的向量。
- 若多邊形的頂點(diǎn)是逆時(shí)針序列，則向量的叉乘 a x b，b x c，c x d，d x e，e x a 的結(jié)果均大于0；
- 若多邊形的頂點(diǎn)是順時(shí)針序列，則向量叉乘的結(jié)果均小于0。
- 但若同時(shí)存在大于0 和 小于0 的結(jié)果，則說明是凹多邊形。
2、閔可夫斯基差
閔可夫斯基差，也可以叫做閔可夫斯基和，它的定義也很好理解，點(diǎn)集A與B的閔可夫斯基和被定義為：
A + B={a + b | a ∈ A，b ∈ B}
如果 A 和 B 是兩個(gè)凸多邊形，則 A + B 也是凸多邊形。
閔可夫斯基和從幾何上的直觀理解是A集合沿B的邊際連續(xù)運(yùn)動(dòng)一周掃過的區(qū)域與B集合本身的并集，也可以是B沿著A的邊界連續(xù)運(yùn)動(dòng)掃過區(qū)域與A自身的并集。
GJK算法用到的不是閔可夫斯基和，而是閔可夫斯基差，即：
A – B={a – b | a ∈ A，b ∈ B}
雖然使用的是減法運(yùn)算，但其仍然是閔可夫斯基和，相當(dāng)于先對(duì)B中的所有點(diǎn)做負(fù)運(yùn)算（相對(duì)原點(diǎn)的鏡像），然后再與A做加法。
GJK算法的核心就是閔可夫斯基差，即若兩個(gè)多邊形相交，則它們的閔可夫斯基差必然包括原點(diǎn)。
先來看兩個(gè)例子。
對(duì)于兩個(gè)不相交的多邊形，shape1為矩形，shape2為三角形，如下圖所示。
它們的閔可夫斯基差如下圖所示，其閔可夫斯基差不包括原點(diǎn)，且兩個(gè)多邊形之間的距離就是其閔可夫斯基差到原點(diǎn)的距離。事實(shí)上，GJK 算法發(fā)明出來的初衷就是為了計(jì)算兩個(gè)凸多邊形之間的距離。
對(duì)于兩個(gè)相交的多邊形，shape1為矩形，shape2為三角形，如下圖所示。
它們的閔可夫斯基差則如下圖所示，可以看到，閔可夫斯基差是包括原點(diǎn)的。這也很好理解，兩個(gè)相交的多邊形，必然有一點(diǎn)既屬于shape1，也屬于shape2，相減則為原點(diǎn)（0，0）。
3、單純形
k階單純形（simplex），指的是k維空間中的多胞形，該多胞形是k+1個(gè)頂點(diǎn)組成的凸包。
在GJK算法中，單純形被大量使用。單純形指的是點(diǎn)、線段、三角形或四面體。例如，0階單純形是點(diǎn)，1階單純形是線段，2階單純形是三角形，3階單純形是四面體。
對(duì)于2維空間的多邊形，最多用到2階單純形。那單純形到底有什么作用呢？
對(duì)于上面兩個(gè)相交的多邊形例子，實(shí)際應(yīng)用中，其實(shí)不需要求出完整的閔可夫斯基差，只需要在閔可夫斯基差內(nèi)形成一個(gè)多邊形，如下圖所示，并使這個(gè)多邊形盡可能包圍原點(diǎn)，這個(gè)多邊形就稱為單純形。即假如單純形包圍原點(diǎn)，則閔可夫斯基差必然包圍原點(diǎn)。
4、Support 函數(shù)
Support函數(shù)的作用是計(jì)算多邊形在給定方向上的最遠(yuǎn)點(diǎn)。如下圖所示，在向量 a 方向的最遠(yuǎn)點(diǎn)為 A 點(diǎn)，在向量 b 方向的最遠(yuǎn)點(diǎn)為 B 點(diǎn)。這里在尋找給定方向上的最遠(yuǎn)點(diǎn)時(shí)，需要用到向量的點(diǎn)乘。
為什么需要Support函數(shù)呢？這是因?yàn)樵跇?gòu)建單純形時(shí)，我們希望盡可能得到閔可夫斯基差的頂點(diǎn)，而不是其內(nèi)部的一個(gè)點(diǎn)，這樣產(chǎn)生的單純形才能包含最大的區(qū)域，增加算法的快速收斂性。
如下圖所示，在給定向量 a 方向上，shape1 的最遠(yuǎn)點(diǎn)為（4，2），在向量 -a 的方向上，shape2 的最遠(yuǎn)點(diǎn)為（5，3），這兩個(gè)點(diǎn)作差即得到點(diǎn)（-1，-1）。利用這種方式得到的點(diǎn)都在閔可夫斯基差的邊上。
5、向量的點(diǎn)乘和叉乘
理解向量的點(diǎn)乘和叉乘，如下圖所示。向量的點(diǎn)乘用來判斷兩個(gè)向量是否同方向；向量的叉乘用來判斷一點(diǎn)在一線段的左側(cè)還是右側(cè)。
有了上述背景知識(shí)，接下來理解 GJK 算法就比較容易了。
6、GJK 算法
下圖是 GJK 算法的偽代碼，其核心邏輯是：給定兩個(gè)多邊形 p 和 q，以及一個(gè)初始方向，通過迭代的方式構(gòu)建、更新單純形，并判斷單純形是否包含原點(diǎn)，若包含原點(diǎn)則兩個(gè)多邊形相交，否則不相交。
GJK 算法的具體步驟如下圖所示。
我們還是通過一個(gè)例子來理解上述步驟。
步驟1：選取初始方向 dir（0,1），如下圖所示；
步驟2：多邊形 p 在初始方向上 dir 的最遠(yuǎn)點(diǎn)為（4,5），多邊形 q 在 -dir 方向上的最遠(yuǎn)點(diǎn)為（1,0），因此第一個(gè) support 點(diǎn)為（3,5）；
步驟3：將初始方向取反 dir 變成（0,-1）；
第一次循環(huán)：
步驟4a：根據(jù)迭代方向 dir（0,-1），得到第二個(gè) support 點(diǎn)（3,-1），如下圖所示；
步驟4b：新的 support 點(diǎn)（3,-1） 與 迭代方向（0,-1） 的點(diǎn)乘結(jié)果大于0，說明跨越原點(diǎn)了；
步驟4c：新的 support 點(diǎn)能夠跨越原點(diǎn)，將其加到 simplex 中，此時(shí) simplex 中有兩個(gè)點(diǎn)；
步驟4d：以這兩個(gè)點(diǎn)的直線的垂線朝向原點(diǎn)的方向（-1,0），作為下一次迭代方向，如下圖所示；
第二次循環(huán)：
步驟4a：根據(jù)迭代方向 dir（-1,0），得到 support 點(diǎn)（-1,2），如上圖所示；
步驟4b：新的 support 點(diǎn)（-1,2） 與 迭代方向（-1,0） 的點(diǎn)乘結(jié)果大于0，說明跨越原點(diǎn)了；
步驟4c：新的 support 點(diǎn)能夠跨越原點(diǎn)，將其加到 simplex 中，此時(shí) simplex 中有三個(gè)點(diǎn)；
步驟4d：這三個(gè)點(diǎn)組成的三角形沒有包含原點(diǎn)，保留離原點(diǎn)最近的邊上的兩個(gè)點(diǎn)（-1,2）和（3,-1），同樣以這兩個(gè)點(diǎn)的直線的垂線朝向原點(diǎn)的方向，作為下一次迭代方向（-3,-4）；
第三次循環(huán)：
步驟4a：根據(jù)迭代方向 dir（-3,-4），得到 support 點(diǎn)（-1,-1），如下圖所示；
步驟4b：新的 support 點(diǎn)（-1,-1） 與 迭代方向（-3,-4） 的點(diǎn)乘結(jié)果大于0，說明跨越原點(diǎn)了；
步驟4c：新的 support 點(diǎn)能夠跨越原點(diǎn)，將其加到 simplex 中，此時(shí) simplex 中有三個(gè)點(diǎn)；
步驟4d：這三個(gè)點(diǎn)組成的三角形包含原點(diǎn)了，說明這兩個(gè)多邊形相交。到此結(jié)束。
從上面的示例中可以看出，GJK 是一種基于迭代的算法，其收斂速度取決于迭代方向。
到這里，我們對(duì)整個(gè) GJK 算法步驟有了一個(gè)基本認(rèn)識(shí)。但是，在上面的步驟4d中，如何判斷三角形是否包含原點(diǎn)，如何查找下一個(gè)迭代方向，以及如何計(jì)算兩個(gè)不相交的多邊形之間的距離，還需要有更細(xì)化的工作，這里不再進(jìn)行敘述，未來將直接把代碼開源出來，敬請(qǐng)期待。
