2D GJK算法判断图形重叠出现的莫名其妙的bug?

Question

不知道这种问题拿出来问会不会不合适, 不过改了一整天都没解决... 不知道怎么描述清楚所以写得啰嗦点...

大致上是按照这个写的: http://www.dyn4j.org/2010/04/gjk-distance-closest-points/
可是即使把别人的代码抄下来也是各种bug

算法的大致描述和自己的思路是:
如果两个凸图形的闵可夫斯基差包含原点, 那么这两个图形相交
闵可夫斯基和就是把两个图形里的点都当作向量, 然后把两个图形的向量都分别相加得到一个新的图形. 闵可夫斯基差就是其中一个图形的向量都取反, 相当于绕原点反转, 然后再求闵可夫斯基和
GJK不直接求闵可夫斯基差, 而是借助下面这个函数, 每次输入一个查找方向, 然后返回闵可夫斯基差图形上距查找方向最远的一个点, 然后通过迭代生成一个个子图形, 判断子图形是否包含原点

proc getSupportPoint(s1: Sprite2D, s2: Sprite2D, dir: Vector2D): Vector2D =
    let
        invM1 = s1.invMatrix
        invM2 = s2.invMatrix
        v1 = s1.matrix * s1.graph.getFarthestPointInDirection(invM1 * dir - invM1 * Vector2D(x: 0, y: 0))
        v2 = s2.matrix * s2.graph.getFarthestPointInDirection(invM2 * dir.negate() - invM2 * Vector2D(x: 0, y: 0))
    #把查找方向变换到物体空间, 找到最远点后再变换回世界空间. 变换矩阵都只是X/Y轴上的尺寸拉伸+旋转+平移, 向量是列向量
    return v1 - v2

↓在凸多边形上找在查找方向上最远的一个点, 我的多边形顶点全是顺时针排序

method getFarthestPointInDirection*(self: Polygon, direction: Vector2D): Vector2D =
    var
        curr = self[0]
        bestProjection: float32 = curr * direction #找在dir上投影最大的那个顶点
        projection: float32

    result = curr

    for i in 1..self.len-1:
        curr = self[i]
        projection = curr * direction

        if projection > bestProjection:
            bestProjection = projection
            result = curr

↓圆形的:

method getFarthestPointInDirection*(self: Circle, direction: Vector2D): Vector2D =
    direction.norm() * self.radius #norm是求单位化向量

维基上给的GJK伪代码是:

   function GJK_intersection(shape p, shape q, vector initial_axis):
       vector  A = Support(p, initial_axis) - Support(q, -initial_axis)
       simplex s = {A}
       vector  D = -A
       loop:
           A = Support(p, D) - Support(q, -D)
           if dot(A, D) < 0:
              reject
           s = s ∪ A
           s, D, contains_origin = NearestSimplex(s)
           if contains_origin:
              accept

我的具体实现是:
假设下面的图的椭圆形是闵可夫斯基差, 小x是原点可能存在的地方, O是原点, 左边是初始状态, 右边是迭代过一次之后的状态
用(1, 0)当作初始方向找到闵可夫斯基差上的第一个顶点A, 把方向取反找到第二个顶点B
然后连线AB, 取AB的指向原点方向的垂线作为查找方向找到第三个顶点C
如果原点在ABC内, 那么判断为重叠
如果原点在绿色线的外面, 那么原点一定不在闵可夫斯基差内, 判断为不重叠
如果都不是, 就抛弃离原点最远的那个点, 用剩下的两个点作为AB, 然后继续查找C

proc isIntersect*(s1, s2: Sprite2D): bool =
    var
        dir = Vector2D(x: 1, y: 0)
        simplexA, simplexB, simplexC: Vector2D
        #i: int

    simplexA = getSupportPoint(s1, s2, dir)
    if simplexA * dir < 0: return false #把OA投影到方向向量上, 如果为负则原点在绿色线外, 原点一定不在闵可夫斯基差内

    dir = dir.negate()
    simplexB = getSupportPoint(s1, s2, dir)
    if simplexB * dir < 0: return false

    let
        ao = simplexA.negate()
        ab = simplexB - simplexA
    dir = tripleProduct(ab, ao, ab) #连续两次叉乘得到一个垂直于AB的贴近AO方向的向量

    while true:
        #i += 1
        #if i > 10000: assert(false)

        simplexC = getSupportPoint(s1, s2, dir)
        if simplexC * dir < 0:
            return false

        let
            ba = simplexA - simplexB
            ac = simplexC - simplexA
            bc = simplexC - simplexB
            acPerp = tripleProduct(ac, ba.negate(), ac)
            bcPerp = tripleProduct(bc, ba, bc)

        if acPerp * simplexA > 0: #把acPerp投影在ao上, 如果为负那么原点就在AC的外面
            simplexB = simplexC #此时点B离原点最远, 用点A和点C作为新的A,B
            dir = acPerp.negate()
        elif bcPerp * simplexB > 0:
            simplexA = simplexC
            dir = bcPerp.negate()
        else: return true #原点在AC和BC内, 所以原点在闵可夫斯基差内, 判断为重叠

自己觉得应该没什么问题, 但实际运行起来就会像开头的两张图那样, 经常出现"超界"的情况, 一开始以为是某些向量算出来太接近0, 但是一个个判断作特殊处理后还是完全没改善