Planning-碰撞检测之GJK

原文:dyn4j:GJK (Gilbert–Johnson–Keerthi)

$GJK$和$SAT$一样用于检测凸多边形，和$SAT$不同，$GJK$可以处理任意形状的凸多边形，然而$SAT$处理圆的碰撞检测时需要用不同的算法。

1. Minkowski Sum(明可夫斯基和)

$GJK$是基于$Minkowski$ $Sum$概念上的，即形状1的所有点和形状2的所有点之和。
A + B = { a + b ∣ a ∈ A , b ∈ B } A + B = \{a + b | a \in A, b \in B \} A+B={a+b∣a∈A,b∈B}
如果$shapeA$和$B$是凸的，则它们的和也是凸的
相应的可以定义它们的差运算：
A − B = { a − b ∣ a ∈ A , b ∈ B } A - B = \{a - b | a \in A, b \in B \} A−B={a−b∣a∈A,b∈B}
如果两个形状重叠，进行$Minkowski$ $Sum$后的形状包含原点。$Minkowski$$Sum$的运算是$shape$ $A$的每个顶点和$shape$ $B$的所有顶点求和（或求差）。所得到点的外包络即是运算所得形状。

2. Simplex

单纯形是代数拓扑中的基本概念，单纯形是三角形和四面体的一种泛化，一个$k$维单纯形是指包含$(k+1)$个节点的凸多面体。不需要计算$Minkowski$ $Sum$，只需要计算$Minkowski$ $Sum$是否包含原点，如果包含原点则两个形状重叠，否则不重叠。在$MinkowskiSum$得到的形状中迭代的构建$Simplex$，判断$Simplex$是否包含原点，包含则重叠，否则不重叠。

3. support函数

$support$函数返回两个形状$Minkowski$ $Sum$的一个点用以构建$Simplex$，$shape1$的点减去$shape2$的点可以得到一个$Minkowski$ $Sum$的一个点，但我们不希望得到重复的点。使用$support$函数得到$shape$在一个$direction$(方向)上的最远点，然后在不同的方向上得到另一个不同的点。在$direction$上选择最远点可以使生成的$Simplex$的面积最大化以增加包含原点的几率增加算法收敛速度。使用这种方法得到的点在$Minkowski$ $Sum$的边上，因此如果得到的点不包含原点则$Minkowski$ $Sum$不包含原点。

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

4. 构建Simplex

首先选择三个点构造$Simplex$，如果包含原点，则两个多边形重叠，否则重新选择点构建新的$Simplex$。由$support$函数可知，需要一个$direction$来选择$Minkowski$ $Sum$点，任意的$direction$都是可以的，但是选择两个多边形的中心点的向量方向是较优的选择。

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.geLast().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 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);
        }
    }
}

Simplex.geLast().dot(d) <= 0的解释：
$Simplex$最新得到的点是$Minkowski$ $Sum$在给定方向上的最远点，如果这个点没有$enclose$原点，则$Minkowski$ $Sum$不会包含原点。如下图，$direction$是$(-1,0)$，点$p(1,-4)$是在$direction$上的最远点。点$p$在$d$上的投影是-1，原点在$d$上的投影是0，由于$p$是$Minkowski$ $Sum$是最远点，所以其不会包含原点。

当得到两个$Minkowski$ $Sum$点时，如下图中左图，$B$是第一个点，$A$是第二个点，$A$和$B$是$Minkowski$ $Sum$包络上的点，如果$origin$在$R1$或者$R4$区域，则$Minkowski$ $Sum$不包含原点，Simplex.geLast().dot(d) <= 0 返回$true$。由图可知，$origin$在$R3$区域内，因此需要计算$AB$的$perpetualar$ $vector$应该指向$R3$。
$$\begin{gathered} AB=B-A \\ AO=O-A \\ perp=(AB\times AO)\times AB \end{gathered}$$
如右图，此时$support$函数得到新的$Minkowski$ $Sum$点$A$，点$B$是第二点（左图中的$A$），点$C$是第一个点（左图中的$B$），$origin$可能存在于$R3$、$R4$和$R5$中，计算$(AC\times AB)\times AB$生成$ABperp$，计算$ABperp\cdot (AO)$判断$origin$是否在$R4$，计算$(AB\times AC)\times AC$生成$ACperp$，计算$ACperp\cdot (AO)$判断$origin$是否在$R3$。

Vector d = // choose a search direction
// get the first Minkowski Sum point
Simple.add(support(A, B, d));
// nagate d for the next point
d.negate();
// start loop
while (true) {
    // add a new point to the simplex because we haven't terminated yet
    Simple.add(support(A, B d));
    // make sure that the last point added actually passed the origin
    if (Simple.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 Minkowski Difference
        return false;
    } else {
        // otherwise determine if the origin is in the current simplex
        if (containsOrigin(Simple, d)) {
            // if it does, there is a collision
            return true;
        }
    }
}

public boolean containOrign(Simples s, Vector d) {
    // get the last point added to the simplex
    a = Simplex.getLast(); // the last point
    // compute AO (same thing as -A)
    ao = a.negate();
    if (Simplex.points.size == 3) {
        // in triangle case
        // get point B and C
        b = Simplex.getB(); // the second point
        c = Simplex.getC(); // the first point
        // compute the edges
        ab = b - a;
        ac = c - a;
        // compute the normals
        abPerp = tripleProduct(ac, ab, ab);
        acPerp = tripltProdect(ab, ac, ac);
        // is the origin in R4
        if (adPerp.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
                Simples.remove(b);
                // set the new direction to ACPerp
                d.set(acPerp);
            } else {
                // otherwise origin in R5, there is a collision
                return true;
            }
        }
    } else {
        // in 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;
}