一、GJK 算法簡介
GJK 算法是由 Gilbert，Johnson，Keerthi 三位前輩發(fā)明的，用來計算兩個凸多面體之間的碰撞檢測，以及最近距離。
GJK 算法可以在 O ( M + N ) O(M+N) O(M+N) 的時間復(fù)雜度內(nèi)，檢測出碰撞，算法在每次迭代的過程中，都會優(yōu)先選擇靠近原點的方向，因此收斂速度會很快。算法的證明過程比較復(fù)雜，但是原理還是比較容易理解的。
GJK的初衷是確定兩個凸包之間的距離。GJK還可以用于在小距離穿透情況下獲取碰撞信息，并可以通過其他算法進行補充以實現(xiàn)更大距離的穿透。
二、前置知識
2.1 二維向量的點乘和叉乘
下圖來自知乎：算法集市
- 向量的點乘用來判斷兩個向量是否同方向
- 向量的叉乘用來判斷一點在一線段的左側(cè)還是右側(cè)
下面是C++代碼實現(xiàn)的向量點乘和叉乘（二維情形）
/*
二維向量叉乘
*/
double product(double* v1,double* v2){
return v1[0] * v2[1] - v1[1] * v2[0];
}
/*
二維向量點乘
*/
double dot(double* v1, double* v2){
return v1[0] * v2[0] + v1[1] * v2[1];
}
2.2 三維向量叉乘
下圖來自 向量的內(nèi)積與外積
C++代碼實現(xiàn)：
/*
三維向量叉乘
*/
double* product3D(double* v1, double* v2){
return new double[]{v1[1] * v2[2] - v2[1] * v1[2],-(v1[0] * v2[2] - v2[0] * v1[2]),v1[0] * v2[1] - v2[0] * v1[1]};
}
2.3 凸多邊形
凸多邊形是一個內(nèi)部為凸集的簡單多邊形。凸多邊形（Convex Polygon）指如果把一個多邊形的所有邊中，任意一條邊向兩方無限延長成為一直線時，其他各邊都在此直線的同旁，那么這個多邊形就叫做凸多邊形，其內(nèi)角全不是優(yōu)角，任意兩個頂點間的線段位于多邊形的內(nèi)部或邊上。
優(yōu)角(reflex angle)亦稱凹角，指大于平角(180°)而小于周角(360°)的角。直角、銳角和鈍角統(tǒng)稱劣角。
在程序中，我們可以利用向量的叉乘來判斷一個多邊形是否為凸多邊形：
- 若多邊形的頂點是逆時針序列，則向量的叉乘 a x b，b x c，c x d，d x e，e x a 的結(jié)果均大于0（abcde代表多邊形的5個頂點，這里只是舉例，實際不一定是5個）
- 若多邊形的頂點是順時針序列，則向量叉乘的結(jié)果均小于0
- 但若同時存在大于0 和 小于0 的結(jié)果，則說明是凹多邊形
下面是判斷多邊形是否為凸多邊形的C++代碼實現(xiàn)（二維情形）
/*
判斷一個多邊形是否為凸多邊形
*/
bool isConvex(vector<double*> poly){
// 判斷多邊形是否為順時針
bool clockwise = product(new double[]{poly[1][0] - poly[0][0], poly[1][1] - poly[0][1]}, new double[]{-1, 0}) >= 0;
for (int i = 0; i < poly.size(); i++){
double d = i + 1 < poly.size() ? product(poly[i], poly[i + 1]) : product(poly[i], poly[0]);
if ((clockwise && d > 0) || (!clockwise && d < 0)){
return false;
}
}
return true;
}
2.4 閔可夫斯基差
閔可夫斯基差，也可以叫做閔可夫斯基和，它的定義也很好理解，點集A與B的閔可夫斯基和被定義為：
A + B = { a + b ∣ a ∈ A ， b ∈ B } A + B = \{a + b | a ∈ A，b ∈ B\} A+B={a+b∣a∈A，b∈B}
定理：如果 A 和 B 是兩個凸多邊形，則 A + B 也是凸多邊形。
閔可夫斯基和從幾何上的直觀理解是A集合沿B的邊際連續(xù)運動一周掃過的區(qū)域與B集合本身的并集，也可以是B沿著A的邊界連續(xù)運動掃過區(qū)域與A自身的并集。
GJK算法用到的不是閔可夫斯基和，而是閔可夫斯基差，即：
A ? B = { a ? b ∣ a ∈ A ， b ∈ B } A - B = \{a - b | a ∈ A，b ∈ B\} A?B={a?b∣a∈A，b∈B}
雖然使用的是減法運算，但其仍然是閔可夫斯基和，相當(dāng)于先對B中的所有點做負運算（相對原點的鏡像），然后再與A做加法
運用：GJK算法的核心就是閔可夫斯基差，即若兩個多邊形相交，則它們的閔可夫斯基差必然包括原點
下面來看兩個例子：
第一個例子：下圖展示了多邊形A和B相交的情況，他們的閔可夫斯基差包含原點
第二個例子：下圖展示了多邊形A和B不相交的情況，他們的閔可夫斯基差沒有包含原點
2.5 單純形
k 階單純形（simplex），指的是k維空間中的多胞形，該多胞形是 k+1 個頂點組成的凸包。
在 GJK 算法中，單純形被大量使用。單純形指的是點、線段、三角形或四面體。例如，0階單純形是點，1階單純形是線段，2階單純形是三角形，3階單純形是四面體。
對于二維空間的多邊形，最多用到2階單純形。那單純形到底有什么作用呢？
對于上面兩個相交的多邊形例子，實際應(yīng)用中，其實不需要求出完整的閔可夫斯基差，只需要在閔可夫斯基差內(nèi)形成一個多邊形，如下圖所示，并使這個多邊形盡可能包圍原點，這個多邊形就稱為單純形。即假如單純形包圍原點，則閔可夫斯基差必然包圍原點。
2.6 Support 函數(shù)
Support 函數(shù)的作用是計算多邊形在給定方向上的最遠點。
如下圖所示，在向量 d 方向的最遠點為 v0 點。這里在尋找給定方向上的最遠點時，需要用到向量的點乘。我們可以遍歷每個頂點和向量d的點乘，找到點乘值最大的頂點，它就是向量 d 方向的最遠點。這個點也被稱為支撐點。
為什么需要Support函數(shù)呢？這是因為在構(gòu)建單純形時，我們希望盡可能得到閔可夫斯基差的頂點，而不是其內(nèi)部的一個點，這樣產(chǎn)生的單純形才能包含最大的區(qū)域，增加算法的快速收斂性。
值得一提的是，針對一些特殊的多邊形，例如：圓、橢圓等，我們可以利用其參數(shù)方程，很容易的得到其方向 d 上的最遠頂點。
下面是C++代碼實現(xiàn)常規(guī)多邊形、圓形和橢圓形的 Support 函數(shù)
/*
Support 函數(shù)（常規(guī)多邊形）
*/
double* support(vector<double*> poly, double* direction){
int maxIndex = 0;
double maxDot = dot(new double[]{poly[0][0], poly[0][1]}, direction);
for (int i = 1; i < poly.size(); i++){
double d = dot(new double[]{poly[i][0], poly[i][1]}, direction);
if (d > maxDot){
maxDot = d;
maxIndex = i;
}
}
return new double[]{poly[maxIndex][0], poly[maxIndex][1]};
}
/*
計算兩個二維向量的夾角（度）[0,PI]
*/
double calc2DVectorsAngle(double* v1, double* v2){
double d1 = sqrt(pow(v1[0] - 0, 2) + pow(v1[1] - 0, 2));
double d2 = sqrt(pow(v2[0] - 0, 2) + pow(v2[1] - 0, 2));
// 獲取弧度夾角 [0,PI]
return acos((v1[0] * v2[0] + v1[1] * v2[1]) / (d1*d2));
}
/*
Support 函數(shù)（圓形）
*/
double* supportCircle(double* centerPoint,double r, double* direction){
// 獲取theta
double theta = calc2DVectorsAngle(direction, new double[]{1, 0});
if (direction[1] < 0){
theta = 2 * PI - theta;
}
// 根據(jù)圓的參數(shù)方程返回支撐點
return new double[]{centerPoint[0] + r * cos(theta), centerPoint[1] + r * sin(theta)};
}
/*
Support 函數(shù)（橢圓形）
*/
double* supportEillpse(double* centerPoint, double a,double b, double* direction){
// 獲取theta
double theta = calc2DVectorsAngle(direction, new double[]{1, 0});
if (direction[1] < 0){
theta = 2 * PI - theta;
}
// 根據(jù)橢圓的參數(shù)方程返回支撐點
return new double[]{centerPoint[0] + a * cos(theta), centerPoint[1] + b * sin(theta)};
}
綜上所述，只要需要進行重疊判斷的兩個多邊形都可以被Support，且都為凸多邊形，那么就可以采用GJK算法進行求解
三、GJK 算法講解
有了上述前置知識，接下來就可以開始講解 GJK 算法了
首先我們要明確的，GJK 的核心邏輯是：給定兩個多邊形 A 和 B，以及一個初始方向，通過迭代的方式構(gòu)建、更新單純形，并判斷單純形是否包含原點，若包含原點則兩個多邊形相交，否則不相交。
3.1 熟悉 GJK 算法流程
下面讓我們用兩個例子，熟悉一下GJK 算法的流程
3.1.1 多邊形重疊的情形
首先，我們可以通過隨機的方式，得到一個初始方向，然后，其中一個多邊形用初始方向找支撐點，另一個多邊形則用初始方向的反方向找支撐點。這樣就得到了兩個支撐點
然后根據(jù)找到的兩個支撐點，做閔可夫斯基差，得到第一個單純形的頂點
根據(jù)經(jīng)驗，我們知道下一個最有探索意義的方向是第一個單純形頂點朝向原點的方向，如下圖所示。同樣，方向確定，我們可以通過 Support 函數(shù)分別得到兩個多邊形在該方向上的支撐點
然后，再根據(jù)得到的兩個支撐點做閔可夫斯基差，得到單純形的第二個頂點，如下圖所示
我們現(xiàn)在可以做一個理智的判斷，看看我們是否越過了原點。我們可以過原點，做垂直于方向D的一條線（二維情形），如果這條線可以把第一個加入單純形的點和第二個加入單純形的點分開到A和B兩邊，那么說明其可以跨越原點。如果不能分到A和B兩邊，說明其單純形不可能包含原點，可以直接判斷兩個多邊形不相交（提前終止條件）。
我們現(xiàn)在需要選擇下一個方向，GJK 選擇的方向是當(dāng)前線段面向原點的法向量方向。然后我們可以根據(jù)這個方向分別得到兩個多邊形的支撐點
根據(jù)得到的兩個支撐點做閔可夫斯基差，得到單純形的第三個頂點。同樣，我們做理智的檢查以確保我們通過了原點。
由于當(dāng)前單純形已經(jīng)有了三個頂點，構(gòu)成一個三角形，我們需要判斷三角形中是否包含原點，很顯然，它沒有。所以我們的下一步是找到另一個新的三角形
為了找到新的三角形，我們需要一個新的方向，GJK 選擇的是當(dāng)前三角形中最接近原點的邊的面向原點的法向量方向，如下圖所示，我們可以得到新的頂點。然后用新的頂點和剛剛用到的邊構(gòu)成新的三角形。此時，新三角形包含了原點，所以我們可以斷定，兩個多邊形是重疊的。至此，程序結(jié)束。
3.1.2 多邊形不重疊的情形
下面，我們觀察多邊形不重疊的情形，看看會發(fā)生什么
首先，我們用同樣的初始化方式得到單純形的第一個頂點
下一個方向是向著原點，我們可以得到第二個頂點，且這個點是經(jīng)過原點的（通過了檢查）
然后，我們根據(jù)單純形中前兩個點組成的線段的面向原點的法向量方向得到單純形的第三個點，它也經(jīng)過原點，通過了檢查。
由于當(dāng)前單純形中已經(jīng)有三個點了，所以我們可以判斷它構(gòu)成的三角形是否包含原點，如下圖所示，三角形沒有包含原點
我們將新方向設(shè)置為三角形中最靠近原點的邊的面向原點的法向量方向，然后計算出新的單純形頂點，然而我們得到的單純形頂點已經(jīng)存在于當(dāng)前單純形中了，所以我們可以斷定兩個多邊形肯定沒有重疊。至此，程序結(jié)束。
3.2 總結(jié) GJK 算法步驟
- 通過隨機的方式獲取初始方向
- 根據(jù)初始方向，用 Support 函數(shù)分別得到兩個多邊形的支撐點，再做閔可夫斯基差，獲得第一個頂點，并放到單純形中
- 以第一個頂點面向原點的方向作為第二次迭代方向
- 根據(jù)第二次迭代方向，用 Support 函數(shù)分別得到兩個多邊形的支撐點，再做閔可夫斯基差，獲得第二個頂點，此時對第二個頂點做過原點檢查，如果沒有通過檢查則能夠斷定兩個多邊形沒有重疊（程序結(jié)束），否則繼續(xù)下面的步驟
- 將第二個頂點加入單純形
- 以第一個和第二個頂點構(gòu)成的線段的面向原點的法向量方向作為第三次迭代的方向
- 根據(jù)第三次迭代方向，用 Support 函數(shù)分別得到兩個多邊形的支撐點，再做閔可夫斯基差，獲得第三個頂點，此時對第三個頂點做過原點檢查，如果沒有通過檢查則能夠斷定兩個多邊形沒有重疊（程序結(jié)束），否則繼續(xù)下面的步驟
- 將第三個頂點加入單純形
- 開始循環(huán)：
- 判斷當(dāng)前單純形中三個頂點組成的三角形是否包含原點，如果包含則可以斷定兩個多邊形重疊（程序結(jié)束），否則進行下面的步驟
- 以當(dāng)前單純形中三個頂點組成的三角形中最靠近原點的邊B的面向原點的法向量方向作為下一次的迭代方向D
- 根據(jù)迭代方向D，用 Support 函數(shù)分別得到兩個多邊形的支撐點，再做閔可夫斯基差，獲得新的頂點
- 對新的頂點做過原點檢查，如果沒有通過檢查則能夠斷定兩個多邊形沒有重疊（程序結(jié)束），否則繼續(xù)下面的步驟
- 如果新的頂點已經(jīng)存在于當(dāng)前單純形，那么可以斷定兩個多邊形沒有重疊（程序結(jié)束），否則繼續(xù)下面的步驟
- 以新的頂點和邊B的兩個端點構(gòu)成新的單純形，返回到循環(huán)的第一步
3.3 講解 GJK 算法細節(jié)
3.3.1 如何檢查新的頂點是否過原點？
如下圖所示，如果我們要判斷A點相對B點是否過了原點，那么就可以判斷原點指向A點的向量和B點指向原點的向量的點乘是否小于0，如果小于0，則A沒過原點，否則A過原點。
3.3.2 如何找到一條邊面向原點的法向量方向？
首先我們定義兩個向量 AO 和 AB，如下圖所示
我們可以通過 AO 和 AB 的叉乘，找到他們的所在平面的法向量方向，如下圖所示
然后再將上一步的結(jié)果和 AB 做叉乘，就可以得到指向原點的目標方向。
這種方法被稱為矢量三重積
3.3.3 如何判斷一點是否在三角形內(nèi)部？
可以采用三角形面積法進行判斷。
計算三角形面積可以采用海倫公式
3.3.4 如何找到三角形中離原點最近的邊？
首先，我們要了解，如何計算原點到邊的距離。
要計算點到邊的距離，在二維情形下可以很簡單的用點到直線距離公式求得。
但是我們要先根據(jù)邊的兩個端點，獲取直線方程
設(shè)邊的兩個端點坐標分別為 （ x 1 ， y 1 ）（ x 2 ， y 2 ） （x_1，y_1）（x_2，y_2） （x1?，y1?）（x2?，y2?）
根據(jù)斜截式可得：
-
求斜率： k = ( y 2 ? y 1 ) / ( x 2 ? x 1 ) k=(y_2-y_1)/(x_2-x_1) k=(y2??y1?)/(x2??x1?)
-
再把 k k k 代入 y ? y 1 = k ( x ? x 1 ) y-y_1=k(x-x_1) y?y1?=k(x?x1?) 即可得到直線方程
-
化為標準式： A x + B y + C = 0 Ax + By + C = 0 Ax+By+C=0，其中 A = k , B = ? 1 , C = y 1 ? k × x 1 A =k ,B =-1 ,C=y_1-k×x_1 A=k,B=?1,C=y1??k×x1?
?設(shè)直線? L ?的方程為? A x + B y + C = 0 ?，點? P ?的坐標為? ( x 0 , y 0 ) ?，則點? P ?到直線? L ?的距離為:? ∣ A x 0 + B y 0 + C ∣ A 2 + B 2 \text { 設(shè)直線 } \mathrm{L} \text { 的方程為 } \mathrm{Ax}+\mathrm{By}+\mathrm{C}=0 \text { ，點 } \mathrm{P} \text { 的坐標為 }(\mathrm{x} 0, \mathrm{y} 0) \text { ，則點 } \mathrm{P} \text { 到直線 } \mathrm{L} \text { 的距離為: } \frac{\left|A x_0+B y_0+C\right|}{\sqrt{A^2+B^2}} ?設(shè)直線?L?的方程為?Ax+By+C=0?，點?P?的坐標為?(x0,y0)?，則點?P?到直線?L?的距離為:?A2+B2?∣Ax0?+By0?+C∣?
C++代碼實現(xiàn)：
/*
傳入三個點，計算點p0到p1和p2構(gòu)成的邊的距離
*/
double calcPointToLineDistance(double* p0,double* p1,double* p2){
double k = (p2[1] - p1[1]) / (p2[0] - p1[0]);
double A = k;
double B = -1;
double C = p1[1] - k * p1[0];
return abs(A * p0[0] + B * p0[1] + C) / sqrt(A*A+B*B);
}
知道如何計算計算原點到邊的距離之后，我們只需要對三角形的三條邊進行遍歷，即可找到離原點最近的邊了。
四、C++ 完整代碼（含測試樣例）
/*
描述：本代碼實現(xiàn)了GJK算法求解二維情形下兩個凸多邊形的重疊判斷問題
時間：2023-1-21 15:41
作者：WSKH
博客：wskh0929.blog.csdn.net
*/
#include <iostream>
#include <vector>
// 圓周率
#define PI acos(-1)
// 浮點型誤差精度
#define ERROR 0.0000001
using namespace std;
// 原點坐標
double* origin = new double[]{0, 0};
/*
打印單純形信息
*/
void printSimplex(int epoch, vector<double*> simplex){
cout << "---------------------------------- 第 " << epoch << " 次迭代 , 單純形的頂點信息 ----------------------------------" << endl;
for (int i = 0; i < simplex.size(); i++){
cout << "(" <<simplex[i][0] << " , " << simplex[i][1] << ")" << endl;
}
}
/*
打印終止信息
flag = 0: 過原點檢查不通過，提前返回 false
flag = 1: 三角形包含原點，提前返回 true
flag = 2: 新頂點重復(fù)存在，提前返回 false
*/
void printStopInfo(int epoch, int flag){
cout << "第 " << epoch << " 次迭代：";
switch (flag)
{
case 0:
cout << "過原點檢查不通過，提前返回 false" << endl;
break;
case 1:
cout << "三角形包含原點，提前返回 true" << endl;
break;
case 2:
cout << "新頂點重復(fù)存在，提前返回 false" << endl;
break;
default:
break;
}
}
/*
判斷兩個二維向量/點是否相等
*/
bool isEquals(double* p1,double* p2){
return abs(p1[0] - p2[0]) < ERROR && abs(p1[1] - p2[1]);
}
/*
計算兩個點的距離
*/
double calcPointToPointDistance(double* p1, double* p2){
return sqrt(pow(p1[0] - p2[0],2) + pow(p1[1] - p2[1],2));
}
/*
二維向量相減
*/
double* diff(double* v1, double* v2){
return new double[]{v1[0] - v2[0], v1[1] - v2[1]};
}
/*
二維向量相加
*/
double* sum(double* v1, double* v2){
return new double[]{v1[0] + v2[0], v1[1] + v2[1]};
}
/*
二維向量叉乘
*/
double product(double* v1,double* v2){
return v1[0] * v2[1] - v1[1] * v2[0];
}
/*
二維向量點乘
*/
double dot(double* v1, double* v2){
return v1[0] * v2[0] + v1[1] * v2[1];
}
/*
三維向量叉乘
*/
double* product3D(double* v1, double* v2){
return new double[]{v1[1] * v2[2] - v2[1] * v1[2],-(v1[0] * v2[2] - v2[0] * v1[2]),v1[0] * v2[1] - v2[0] * v1[1]};
}
/*
判斷一個多邊形是否為凸多邊形
*/
bool isConvex(vector<double*> poly){
// 判斷多邊形是否為順時針
bool clockwise = product(new double[]{poly[1][0] - poly[0][0], poly[1][1] - poly[0][1]}, new double[]{-1, 0}) >= 0;
for (int i = 0; i < poly.size(); i++){
double d = i + 1 < poly.size() ? product(poly[i], poly[i + 1]) : product(poly[i], poly[0]);
if ((clockwise && d > 0) || (!clockwise && d < 0)){
return false;
}
}
return true;
}
/*
Support 函數(shù)（常規(guī)多邊形）
*/
double* support(vector<double*> poly, double* direction){
int maxIndex = 0;
double maxDot = dot(new double[]{poly[0][0], poly[0][1]}, direction);
for (int i = 1; i < poly.size(); i++){
double d = dot(new double[]{poly[i][0], poly[i][1]}, direction);
if (d > maxDot){
maxDot = d;
maxIndex = i;
}
}
return new double[]{poly[maxIndex][0], poly[maxIndex][1]};
}
/*
計算兩個二維向量的夾角（度）[0,PI]
*/
double calc2DVectorsAngle(double* v1, double* v2){
double d1 = sqrt(pow(v1[0] - 0, 2) + pow(v1[1] - 0, 2));
double d2 = sqrt(pow(v2[0] - 0, 2) + pow(v2[1] - 0, 2));
// 獲取弧度夾角 [0,PI]
return acos((v1[0] * v2[0] + v1[1] * v2[1]) / (d1*d2));
}
/*
Support 函數(shù)（圓形）
*/
double* supportCircle(double* centerPoint,double r, double* direction){
// 獲取theta
double theta = calc2DVectorsAngle(direction, new double[]{1, 0});
if (direction[1] < 0){
theta = 2 * PI - theta;
}
// 根據(jù)圓的參數(shù)方程返回支撐點
return new double[]{centerPoint[0] + r * cos(theta), centerPoint[1] + r * sin(theta)};
}
/*
Support 函數(shù)（橢圓形）
*/
double* supportEillpse(double* centerPoint, double a,double b, double* direction){
// 獲取theta
double theta = calc2DVectorsAngle(direction, new double[]{1, 0});
if (direction[1] < 0){
theta = 2 * PI - theta;
}
// 根據(jù)橢圓的參數(shù)方程返回支撐點
return new double[]{centerPoint[0] + a * cos(theta), centerPoint[1] + b * sin(theta)};
}
/*
根據(jù)給定方向和多邊形，獲取單純形新頂點
*/
double* getNewVertex(vector<double*> poly1, vector<double*> poly2, double* direction){
double* supportPoint1 = support(poly1, direction);
double* supportPoint2 = support(poly2, new double[]{-direction[0], -direction[1]});
return diff(supportPoint1, supportPoint2);
}
/*
獲取初始方向
*/
double* getInitDirection(){
return new double[]{1, 0};
}
/*
判斷兩個點是否過原點
*/
bool isCrossingOrigin(double* A,double* B){
return dot(A, diff(origin, B)) >= 0;
}
/*
傳入三個點，計算點p0到p1和p2構(gòu)成的邊的距離
*/
double calcPointToLineDistance(double* p0,double* p1,double* p2){
double k = (p2[1] - p1[1]) / (p2[0] - p1[0]);
double A = k;
double B = -1;
double C = p1[1] - k * p1[0];
return abs(A * p0[0] + B * p0[1] + C) / sqrt(A*A+B*B);
}
/*
傳入兩個點，獲取它構(gòu)成的邊面向原點的法向量
*/
double* getLineFaceToOriginVector(double* A, double* B){
double* AB = diff(B, A);
double* AO = diff(origin, A);
double* res = product3D(new double[]{AB[0], AB[1], 0}, new double[]{AO[0], AO[1],0});
return product3D(res, new double[]{AB[0], AB[1], 0});
}
/*
傳入三個點，根據(jù)海倫公式計算三角形面積
*/
double calcTriangleArea(double* p1, double* p2, double* 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(double* p1, double* p2, double* 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);
return abs(s1 + s2 + s3 - s) < ERROR;
}
/*
GJK算法（返回兩個多邊形是否重疊）
*/
bool GJK(vector<double*> poly1, vector<double*> poly2){
// 初始化單純形
vector<double*> simplex;
// 第1次迭代
double* direction = getInitDirection();
double* vertex = getNewVertex(poly1,poly2,direction);
simplex.push_back(vertex);
printSimplex(1, simplex);
// 第2次迭代
direction = diff(origin, vertex);
vertex = getNewVertex(poly1, poly2, direction);
// 過原點檢查
if (!isCrossingOrigin(simplex[0], vertex)){
printStopInfo(2, 0);
return false;
}
simplex.push_back(vertex);
printSimplex(2, simplex);
// 第3次迭代
direction = getLineFaceToOriginVector(simplex[1], simplex[0]);
vertex = getNewVertex(poly1, poly2, direction);
// 過原點檢查
if (!isCrossingOrigin(simplex[0], vertex)){
printStopInfo(3, 0);
return false;
}
simplex.push_back(vertex);
printSimplex(3, simplex);
// 開始循環(huán)
for (int epoch = 4;; epoch++){
// 判斷當(dāng)前單純形的三個頂點組成的三角形是否包含原點
if (isContainOrigin(simplex[0], simplex[1], simplex[2])){
printStopInfo(epoch - 1, 1);
return true;
}
// 找到三角形離原點最近的邊
double minDistance;
int minIndex1 = -1;
int minIndex2 = -1;
for (int i = 0; i < simplex.size(); i++){
for (int j = i + 1; j < simplex.size(); j++){
double distance = calcPointToLineDistance(origin, simplex[i], simplex[j]);
if (minIndex1 == -1 || distance < minDistance){
minDistance = distance;
minIndex1 = i;
minIndex2 = j;
}
}
}
// 找方向
direction = getLineFaceToOriginVector(simplex[minIndex1], simplex[minIndex2]);
vertex = getNewVertex(poly1, poly2, direction);
// 是否存在于當(dāng)前單純形檢查
for (int i = 0; i < simplex.size(); i++){
if (isEquals(simplex[i], vertex)){
printStopInfo(epoch, 2);
return false;
}
}
// 過原點檢查
if (!isCrossingOrigin(simplex[0], vertex)){
printStopInfo(epoch, 0);
return false;
}
// 更新單純形
double* vertex1 = simplex[minIndex1];
double* vertex2 = simplex[minIndex2];
simplex.clear();
simplex.push_back(vertex);
simplex.push_back(vertex1);
simplex.push_back(vertex2);
printSimplex(epoch, simplex);
}
}
/*
程序主函數(shù)
*/
int main(){
// 返回碼
int returnCode = 0;
// 創(chuàng)建兩個多邊形 poly1 和 poly2
vector<double*> poly1;
poly1.push_back(new double[]{0,0});
poly1.push_back(new double[]{3,0});
poly1.push_back(new double[]{3,3});
poly1.push_back(new double[]{0,3});
vector<double*> poly2;
// 重疊測試
//poly2.push_back(new double[]{2,2});
//poly2.push_back(new double[]{5,2});
//poly2.push_back(new double[]{5,5});
//poly2.push_back(new double[]{2,5});
// 不重疊測試
poly2.push_back(new double[]{3, 3});
poly2.push_back(new double[]{5, 3});
poly2.push_back(new double[]{3, 5});
poly2.push_back(new double[]{3, 5});
// 檢驗兩個多邊形是否為凸
if (!isConvex(poly1)){
cout << "錯誤: poly1 為凹多邊形" << endl;
returnCode = -1;
}
if (!isConvex(poly2)){
cout << "錯誤: poly2 為凹多邊形" << endl;
returnCode = -1;
}
// 調(diào)用GJK算法進行重疊判斷
if (returnCode == 0){
bool isOverlap = GJK(poly1, poly2);
cout << "重疊判斷結(jié)果為: 兩個多邊形 <" << (isOverlap ? "重疊" : "未重疊") << ">" << endl;
}
system("pause");
return returnCode;
}
4.1 重疊測試
運行結(jié)果展示：
4.2 不重疊測試
文章來源：http://www.zghlxwxcb.cn/news/detail-721422.html
文章來源地址http://www.zghlxwxcb.cn/news/detail-721422.html
到了這里，關(guān)于【計算幾何】凸多面體重疊判斷算法：GJK 算法詳解 & C++代碼實現(xiàn)二維情形的凸多邊形重疊判斷的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
![[計算幾何] 2 二維凸包/笨蛋(我)也能看懂的二維凸包算法](https://imgs.yssmx.com/Uploads/2024/02/618062-1.png)
