引擎開發_ 碰撞檢測_GJK 算法詳細介紹

原地址：https://blog.csdn.net/heyuchang666/article/details/55192932

概述

和SAT(分離軸定理)算法一樣，GJK算法也只對凸體有效。 GJK算法的優勢是：通過support函數（後面會詳細講述），從而支持任何凸體形狀之間的碰撞檢測；相比SAT算法，你不需要一些額外的操作，比如增加特殊的代碼和算法處理曲面形狀。

GJK是一個迭代算法，但是如果事先給出穿透/分離向量，則它的收斂會很快，可以在常量時間內完成。在3D環境中，GJK可以取代SAT算法。GJK算法的最初目的是計算兩個凸體之間的距離，在兩個物體穿透深度比較小的情況下，可用它判定物體之間的碰撞。它也可以和別的算法相結合，用來檢測兩個物體之間深度穿透時候的碰撞情況。

凸體(凸多面體或凸多邊形)

面說過，GJK算法只適用於凸體形狀。凸體(其實就是一條直線穿越凸體，和該凸體殼的交點不能超過2個)的定義在介紹SAT算法時講過，可參照那篇文章瞭解相關信息。

明可夫斯基和(Minkowski Sum)

GJK算法中使用了明可夫斯基和的概念。明可夫斯基和很好理解，假設有兩個物體，它們的明可夫斯基和就是物體1上的所有點和物體2上的所有點的和集。用公式表示就是：

A + B = {a + b|a∈A, b∈B}

如果兩個物體都是凸體，它們的明可夫斯基和也是凸體。

對於減法，明可夫斯基和的概念也成立，這時也可稱作明可夫斯基差。

A – B = A + (-B) = {a + (– b)|a∈A, b∈B} = {a – b)|a∈A, b∈B}

接着往下講，在兩個物體之間執行明可夫斯基差操作的解釋如下：

如果兩個物體重疊或者相交，它們的明可夫斯基差肯定包括原點。

我們看一個例子，圖1中兩個物體進行明可夫斯基差操作，將得到圖2的形狀。可以看到，該形狀包含原點，這是因爲這兩個物體是相交的。

執行這些操作需要物體1的頂點數*物體2的頂點數*2(原作者用的二維向量，如果在三維空間，當然就是*3了，如果是向量減法數量就什麼都不用乘了) 個減法操作。物體包含無窮多個點，但由於是凸體，我們可以只對它們的頂點執行明可夫斯基差操作。在執行GJK算法過程中，實際上我們並不需要顯式計算物體之間明可夫斯基差，這也是GJK算法的優勢所在。

單純形(Simplex)

我們不需要顯式計算物體之間的明可夫斯基差，只要知道它們的明可夫斯基差是否包含原點就ok了。如果包含原點，物體之間就相交，否則，則不相交。

我們可以在明可夫斯基差形成的物體內迭代的形成一個多面體(或多變形），並使這個多面體儘量包圍原點。如果這個多面體包含原點，顯然明可夫斯基差形成的物體必然包括原點。這個多面體就稱作單純形。

Support函數

下面我們講述如何建立一個單純形？首先看什麼是support函數，給定兩個凸體，該函數返回這兩個凸體明可夫斯基差形狀中的一個點。我們知道，物體1上的一個點，它的位置減去物體2上的一個點的位置，可以得到它們明可夫斯基差形狀上的一個點，但我們不希望每次都得到相同的點。如何保證做到這一點呢？我們可以給support函數傳遞一個參數，該參數表示一個方向(direction),方向不同，得到的點也不同。

在某個方向上選擇最遠的點有重要作用，因爲這樣產生的單純形包含最大的空間區域，增加了算法快速返回的可能。另外，通過這種方式返回的點都在明可夫斯基差形狀的邊上。如果我們不能通過一個過原點的方向在單純形上增加一個點，則明可夫斯基差不過原點，這樣在物體不想交的情況下，算法會很快退出。

public Point support(Shape shape1, Shape shape2, Vector d) {
// d is a vector direction (doesn't have to be normalized)
// get points on the edge of the shapes in opposite directions
Point p1 = shape1.getFarthestPointInDirection(d);
Point p2 = shape2.getFarthestPointInDirection(d.negative());
// perform the Minkowski Difference
Point p3 = p1.subtract(p2);
// p3 is now a point in Minkowski space on the edge of the Minkowski Difference
return p3;
}

下面這個例子使用圖2的物體形狀，執行函數三次

開始操作，使用d = (1, 0)

p1 = (9, 9);
p2 = (5, 7);
p3 = p1 - p2 = (4, 2);

第二步，使用d = (-1, 0)

p1 = (4, 5);
p2 = (12, 7);
p3 = p1 - p2 = (-8, -2);

注意：p1可能是 (4, 5) 或者 (4, 11)。這兩個都將在明可夫斯差形狀的邊上產生一個點。

下一步假定 d = (0, 1)

p1 = (4, 11);
p2 = (10, 2);
p3 = p1 - p2 = (-6, 9);

這樣，我們得到圖3所示的單純形。

判定碰撞

前面我們說過，兩個物體的明可夫斯基差中的單純形包含原點時候，這個兩個物體相交。在圖3中，單純形沒有包含原點，但實際上，這兩個物體是相交的。問題在於我們選擇的方向，在第三步中，如果我們選擇d = (0, -1) 作爲方向，那麼

p1 = (4, 5);
p2 = (5, 7);
p3 = p1 - p2 = (-1, -2);

這樣產生的單純形如圖4所示，顯然它包含了原點，我們由此能夠判定這兩個物體之間有碰撞。

可見，方向的選擇影響輸出結果。如果我們得到的單純形不包含原點的話，我們能夠用另一個點代替，產生新的單純形來判斷是否碰撞。這也是這個算法需要迭代的原因。我們不能保證我們最初選擇的三個點包含原點，也不能保證明可夫斯基差形狀包含原點。

如果我們改變第三個明可夫斯基差圖形中選擇點的方式，我們就能夠包圍原點，改變的方式如下所示：

d = ...
a = support(..., d)
d = ...
b = support(..., d)
AB = a - b
AO = a - ORIGIN
d = AB x AO x AB
c = support(..., d)

c中使用的方向d依賴於a和b形成的直線，通過用相反的方向，我們可以選擇離a最遠的b點。

d = ...
a = support(..., d)
b = support(..., -d)
AB = a - b
AO = a - ORIGIN
d = AB x AO x AB
c = support(..., d)

現在，我們僅需要爲第一次的明可夫斯基差圖形求邊界交點選擇方向。當然我們也可以選擇任意的方向，比如兩個物體形狀中心的差作爲方向等等。怎樣選擇有很多的優化工作去做。

迭代

即使我們使用上面的方法，也仍有可能在三步內不能得到包含原點的單純形，所以我們必須用迭代的方法創建單純形，每次生成的單純形比上次更接近包含原點。我們也需要檢查兩個條件：1）現在的單純形包含原點嗎? 2)我們能夠包含原點嗎?

下面我們看看迭代算法主要代碼：

Vector d = // choose a search direction
// get the first Minkowski Difference point
Simplex.add(support(A, B, d));
//下面開始循環: 第一次迭代
// negate d for the next point
d.negate();
// start looping
while (true) {
// add a new point to the simplex because we haven't terminated yet
Simplex.add(support(A, B, d));
// make sure that the last point we added actually passed the origin
if (Simplex.getLast().dot(d) <= 0) {
// if the point added last was not past the origin in the direction of d
// then the Minkowski Sum cannot possibly contain the origin since
// the last point added is on the edge of the Minkowski Difference
return false;
} else {
// otherwise we need to determine if the origin is in
// the current simplex
if (Simplex.contains(ORIGIN)) {
// if it does then we know there is a collision
return true;
} else {
// otherwise we cannot be certain so find the edge who is
// closest to the origin and use its normal (in the direction
// of the origin) as the new d and continue the loop
d = getDirection(Simplex);
}
}
}

下面我們演示一下這個算法框架在圖1的例子中如何工作。我們假設最初的方向是2個物體中心的連線的方向。

d = c2 - c1 = (9, 5) - (7.5, 6) = (1.5, -1);
p1 = support(A, B, d) = (9, 9) - (5, 7) = (4, 2);
Simplex.add(p1);
d.negate() = (-1.5, 1);

下面開始循環:
第一次迭代

last = support(A, B, d) = (4, 11) - (10, 2) = (-6, 9);
//we past the origin so check if we contain the origin
// we dont because we are line // get the new direction by (AB x AO) x AB = B(A.dot(C)) - A(B.dot(C))
AB = (-6, 9) - (4, 2) = (-10, 7);
AO = (0, 0) - (-6, 9) = (6, -9);
ABxAOxAB = AO(149) - AB(-123)
= (894, -1341) - (1230, -861)
= (-336, -480)
= (-0.573, -0.819)

第一次迭代的結果，這時，我們在明可夫斯基差中有一個線段的單純形（棕色），以及下一次使用的方向（藍色），這個方向過垂直於上次增加的兩個頂點形成的線段(藍色垂直於棕色)。注意，這個方向不需要歸一化，這兒歸一化主要是驗證給定方向的縮放是否有效。

第二次迭代

last = support(A, B, d) = (4, 5) - (12, 7) = (-8, -2)
proj = (-8, -2).dot(-336, -480) = 2688 + 960 = 3648
//we past the origin so check if we contain the origin
//we dont (see Figure 6a)
// the new direction will be the perp of (4, 2) and (-8, -2)
// and the point (-6, 9) can be removed[把離原點較遠的點移去]
AB = (-8, -2) - (4, 2) = (-12, -4);
AO = (0, 0) - (-8, -2) = (8, 2);
ABxAOxAB = AO(160) - AB(-104)
= (1280, 320) - (1248, 416)
= (32, -96)
= (0.316, -0.948)

第二次迭代後，單純形仍沒有包含原點，所以我們不能推斷出兩個物體是否相交。在第二次迭代中，我們移去了 (-6, 9)點，因爲我們任何時刻只需要3個點，我們在迭代開始後，會增加一個新的點。

第三次迭代

last = support(A, B, d) = (4, 5) - (5, 7) = (-1, -2)
proj = (-1, -2).dot(32, -96) = -32 + 192 = 160
// we past the origin so check if we contain the origin
// we do (Figure 7)!

檢測單純形

在上面的算法中，我們通過圖和僞代碼的形式進行了兩個操作：一個是怎麼知道現在的單純形是否包含原點；另一個是我們怎麼選擇下一次迭代的方向。在前面的僞代碼中，爲了便於理解，我把這兩個步驟分開，但實際上他們應該放在一起，應爲它們有很多共用的東西。

通過一系列基於點積操作的面測試（在二維是線測試），我們能夠判定原點位於單純形的什麼位置。首先我們必須處理線段測試，看前面第一次迭代的例子，增加第二個點後(代碼第9行)，單純形現在是一條線段（AB）。我們能夠通過Voronoi 區域判定單純形是否包含原點(看圖8).

線段的兩個端點是A和B，A是增加到單純形的最後一個頂點。我們知道A和B在明可夫斯基差的邊上，因此原點不能位於R1和R4區域，這是因爲11行的代碼沒有返回false，即AB和AO的點積大於0，所以原點位於R2或者R3區域。當單純形(這兒是線段)沒有包括原點的時候，我們就選擇一個新的方向，準備下一次迭代。這可以通過下面的代碼完成：

// the perp of AB in the direction of O can be found by
AB = B - A;
AO = O - A;
perp = AB x AO x AB;

當原點位於線段上的時候，我們將得到零向量，在11行的代碼將會返回false，如果我們要考慮接觸碰撞（兩個物體正好接觸上），這時就要做一些特殊處理，可以考慮用AB的左手或右手法向作爲新的方向。【如果爲0，而且原點在AB之間，就可返回true，直接判定爲接觸碰撞】
第二次迭代中個，我們得到一個三角形單純形(ABC)（圖9）

圖中白色的區域不會被測試，因爲通過了11行代碼的測試[否則會返回false]，顯然原點不會位於該區域。R2區域也不可能包含原點，因爲上一個方向是在相反的方向，所以我們需要測試的是R3,R4,R5區域，我們能夠執行AC x AB x AB 得到一個垂直於AB的向量，接着執行 ABPerp.dot(AO) 去判定是否原點在R4區域（小於0的話不在R4）。

AB = (-6, 9) - (-8, -2) = (2, 11)
AC = (4, 2) - (-8, -2) = (12, 4)
// AC x AB x AB = AB(AC.dot(AB)) - AC(AB.dot(AB))
ABPerp = AB(68) - AC(125)
= (136, 748) - (1500, 500)
= (-1364, 248)
= (-11, 2)
// compute AO
AO = (0, 0) - (-8, -2) = (8, 2)
ABPerp.dot(AO) = -11 * 8 + 2 * 2 = -84
// its negative so the origin does not lie in R4

通過更多的測試，我們能夠判定原點的位置:

AB = (-6, 9) - (-8, -2) = (2, 11)
AC = (4, 2) - (-8, -2) = (12, 4)
// AB x AC x AC = AC(AB.dot(AC)) - AB(AC.dot(AC))
ACPerp = AC(68) - AB(160)
= (816, 272) - (320, 1760)
= (496, -1488)
= (1, -3)
// compute AO
AO = (0, 0) - (-8, -2) = (8, 2)
ACPerp.dot(AO) = 1 * 8 + -3 * 2 = 2
// its positive so that means the origin lies in R3正值表示在R3,負的表示在R5

所以我們能夠判定原點在R3區域。最後，我們還要選擇一個方向，以便得到在此方向上的下一個明可夫斯基點。由於已經知道AC的Voronoi區域包含原點，所以這是很容易實現的：
AC x AO x AC
這時，已經不需要點B,所以我們去掉它。最終代碼如下所示：

Vector d = // choose a search direction
// get the first Minkowski Difference point
Simplex.add(support(A, B, d));
// negate d for the next point
d.negate();
// start looping
while (true) {
// add a new point to the simplex because we haven't terminated yet
Simplex.add(support(A, B, d));
// make sure that the last point we added actually passed the origin
if (Simplex.getLast().dot(d) <= 0) {
// if the point added last was not past the origin in the direction of d
// then the Minkowski Sum cannot possibly contain the origin since
// the last point added is on the edge of the Minkowski Difference
return false;
} else {
// otherwise we need to determine if the origin is in
// the current simplex
if (containsOrigin(Simplex, d) {
// if it does then we know there is a collision
return true;
}
}
}
public boolean containsOrigin(Simplex s, Vector d) {
// get the last point added to the simplex
a = Simplex.getLast();
// compute AO (same thing as -A)
ao = a.negate();
if (Simplex.points.size() == 3) {
// then its the triangle case
// get b and c
b = Simplex.getB();
c = Simplex.getC();
// compute the edges
ab = b - a;
ac = c - a;
// compute the normals
abPerp = tripleProduct(ac, ab, ab);
acPerp = tripleProduct(ab, ac, ac);
// is the origin in R4
if (abPerp.dot(ao) > 0) {
// remove point c
Simplex.remove(c);
// set the new direction to abPerp
d.set(abPerp);
} else {
// is the origin in R3
if (acPerp.dot(ao) > 0) {
// remove point b
Simplex.remove(b);
// set the new direction to acPerp
d.set(acPerp);
} else{
// otherwise we know its in R5 so we can return true
return true;
}
}
} else {
// then its the line segment case
b = Simplex.getB();
// compute AB
ab = b - a;
// get the perp to AB in the direction of the origin
abPerp = tripleProduct(ab, ao, ab);
// set the direction to abPerp
d.set(abPerp);
}
return false;
}

上面是二維凸多邊形碰撞檢測的代碼。在判斷原點是否包含在多面體中(兩個物體的明可夫斯基差）時，我們使用了在基於三角形的單純形測試法。這是根據Caratheodory定理：一個凸多面體的中任意一個點，能夠被表示爲其n+1點的組合。凸多面體是2維的，所以測試時用了三角形(3個點)，在3D的情況下，我們則需要測試四面體就ok了(4個點)。
現在已經完成了GJK碰撞檢測算法教程。最初的GJK算法是計算兩個凸體之間的距離。另外，如果你需要碰撞信息，比如法向和深度，你應該自己修改GJK算法或者把它和別的算法結合起來。EPA就是一個這樣的算法。

引擎開發_ 碰撞檢測_GJK 算法詳細介紹

引擎開發_ 碰撞檢測_GJK 算法詳細介紹

c++ const 指針的情況

有向圖判斷是否有環

單向鏈表的插入排序和歸併排序

3D桌球，cocos2d-x lua 打造

Mac下配置sublime實現LaTeX

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結