EPA, 求嵌入深度和嵌入方向 (fix)

参考: http://www.dyn4j.org/2010/05/epa-expanding-polytope-algorithm/

一个在线演示: http://sandbox.runjs.cn/show/xseojpfa

GJK可以判断两个凸图形是否重叠, EPA可以基于GJK的工作找出分离两个图形的最小向量.

如果原点在闵可夫斯基差内部, 那么两个图形重叠. 而闵可夫斯基差边界上离原点最近的点的向量就是这个最小分离向量.

EPA把GJK结束时的simplex进行扩展, 从闵可夫斯基差的边界上用和GJK相同的getSupportPoint函数选取顶点, 直到扩展后的多边形包含了闵可夫斯基差上离原点最近的那条边.

getPenetration: function (tolerance) {
    var epa = new Contact.EPA(
        this._simplexA,
        this._simplexB,
        this._simplexC,
        this,
        tolerance
    );
    return this.penetration = epa.solve();
}

getClosestPointToOrigin: function (a, b) {
    var ab = b.sub(a).norm(),
        ao = a.negate();
    return a.add(ab.mul(ab.dot(ao))); //把原点投影到ab上
}

getPenetration: function (tolerance) {
    var epa = new Contact.EPA(
        this._simplexA,
        this._simplexB,
        this._simplexC,
        this,
        tolerance
    );
    return this.penetration = epa.solve();
}

getClosestPointToOrigin: function (a, b) {
    var ab = b.sub(a).norm(),
        ao = a.negate();
    return a.add(ab.mul(ab.dot(ao)));
}

EPA: pngx.Class(
    null,
    function (simplexA, simplexB, simplexC, contact, tolerance) {
        this.vertices = [simplexA, simplexB, simplexC];
        this.contact = contact;
        this.tolerance = tolerance || 0.05;
    },
    {
        vertices: null,
        contact: null,
        tolerance: null,
        closestIndex: null,
        closestNormal: null,
        closestDistance: null,
        penetration: null,
        findClosestEdge: function () { //找出当前扩展多边形内离原点最近的一条边
            var bestDistance = Infinity,
                bestNormal,
                bestIndex,
                vertices = this.vertices,
                len = vertices.length;
            for (var i = 0; i < len; ++i) {
                var v0 = vertices[i], //多边形的一个顶点, 同时也是从原点指向多边形外部的一个向量
                    v1 = vertices[(i + 1) % len],
                    v01 = v1.sub(v0),
                    normal = pngx.Vector2D.tripleProduct(v01, v0, v01).norm(),//取指向多边形外部的, 垂直于V0-V1的边的向量
                    distance = v0.dot(normal); //从原点到当前边的距离
                if (distance < bestDistance) {
                    bestDistance = distance;
                    bestNormal = normal;
                    bestIndex = i;
                }
            }
            this.closestIndex = bestIndex;
            this.closestNormal = bestNormal;
            this.closestDistance = bestDistance;
        },
        insert: function (index, point) {
            var vertices = this.vertices,
                len = vertices.length;
            vertices.splice((index + 1) % len, 0, point);
        },
        solve: function () {
            var contact = this.contact,
                tolerance = this.tolerance;
            for (; ;) { //多数情况迭代两三次就能完成
                this.findClosestEdge(); //取扩展多边形中离原点最近的那条边的法向量为搜索方向, 在这个方向上最有可能找出最接近原点的闵可夫斯基差上的边
                var normal = this.closestNormal,
                    point = contact.getSupportPoint(normal),
                    distance = point.dot(normal); //新找出的顶点到原点的距离
                if (distance - this.closestDistance <= tolerance) { //新找出点和上一次找出的点非常接近, point已经足够接近闵可夫斯基差边界上离原点最近的那个点了
                    return this.penetration = normal.mul(distance);
                }

                this.insert(this.closestIndex, point);
            }
        }
    }
)