GJK-2D

GJK算法是由Elmer G. Gilbert, Daniel W. Johnson, and S. Sathiya Keerthi 在1988年提出的一种确定两个凸集的最短距离算法。

当该算法应用在碰撞检测中时，其思想是在几何体A和几何体B的闵可夫斯基差空间内迭代构建一个单纯形，尽可能地使其包含原点。若其包含原点，则闵可夫斯基差空间包含原点，两个几何体相交。

概念

凸（Convex）

几何体的任意两点连线都包含在几何体内，称为凸几何体。

闵可夫斯基差（Minkowski difference）

几何体 $A$ 和几何体 $B$ 作差，所形成的空间称为闵可夫斯基差空间：

A - B = \{a - b | a \in A, b \in B\}

有推论：若两个几何体相交，当且仅当其闵可夫斯基差空间包含原点。

几何体A与B不相交，其闵可夫斯基差不包含原点。

几何体A与B相交，其闵可夫斯基差包含原点。

单纯形（simplex）

n-单纯形是n维欧几里得空间中的n+1个仿射无关的点的集合的凸包。如0-单纯形是点，1-单纯形是线段，2-单纯形是三角形，3-单纯形是四面体。

支撑函数（support function）

s_X(\vec{d}) = \argmax_{x}\{x \cdot \vec{d} | x \in X \}

即几何体 $X$ 在方向 $\vec{d}$ 上最远的点，称为支撑点。

有性质：

s_{A-B}(\vec{d}) = s_A(\vec{d})-s_B(-\vec{d})

即用几何体A在方向 $\vec{d}$ 上最远的点减去几何体B在方向 $-\vec{d}$ 上最远的点，所得的点就是其闵可夫斯基差在方向 $\vec{d}$ 上最远的点。

算法

初始化与主循环

1.首先选择初始方向 $dir$ ，可以是随机的或者两个几何体的中心差

2.计算闵可夫斯基差支撑点 $A$

3.将新支撑点并入单纯形

4.更新搜索方向dir为 $OA$

5.计算新的闵可夫斯基差支撑点 $A$

6.判定新计算出来的支撑点在搜索方向上的在投影长度，若小于0，即 $OA\cdot \vec{dir} < 0$ ，则说明已无法找到能跨越原点的点，两个几何体不相交

7.将新支撑点并入单纯形

8.判定单纯形是否包含原点，并更新单纯形和搜索方向

9.若包含原点，则两个何体相交；否则跳到第5步


function GJK_intersection(shape p, shape q, vector initial_axis):
    vector A := Support(p, initial_axis) − Support(q, −initial_axis)
    simplex s := {A}
    vector dir := −A

    loop:
        A := Support(p, dir) − Support(q, −dir)
        if dot(A, dir) < 0:
            reject
        s := s ∪ A
        s, dir, contains_origin := SimplexOrigin(s, dir)
        if contains_origin:
            accept

支撑点的计算

遍历几何体的顶点，计算其在搜索方向上的投影，即 $v\cdot \vec{dir}$ ，最大的就是支撑点

function Support(shape p, vector dir):
    maxLen = -FLT_MAX
    for vertex in p:
        length = dot(vertex, dir)
        if(length > maxLen):
            maxLen = length
            support = vertex
    return support

单纯形原点包含的判定和更新及搜索方向的更新

分为2个点的线段和3个点的三角形两种情况处理

function SimplexOrigin(simplex s, vector dir):
    switch (s.size())
    {
        case 2: // line segment
            contain_origin = Simplex2(s, d)
        case 3: // triangle
            contain_origin = Simplex3(s, d)
    }
    return contain_origin

线段

前面由 $OA\cdot \vec{d} < 0$ 判定过，原点不可能在 $R4$ 区域。

A是新的支撑点，即搜索方向为 $B$ 指向 $A$ ，所以原点不可能在 $R1$ 区域。

那么若原点在线段上，则有 $AB \times AO \times AB = \vec{0}$ 。闵可夫斯基差包含原点，几何体相交，结束。

否则， $AB \times AO \times AB$ 的方向为垂直于 $AB$ 指向原点，作为下一次搜索方向。

funtion Simplex2(simplex s, vector dir):
    vector A = s[1]
    vector B = s[0]
    vector AB = B - A
    vector AO = - A
    vector ABOB = TripleProduct(AB, AO, AB) // norm to AB toward origin

    if(ABOB.norm() <= EPSILON) // origin lies on
        return true
    else
        dir = ABOB
        return false

三角形

前面由 $OA\cdot \vec{d} < 0$ 判定过，所以原点不可能在 $R1$ （白色）区域。

$A$ 点为最新加入的点，即上次搜索方向为远离 $BC$ 指向 $A$ 的方向，所以原点不可能在 $R4$ 区域。

若原点在 $R2$ 区域，当且仅当 $AC \times AB \times AB \cdot AO > 0$ 。 $AC \times AB \times AB$ 的方向为垂直于 $AB$ 指向原点，作为下一次搜索方向。

若原点在 $R3$ 区域，当且仅当 $AB \times AC \times AC \cdot AO > 0$ 。 $AB \times AC \times AC$ 的方向为垂直于 $AC$ 指向原点，作为下一次搜索方向。

否则，原点在 $R5$ 区域。闵可夫斯基差包含原点，几何体相交，结束。

function Simplex3(simplex s, vector d):
    vector A = simplex[2]
    vector B = simplex[1]
    vector C = simplex[0]
    vector AB = B - A
    vector AC = C - A
    vector AO = -A
    vector ACBB = TripleProduct(AC, AB, AB) // norm to AB toward far from C
    vector ABCC = TripleProduct(AB, AC, AC) // norm to AC toward far from B

    if (ACBB.dot(AO) > 0): // R2 region
        s.delete(C)
        dir = ACBB
    else:
        if (ABCC.dot(AO) > 0): // R3 region
            s.delete(B)
            dir = ABCC
        else: // R5 region
            return True
    return False

程序

python版 GJK.py

"""
This module provides GJK algorithm
"""

import numpy as np
EPS = 1e-6
  
def TripleProduct(a, b, c):
    return b*(np.dot(c,a))-a*(np.dot(c,b))

def SupportFunction(points, dir):
    """
    Get support point.

    :param points: list of points
    :param dir: direction
    :return: support point
    """

    longest = sum(points[0]*dir)
    point = points[0]
    for index in range(len(points)):
        lenth = 0
        for ind in range(len(dir)):
            lenth += points[index][ind] * dir[ind]
        if(lenth > longest):
            point = points[index]
            longest = lenth
    return point

def MinkowskiDifference(points1, points2, dir):
    """
    Get Minkowski Difference.

    :param points1: list of points of object 1
    :param points2: list of points of object 2
    :param dir: direction
    :return: list of points of Minkowski Difference
    """

    spa = SupportFunction(points1, dir)
    spb = SupportFunction(points2, -dir)
    spmd = spa - spb
    
    return spmd

def TestSimplex2ContainOrigin(simplex, dir):
    """
    Test if simplex contains origin.

    :param simplex: list of points of simplex
    :param dir: direction
    :return: True if contains origin, False if not
    """

    A = simplex[1]
    B = simplex[0]
    AB = B - A
    AO = - A
    ABOB = TripleProduct(AB, AO, AB) # norm to AB toward origin

    dir[:] = ABOB
    
    return False

def TestSimplex3ContainOrigin(simplex, dir):
    """
    Test if simplex contains origin.

    :param simplex: list of points of simplex
    :param dir: direction
    :return: True if contains origin, False if not
    """

    A = simplex[2]
    B = simplex[1]
    C = simplex[0]
    AB = B - A
    AC = C - A
    AO = -A
    ACBB = TripleProduct(AC, AB, AB) # norm to AB toward far from C
    ABCC = TripleProduct(AB, AC, AC) # norm to AC toward far from B

    if (ACBB.dot(AO) > 0):
        simplex[0] = B
        simplex[1] = A
        simplex.pop()
        dir[:] = ACBB
    else:
        if (ABCC.dot(AO) > 0):
            simplex[0] = C
            simplex[1] = A
            simplex.pop()
            dir[:] = ABCC
        else:
            return True
    return False

def TestSimplexContainOrigin(simplex, dir):
    """
    Test if simplex contains origin.
    """

    isContainOrigin = False
    match len(simplex):
        case 2: # line segment
            isContainOrigin = TestSimplex2ContainOrigin(simplex, dir)
        case 3: # triangle
            isContainOrigin = TestSimplex3ContainOrigin(simplex, dir)
    return isContainOrigin

def GJK(points1, points2):
    """
    GJK algorithm.

    :param points1: list of points of object 1
    :param points2: list of points of object 2
    :return: True if collision, False if not
    """

    # use center difference as initial direction
    center1 = sum(points1)/len(points1)
    center2 = sum(points2)/len(points2)
    dir = center1 - center2
    if(np.linalg.norm(dir) < EPS):
        dir = np.array(1.0, 0.0, 0.0)
    dir /= np.linalg.norm(dir)

    spmd = MinkowskiDifference(points1, points2, dir)

    simplex = [spmd]
    dir = -dir

    while True:
        spmd = MinkowskiDifference(points1, points2, dir)
        if(np.dot(spmd, dir) < 0):
            return False
    
    simplex.append(spmd)

    if(TestSimplexContainOrigin(simplex, dir)):
        return True

测试

相交

import numpy as np
import GJK
A = np.asarray([[4, 5], [9, 9], [4, 11]])
B = np.asarray([[5, 7], [7, 3], [10, 2], [12, 7]])

isCollide = GJK.GJK(A, B)
print(isCollide)

$A=[[4, 5], [9, 9], [4, 11]]$

$B=[[5, 7], [7, 3], [10, 2], [12, 7]]$

闵可夫斯基差

沿初始方向的第一个支撑点。

第二个支撑点。此时单纯形是一条线段。

第三个支撑点。此时单纯形是一个三角形。

判定包含原点，相交。

不相交

import numpy as np
import GJK
A = np.asarray([[4, 5], [9, 9], [4, 11]])
B = np.asarray([[5+2, 7], [7+2, 3], [10+2, 2], [12+2, 7]])

isCollide = GJK.GJK(A, B)
print(isCollide)