GJK碰撞检测算法的另一种实现

phenomLi commented 5 years ago

最近在看box2d的源码，看得好累。发现box2d的碰撞检测不止用到了SAT（分离轴）算法，还有GJK算法和V-clip算法（连google都找不到的冷门算法，不知道具体原理）。box2d貌似把3种算法糅合起来了，根本看不懂。

不过看到GJK时，我上网了解了一下，感觉眼前一亮，因为看到一种跟分离轴思路完全不同的碰撞检测算法还是很开心的。和SAT一样，GJK算法也只对凸多边形有效。GJK算法的最初目的是计算两个凸体之间的距离，但是后来却广泛应用于碰撞检测。

GJK基本思想

GJK原理是：如果两个凸图形的闵可夫斯基差包含原点, 那么这两个图形重叠，即问题转变成判断一个 闵可夫斯基差（Minkowski Difference） 图形是否包含原点。其中用到了闵可夫斯基差，我们可以先看看什么是闵可夫斯基和：

假设有两个多边形A，B，那么闵可夫斯基和用公式表示就是： A + B = {a + b|a∈A, b∈B}

用通俗的话说就是假如多边形A具有顶点{a, b, c}，多边形B具有顶点{d, e, f, g}，那么A和B的闵可夫斯基和就是： {a + d, a + e, a + f, a + g, b + d, b + e, b + f, b + g, c + d, c + e, c + f, c + g}
而对于求闵可夫斯基差，只要把闵可夫斯基和的加号变成减号就行。

一些思考

GJK的原理个人感觉比SAT简单，但是代码实现有一定难度。上网查了一下GJK的实现方法，清一色都是用单纯形逼近原点，不直接算出闵可夫斯基差，这种方法难度较大，而且比较晦涩难懂（其实是我菜）。于是我就认真思考了一下（其实很久）：

其实按照GJK的定义，直接算出闵可夫斯基差形成的凸多边形，然后判定是否包含原点不就行了？先不考虑性能，理论上是可行，但是一个问题是求闵可夫斯基差得到的是点集而不是多边形，所以还需要一种求离散点最小外接多边形的算法，再用一个算法判断点是否在多边形内。

求离散点最小外接多边形的算法其实我曾经做毕设时知道一种，名字叫边界查找算法，现在拿出来复习一下：

找到离散点中，保证y坐标最大的情况下，x坐标最小的点，记做A点以A点为原点，x轴正反向射线顺时针扫描，找到旋转角最小时扫描到的点，记做B点。
以B点为原点，AB方向射线顺时针扫描，找到旋转角最小时扫描到的点，记做C点。
以C点为原点，BC方向射线顺时针扫描，找到旋转角最小时扫描到的点，记做D点。
以此类推，直到找到起始点A。

如图示：

于是手起刀落敲了一个demo：

完成之后，喝了杯茶，发现有点不对：我的目的是要判断原点是否在点集形成的最小外接凸多边形内，但是其实按照边界查找算法的思路，我可以不必真的把外接多边形找出来再判断点是否在多边形内， 我只要在构造外接多边形的过程中，查看原点是否会成为外接多边形的其中一个顶点便可！ 为什么呢？如果原点不在点集构成的外接多边形内，在构造时为了包住每一个点，原点一定会被连接。

代码实现

因为我已经有了demo的完整的求外接多边形代码，所以我只要在demo的代码上稍作修改即可：

type polygonVex = Array<number[]>;

/**
 * GJK主类
 */
class GJK {

    /**
     * 计算闵可夫斯基差点集
     * @param vexs1 多边形顶点1
     * @param vexs2 多边形顶点2
     */
    minkowskiDifference(vexs1: Array<number[]>, vexs2: Array<number[]>): Array<number[]> {
        let md = [];

        // 顶点相加
        vexs1.map(v1 => vexs2.map(v2 => {
            md.push(Vector.sub(v1, v2));
        }));

        return md;
    }

    /**
     * 查找起始点（保证y最大的情况下、尽量使x最小的点）
     * @param points 点集
     */
    findStartPoint(points: Array<number[]>): vector {
        let sp = points[0];

        // 找到最靠上靠左的点 
        points.map(p => {
            if (p[1] < sp[1] || (p[1] == sp[1] && p[0] < sp[0])) { 
                sp = p;
            }
        });

        return sp;
    }

    /**
     * 检测碰撞
     * @param vexs1 多边形顶点1
     * @param vexs2 多边形顶点2
     */
    public detection(vexs1: polygonVex, vexs2: polygonVex): boolean {
            // 记录哪些点已经被加入到外接多边形，已加入的点的对应下标为true
        let foundList: boolean[],
            // 闵可夫斯基差点集
            md = this.minkowskiDifference(vexs1, vexs2),
            // 开始点
            startPoint = this.findStartPoint(md),
            // 当前计算出的点
            curPoint = startPoint,
            // 上一次被选的点
            lastPoint = startPoint,
            // 当前夹角余弦值
            curAngle = 0,
            // 最小夹角余弦值
            minAngle = -1,
            // 当前点的下标
            index = -1,
            // 上两点的方向射线
            lastDir = [1, 0],
            // 原点
            origin = [0, 0];

        // 把原点也加入到点集里
        md.push(origin);

        // 外部循环
        do {

            // 内部循环：遍历所有点集
            for(let i = 0, len = md.length; i < len; i++) {
                // 若当前点已经被选，则跳过
                if(foundList[i]) {
                    continue;
                }

                // 当前点与上一个被选的点的方向向量
                let v = Vector.sub(md[i], lastPoint);

                // 计算当前点与上两点射线的夹角的余弦值
                curAngle = Vector.ang(v, lastDir);

                // 若当前余弦值比最小的余弦值要大，即当前夹角比最小夹角要小（cos为减函数）
                if(curAngle > minAngle) {
                    // 更新最小夹角余弦值
                    minAngle = curAngle;
                    // 记录当前下标
                    index = i;
                }
            }

            // 若当前选中的点是原点，则表明原点不在最小凸外接多边形，判定两多边形形不相交
            if(Vector.eql(origin, md[index])) {
                return false;
            }
            else {
                // 若当前点没有被选择过
                if(!foundList[index]) {
                    // 设置当前点为已选择
                    foundList[index] = true;
                    curPoint = md[index];
                    // 更新上两点射线方向
                    lastDir = Vector.sub(curPoint, lastPoint);
                    // 更新上一次选择点
                    lastPoint = curPoint;
                    // reset最小夹角余弦值
                    minAngle = -1;
                }
            }

        // 循环至开始点退出
        } while(!Vector.eql(curPoint, startPoint));

        // 判定相交
        return true;
    }
}

最后

注意，这个算法不是GJK算法的一种“改良”，只是提供了另一种实现的思路而已，因为我也不敢说我这个比原版的要快要好。最好的情况下，也就是第一个算出的点就是原点，此时复杂度为O(n)，不过这种情况几率不大；而最坏情况下，也就是不发生碰撞，此时完整得构造出了一个外接多边形，复杂度为O(n^2)，所以平均下来就是O(n^2)，应该比不过原版，但是感觉会比SAT好一些。GJK这个算法我暂时还没有办法应用在物理引擎中，因为还不知道如何利用它计算碰撞法线和相交深度之类的东西。当然这无所谓，反正只是学习学习嘛。

最后，上面提到的判断一个点是否在多边形内，可以了解下射线法。很巧妙很神奇的一种算法。

Hell0057 commented 3 years ago

GJK原理： http://www.dyn4j.org/2010/04/gjk-gilbert-johnson-keerthi/ 碰撞法线和相交深度：EPA算法 http://www.dyn4j.org/2010/05/epa-expanding-polytope-algorithm/

phenomLi commented 3 years ago

GJK原理： http://www.dyn4j.org/2010/04/gjk-gilbert-johnson-keerthi/ 碰撞法线和相交深度：EPA算法 http://www.dyn4j.org/2010/05/epa-expanding-polytope-algorithm/

当时写这篇文章的时候还没有完全理解GJK的思想，感谢指正

phenomLi / Blog