(伪)EPA(Expanding Polytope Algorithm) 求重叠的凸图形的嵌入方向与深度

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

接前篇, 用GJK可以判断两个凸图形是否重叠, 而EPA可以求重叠其嵌入的深度的与方向.

两个凸图形的闵可夫斯基差如果包含原点, 那么两个凸图形重叠. 而闵可夫斯基差上的一条边到原点的距离就是两个图形的最小嵌入深度, 这条边的垂线方向就是两个图形的最小嵌入方向.

用GJK得到包含原点的单纯形后, EPA对这个单纯形作扩展, 使得扩展后的单纯形包含闵可夫斯基差上距原点距离最小的边.

不过要求嵌入深度和方向只需要求得那一条边就足够了, 并不需要保留整个扩展单纯形. 所以我改一下变成这样:

如图, 假设椭圆形是闵可夫斯基差的形状, ABC是GJK结束时所得到的单纯形, 点O是原点.

首先求得AC和BC上距离原点最近的点da和db, 用da和db的模长判断AC和BC到原点的距离.

如果AC离原点比BC更近, 那么抛弃点B, 用点A和C当作新的单纯形顶点A‘和B‘, 用向量da的方向当作新的搜索方向.

否则, 抛弃点A(如下图所示), 用点B和点C当作新的单纯形顶点A‘和B‘, 用向量db当作新的搜索方向.

之后, 用和GJK里相同的getSupportPoint函数找到新的C‘.

现在, 把点A(或点B也可)以及点C投影到搜索方向上, 求其差值det(如图中蓝色虚线所示).

如果det足够小, 那么说明已经找到一条闵可夫斯基差上尽可能接近原点的边了, 此时返回AB上距原点最近的点向量vec, vec的方向就是嵌入方向, vec的模长就是最小嵌入距离.

如果det太大, 就重复以上过程.

proc getPenetrationVector*(s1, s2: Sprite2D, sA, sB, sC: Vector2D, tolerance: float32 = 0.2): Vector2D =
    var
        a = sA
        b = sB
        c = sC
        dir: Vector2D
    while true:
        let
            da = getClosestPointToOrigin(a, c)
            db = getClosestPointToOrigin(b, c)
        if da.sqLen > db.sqLen:
            a = b
            b = c
            dir = db.norm()
        else:
            b = c
            dir = da.norm()
        c = getSupportPoint(s1, s2, dir)
        if (c - a) * dir <= tolerance:
            return getClosestPointToOrigin(a, b)

求线段上到原点距离最近的点的方法是把原点投影到该线段上.

proc getClosestPointToOrigin(a, b: Vector2D): Vector2D =
    let
        ab = norm(b - a)
        ao = a.negate()
    return a + ab * (ab * ao)

最后效果如图, 白色的线表示把多边形推离椭圆形需要的长度最小的向量