物理引擎学习04-GJK计算多边形之间的最近距离

计算多边形之间的最近距离，才是GJK算法原本的目的。只有两个多边形不相交，计算最近距离才有效。如果相交，则最近距离无效，但是可以使用EPA算法要计算碰撞深度。本文的写作目的，主要是对GJK算法的理解和应用。对算法本身感兴趣的朋友，可以阅读源论文的文献。本系列GJK算法文章共三篇，本篇是第二篇，强烈建议从第一篇开始看:

• GJK碰撞检测算法基础
• GJK计算多边形之间的最近距离
• GJK和EPA计算穿透向量

本文作者游蓝海。原创不易，未经许可，禁止任何形式的转载。

1. 基本原理

计算多边形之间的距离，本质是在两个多边形上找到距离最近的边，然后计算两个边之间的最近距离。计算最近距离的算法和GJK碰撞检测算法类似，同样使用闵可夫斯基差来构建单形体，使用同样的support函数。当没有发生碰撞的时候，距离原点最近的闵可夫斯基差集多边形的边，就是我们要计算最近距离的关键。为了方便表示，下文将该边称作“差集最近边”。

GJK最近距离算法的两个关键点为：

• 原点到差集最近边的距离，就是两个多边形之间的最近距离；
• 构成差集最近边的两个support点，分别来自两个多边形的最近边。因此，通过support点，可以反推出两个多边形的最近边。

需要注意的是，两个多边形最近的位置不一定是边，也可能是顶点。不过这种情况，可以认为是长度为0的特殊边。

2. 算法解析

2.1 算法伪代码

/// 按步骤分解，碰撞检测
public bool GJK(Shape shapeA, Shape shapeB)
{
    Vector2 direction = findFirstDirection();
    simplex.add(support(direction));
    simplex.add(support(-direction));

    direction = -getClosestPointToOrigin(simplex.get(0), simplex.get(1));
    while(true)
    {
        SupportPoint p = support(direction);

        // 新点与之前的点重合了。也就是沿着dir的方向，已经找不到更近的点了。
        if (Vector2.Distance(p.point, simplex.get(0)) == 0 ||
            Vector2.Distance(p.point, simplex.get(1)) == 0)
        {
            break;
        }

        simplex.add(p);
        // 单形体包含原点了
        if (simplex.contains(Vector2.zero))
        {
            return true;
        }

        direction = findNextDirection();
    }

    ComputeClosetPoint();
    return false;
}

该算法与上一章的GJK碰撞检测算法很相似，但有两个细微的差别：

1. 选择下次的迭代方向不同。上一章用的是原点到直线的垂线作为迭代方向；本章用的是原点到线段的最近向量，作为迭代方向。注意，原点到线段的最近距离不一定就是垂足，可能是线段的某个端点。
2. 不发生碰撞的结束条件不同。上一章迭代算法最终结束时，可能是一个不包含原点的三角形，为了得到差集最近边，我们可以舍弃掉距离原点较远的那个边。这里我们让算法多迭代一次，恰好可以舍弃掉那个点，同时会额外会产生一个重复的support点。因此只要发现support点位置重叠了，就表明迭代算法可以结束了。

计算步骤的分解动图:

2.2 计算多边形间的最近距离

最近距离就是原点到差集最近边的距离。先求出原点到线段的最近点，然后计算原点到最近点的距离即可。设线段为ab，原点为o，先计算出ao在ab上的投影长度:

• 如果投影小于0，说明最近点为a点；
• 如果大于ab的长度，说明最近点为b点；
• 否则，就在ab之间。

这里有个小技巧，计算投影长度，需要将被投影向量ab单位化，也就是要求ab的长度。但是为了让ao在ab上投影长度单位化到[0, 1]之间，需要额外除以ab的长度，因此计算投影的时候，直接就除以ab长度的平方了，就省去了计算长度时的开方运算。

Vector2 getClosestPointToOrigin(Vector2 a, Vector2 b)
{
    Vector2 ab = b - a;
    Vector2 ao = Vector2.zero - a;

    float sqrLength = ab.sqrMagnitude;

    // ab点重合了
    if(sqrLength < float.Epsilon)
    {
        return a;
    }

	// 计算投影长度，并单位化到[0, 1]，需要额外除以ab的长度。因此这里就直接除以长度的平方了。
    float projection = Vector2.Dot(ab, ao) / sqrLength;
    if (projection < 0)
    {
        return a;
    }
    else if (projection > 1.0f)
    {
        return b;
    }
    else
    {
        return a + ab * projection;
    }
}

2.3 计算多边形间的最近点

我们改造一下support方法，每次将生成support点用到的两个顶点也记录下来。这样通过support点，可以反向找回多边形的边。

设是差集最近边为$L=AB$，A、B是support点，构成A、B两的顶点分别自两个多边形的边$E1=Aa-Ba$、$E2=Ab-Bb$。则求两个凸包的最近距离，就演变成了求E1和E2两个边的最近距离。

设Q是原点到L的垂直向量，也就是垂足，则有:
$$\{\begin{array}{l}L=B-A\\ Q\cdot L=0\end{array}$$
因为Q是L上的点，可以用$r1、r2$来表示$Q$: $Q=A\ast r1+B\ast r2$ ，其中$r1+r2=1$ 。则有:
$$(A\ast r1+B\ast r2)\cdot L=0$$
用$r2$代替$r1$，$r1=1-r2$，则有:
$$\begin{gathered} (A-A\ast r2+B\ast r2)\cdot L=0 \\ (A+(B-A)\ast r2)\cdot L=0 \\ L\cdot A+L\cdot L\ast r2=0 \\ r2=-(L\cdot A)/(L\cdot L) \end{gathered}$$

推导过程略显复杂，但是最终的公式却很简单:
$$\{\begin{array}{l}r2=-(L\cdot A)/(L\cdot L)\\ r1=1-r2\\ Qa=Aa\ast r1+Ba\ast r2\\ Qb=Ab\ast r1+Bb\ast r2\end{array}$$
计算最近点的伪代码如下:

void ComputeClosetPoint()
{
    SupportPoint A = simplex.getSupport(0);
    SupportPoint B = simplex.getSupport(1);

    Vector2 L = B.point - A.point;
    float sqrDistanceL = L.sqrMagnitude;
    // support点重合了
    if (sqrDistanceL < 0.0001f)
    {
        closestOnA = closestOnB = A.point;
    }
    else
    {
        float r2 = -Vector2.Dot(L, A.point) / sqrDistanceL;
        r2 = Mathf.Clamp01(r2);
        float r1 = 1.0f - r2;

        closestOnA = A.fromA * r1 + B.fromA * r2;
        closestOnB = A.fromB * r1 + B.fromB * r2;
    }
}

3. 小结

GJK计算多边形之间的最近距离，本质上是使用GJK算法求得差集最近边。而构成差集最近边的两个support点，就是来自两个多边形的最近边上的4个顶点。然后就将问题转换成，计算两条边之间的最近距离和最近点。

本章Demo使用Unity3D引擎开发，Demo工程已上传github: https://github.com/youlanhai/learn-physics/tree/master/Assets/04-gjk-closest-point

4. 参考

• GJK算法论文: https://ieeexplore.ieee.org/document/2083?arnumber=2083
• GJK – Distance & Closest Points: http://www.dyn4j.org/2010/04/gjk-distance-closest-points

本系列文章会和我的个人公众号同步更新，感兴趣的朋友可以关注下我的公众号：游戏引擎学习。扫下面的二维码加关注：