在自動駕駛系統運動規劃模塊的碰撞檢測中，通常分為 粗略碰撞檢測 和 精細碰撞檢測 兩個步驟。

粗略碰撞檢測用來将兩個明顯不相交的物體快速排除，使用 外接圓的包圍形 或 軸對齊包圍矩形（Axis Aligned Bounding Box，AABB）都是比較好的方式。

碰撞檢測算法之包圍形法

外接圓

AABB 和 OBB

基于AABB的碰撞檢測判斷

精細碰撞檢測則用來準确判斷兩個物體是否相交。分離軸定理（Separating Axis Theorem，SAT）是一種可用于精确判斷兩個物體是否相交的算法，不僅适用于 Box（矩形），還适用于 凸多邊形（Polygon）。基于分離軸定理的算法原理簡單，即隻要能找到一條可将兩個多邊形分開的直線，則這兩個多邊形不相交。

碰撞檢測算法之分離軸定理

在精細碰撞檢測階段，除了 SAT 算法，另外一個就是 GJK（Gilbert–Johnson–Keerthi）算法。相比 SAT 算法，GJK 算法更加高效。

在介紹 GJK 算法之前，我們需要先了解一些背景知識。

凸多邊形的定義：對于平面上的一個多邊形，如果延長它的任何一條邊，都使整個多邊形位于一邊延長線的同側，這樣的多邊形叫做凸多邊形。

根據上述定義，人可以直觀判斷出一個多邊形是否為凸多邊形，但在程序中，如何判斷一個多邊形是否為凸多邊形呢？

答案是利用向量的叉乘。如下圖所示，根據多邊形的頂點坐标，依次求出每條邊的向量。

• 若多邊形的頂點是逆時針序列，則向量的叉乘 a x b，b x c，c x d，d x e，e x a 的結果均大于0；
• 若多邊形的頂點是順時針序列，則向量叉乘的結果均小于0。
• 但若同時存在大于0 和 小于0 的結果，則說明是凹多邊形。

闵可夫斯基差，也可以叫做闵可夫斯基和，它的定義也很好理解，點集A與B的闵可夫斯基和被定義為：

A B = {a b | a ∈ A，b ∈ B}

如果 A 和 B 是兩個凸多邊形，則 A B 也是凸多邊形。

闵可夫斯基和從幾何上的直觀理解是A集合沿B的邊際連續運動一周掃過的區域與B集合本身的并集，也可以是B沿着A的邊界連續運動掃過區域與A自身的并集。

GJK算法用到的不是闵可夫斯基和，而是闵可夫斯基差，即：

A - B = {a - b | a ∈ A，b ∈ B}

雖然使用的是減法運算，但其仍然是闵可夫斯基和，相當于先對B中的所有點做負運算（相對原點的鏡像），然後再與A做加法。

GJK算法的核心就是闵可夫斯基差，即若兩個多邊形相交，則它們的闵可夫斯基差必然包括原點。

先來看兩個例子。

對于兩個不相交的多邊形，shape1為矩形，shape2為三角形，如下圖所示。

它們的闵可夫斯基差如下圖所示，其闵可夫斯基差不包括原點，且兩個多邊形之間的距離就是其闵可夫斯基差到原點的距離。事實上，GJK 算法發明出來的初衷就是為了計算兩個凸多邊形之間的距離。

對于兩個相交的多邊形，shape1為矩形，shape2為三角形，如下圖所示。

它們的闵可夫斯基差則如下圖所示，可以看到，闵可夫斯基差是包括原點的。這也很好理解，兩個相交的多邊形，必然有一點既屬于shape1，也屬于shape2，相減則為原點（0，0）。

k階單純形（simplex），指的是k維空間中的多胞形，該多胞形是k 1個頂點組成的凸包。

在GJK算法中，單純形被大量使用。單純形指的是點、線段、三角形或四面體。例如，0階單純形是點，1階單純形是線段，2階單純形是三角形，3階單純形是四面體。

對于2維空間的多邊形，最多用到2階單純形。那單純形到底有什麼作用呢？

對于上面兩個相交的多邊形例子，實際應用中，其實不需要求出完整的闵可夫斯基差，隻需要在闵可夫斯基差内形成一個多邊形，如下圖所示，并使這個多邊形盡可能包圍原點，這個多邊形就稱為單純形。即假如單純形包圍原點，則闵可夫斯基差必然包圍原點。

Support函數的作用是計算多邊形在給定方向上的最遠點。如下圖所示，在向量 a 方向的最遠點為 A 點，在向量 b 方向的最遠點為 B 點。這裡在尋找給定方向上的最遠點時，需要用到向量的點乘。

為什麼需要Support函數呢？這是因為在構建單純形時，我們希望盡可能得到闵可夫斯基差的頂點，而不是其内部的一個點，這樣産生的單純形才能包含最大的區域，增加算法的快速收斂性。

如下圖所示，在給定向量 a 方向上，shape1 的最遠點為（4，2），在向量 -a 的方向上，shape2 的最遠點為（5，3），這兩個點作差即得到點（-1，-1）。利用這種方式得到的點都在闵可夫斯基差的邊上。

理解向量的點乘和叉乘，如下圖所示。向量的點乘用來判斷兩個向量是否同方向；向量的叉乘用來判斷一點在一線段的左側還是右側。

向量法判斷點與線段的關系(一)

向量法判斷點與線段的關系(二)

有了上述背景知識，接下來理解 GJK 算法就比較容易了。

下圖是 GJK 算法的僞代碼，其核心邏輯是：給定兩個多邊形 p 和 q，以及一個初始方向，通過叠代的方式構建、更新單純形，并判斷單純形是否包含原點，若包含原點則兩個多邊形相交，否則不相交。

GJK 算法的具體步驟如下圖所示。

我們還是通過一個例子來理解上述步驟。

步驟1：選取初始方向 dir（0,1），如下圖所示；

步驟2：多邊形 p 在初始方向上 dir 的最遠點為（4,5），多邊形 q 在 -dir 方向上的最遠點為（1,0），因此第一個 support 點為（3,5）；

步驟3：将初始方向取反 dir 變成（0,-1）；

第一次循環：

步驟4a：根據叠代方向 dir（0,-1），得到第二個 support 點（3,-1），如下圖所示；

步驟4b：新的 support 點（3,-1） 與 叠代方向（0,-1） 的點乘結果大于0，說明跨越原點了；

步驟4c：新的 support 點能夠跨越原點，将其加到 simplex 中，此時 simplex 中有兩個點；

步驟4d：以這兩個點的直線的垂線朝向原點的方向（-1,0），作為下一次叠代方向，如下圖所示；

第二次循環：

步驟4a：根據叠代方向 dir（-1,0），得到 support 點（-1,2），如上圖所示；

步驟4b：新的 support 點（-1,2） 與 叠代方向（-1,0） 的點乘結果大于0，說明跨越原點了；

步驟4c：新的 support 點能夠跨越原點，将其加到 simplex 中，此時 simplex 中有三個點；

步驟4d：這三個點組成的三角形沒有包含原點，保留離原點最近的邊上的兩個點（-1,2）和（3,-1），同樣以這兩個點的直線的垂線朝向原點的方向，作為下一次叠代方向（-3,-4）；

第三次循環：

步驟4a：根據叠代方向 dir（-3,-4），得到 support 點（-1,-1），如下圖所示；

步驟4b：新的 support 點（-1,-1） 與 叠代方向（-3,-4） 的點乘結果大于0，說明跨越原點了；

步驟4c：新的 support 點能夠跨越原點，将其加到 simplex 中，此時 simplex 中有三個點；

步驟4d：這三個點組成的三角形包含原點了，說明這兩個多邊形相交。到此結束。

從上面的示例中可以看出，GJK 是一種基于叠代的算法，其收斂速度取決于叠代方向。

到這裡，我們對整個 GJK 算法步驟有了一個基本認識。但是，在上面的步驟4d中，如何判斷三角形是否包含原點，如何查找下一個叠代方向，以及如何計算兩個不相交的多邊形之間的距離，還需要有更細化的工作，這裡不再進行叙述，未來将直接把代碼開源出來，敬請期待。

更多精彩资讯请关注tft每日頭條，我们将持续为您更新最新资讯!