裁剪出碰撞点(manifold) (for GJK的通用改)

用EPA得到图形的嵌入方向之后, 就可以裁出碰撞点(边)了.

首先需要找到图形在嵌入方向上最远的一条边.

多边形:

method getFarthestEdgeInDirection*(self: Polygon, direction: Vector2D): Line2D =
    var
        bestIndex: int = 0
        bestProjection: float32 = self[0] * direction
        projection: float32

    #先找出在direction上最远的一个顶点
    for i in 1..self.len-1:
        projection = self[i] * direction

        if projection > bestProjection:
            bestIndex = i
            bestProjection = projection

    #选出与该顶点相邻的两条边, 取在direction上投影最小的那条返回
    let
        left = self[(bestIndex + self.len - 1) mod self.len]
        right = self[(bestIndex + 1) mod self.len]
        mid = self[bestIndex]
        leftProj = abs(left * direction)
        rightProj = abs(right * direction)

    if leftProj > rightProj:
        return newLine2D(left, mid)
    else:
        return newLine2D(mid, right)

圆形只能得到一个点, 不过为了统一还是返回一条长度是0的线段:

method getFarthestEdgeInDirection*(self: Circle, direction: Vector2D): Line2D =
    let p = direction.norm() * self.radius
    return newLine2D(p, p)

还可以为扇形等各种凸图形写一个getFarthestEdgeInDirection函数, 这样就能处理所有的凸图形. 对直线边都返回一条线段, 对弧线边都返回一个点.

于是对所有的凸图形碰撞都分为三种情况, 点与点(两个弧边相碰撞), 点与线段(一个弧形边与一条直线边相碰撞), 线段与线段(两条直线边)

proc getManifold*(self: Contact): Manifold =
    let
        edge1 = self.collisionEdge1
        edge2 = self.collisionEdge2
        isPoint1 = edge1.isPoint()
        isPoint2 = edge2.isPoint()

    if isPoint1:
        if isPoint2:
            result = getPointsManifold(edge1.a, edge2.a)
        else:
            result = getPointLineManifold(edge1.a, edge2)
    else:
        if isPoint2:
            result = getPointLineManifold(edge2.a, edge1)
        else:
            result = getLinesManifold(edge1, edge2, self.penetration)

    result.penetration = self.penetration
    result.rigidA = self.rigidA
    result.rigidB = self.rigidB

实际上的刚体碰撞不会陷入到另一个物体内部, 裁剪出来的碰撞点(边)只要在两个图形刚好接触或者小程度的重叠时看上去合理就没问题.

对于点与点的情况, 可以简单的取两点中点:

proc getPointsManifold(p1, p2: Vector2D): Manifold =
    new(result)
    let p = (p1 + p2) * 0.5
    result.add(p)

点与线段的情况:

如果弧上的点夹在线段两点之间, 那么说明是圆形去碰多边形上的一条边, 可以直接返回P点, 也可返回P与线段的中点.

否则, 就是多边形的一个角去碰圆弧, 返回线段上离P最近的那个顶点.

proc getPointLineManifold(point: Vector2D, line: Line2D): Manifold =
    new(result)
    let
        a = line.a
        b = line.b
    if (point - a) * (b - a) < 0: #cos < 0, 夹角大于90°, P在A的外侧
        result.add(line.a)
    elif (point - b) * (a - b) < 0:
        result.add(line.b)
    else:
        result.add(point)

两条线段的情况:

把与嵌入方向更垂直的那一条边找出来当作"参考边", 把另一条当作"事件边".

用参考边去裁剪事件边, 把事件边在参考多边形外侧的部分全部裁掉, 再把事件边在参考边左侧与右侧的部分也裁掉.

借助下面这个函数, 把edge和start都投影在dir上, 把edge投影比start小的部分都裁掉.

proc clipEdge(edge: Manifold, dir: Vector2D, start: Vector2D): Manifold =
    new(result)
    let
        min = start * dir
        proj1 = edge.a * dir - min
        proj2 = edge.b * dir - min
    if proj1 >= 0:
        result.add(edge.a)
    if proj2 >= 0:
        result.add(edge.b)
    if proj1 * proj2 < 0:
        result.add((edge.b - edge.a) * (proj1 / (proj1 - proj2)) + edge.a)

proc getLinesManifold(l1, l2: Line2D, dir: Vector2D): Manifold =
    var
        refVector: Vector2D
        refEdge: Line2D
        incEdge: Manifold
        normal: Vector2D

    let
        v1 = l1.b - l1.a
        v2 = l2.b - l2.a
        proj1 = abs(v1 * dir)
        proj2 = abs(v2 * dir)

    if proj1 < proj2: #投影较小的边更垂直, 作为参考边
        refEdge = l1
        refVector = v1
        incEdge = newManifold(l2)
        normal = tripleProduct(v1, dir.negate(), v1) #得到一个指向(-dir)方向的垂直于v1的向量
    else:
        refEdge = l2
        refVector = v2
        incEdge = newManifold(l1)
        normal = tripleProduct(v2, dir, v2)

    incEdge = clipEdge(incEdge, normal, refEdge.a) #把在参考多边形外部的部分裁剪掉
    if incEdge.length < 2: return incEdge

    normal = tripleProduct(normal, refEdge.b - refEdge.a, normal) #把在a外的部分裁剪掉
    incEdge = clipEdge(incEdge, normal, refEdge.a)
    if incEdge.length < 2: return incEdge

    incEdge = clipEdge(incEdge, normal.negate(), refEdge.b)
    return incEdge

效果如下: