物理引擎学习05-GJK和EPA计算穿透向量

article/2025/9/25 1:47:54

EPA，是扩展多边形算法(Epanding Polytop Algorithm) ，用来计算两个多边形碰撞的穿透深度和方向，可用于将两个发生碰撞的多边形分离。本文的写作目的，主要是对GJK和EPA算法的理解和应用。对算法本身感兴趣的朋友，可以阅读源论文的文献。本系列GJK算法文章共三篇，本篇是第三篇，强烈建议从第一篇开始看:

GJK碰撞检测算法基础
GJK计算多边形之间的最近距离
GJK和EPA计算穿透向量

本文作者游蓝海。原创不易，未经许可，禁止任何形式的转载。
在这里插入图片描述

1. 基本原理

当碰撞发生时，原点到最近的闵可夫斯基差集多边形的边(下文称作“差集最近边”)的距离，就是穿透深度，原点到该边的垂直向量就是穿透向量的方向。因此，核心问题就转换成了，如何求得距离原点最近的差集边。

当碰撞检测完毕后，我们会得到一个单形体(Simplex)，该单形体可能包含两个或三个support点，将这些support点首尾相连构成封闭的多边形。EPA算法每次迭代的时候，从这个多边形中选择一条最近的边，沿着该边的法线方向(原点到边的垂线方向)，向外扩展。直到某条边无法向外扩展时，则该边就是差集最近边。

可以看到，EPA算法和GJK求最近距离算法很相似，都是在找一个差集最近边。不过EPA用于发生碰撞的情况下，GJK求最近距离用于没有发生碰撞的情况下。理论上EPA也可以用于求两个多边形的最近边，只不过EPA收敛的速度没有GJK算法快。

2. 算法解析

EPA算法的前提是，两个多边形发生了碰撞。因此，我们要先使用GJK算法检测出碰撞，然后将得到的单形体，作为EPA算法开始的条件。

2.1 算法伪代码

Vector2 EPA(Simplex simplex)
{// 构造一个首尾相连的多边形simplexEdge.initEdges(simplex);while(true){// 找到距离原点最近的边Edge e = simplexEdge.findClosestEdge();// 沿着边的法线方向，尝试找一个新的support点Vector2 point = support(e.normal);// 无法找到能够跨越该边的support点了。也就是说，该边就是差集最近边float distance = Vector2.Dot(point, e.normal);if (distance - e.distance <= 0){// 返回穿透向量return e.normal * distance;}// 将新的support点插入到多边形中。// 也就是将边e从support点位置分割成两条新的边，并替换到多边形集合中。simplexEdge.insertEdgePoint(e, point);}
}

算法步骤描述：

构造一个首尾相连的多边形，也就是得到边的集合；
EPA迭代开始；
找到一个距离原点最近的边；
沿着该边的法线方向，尝试查找一个support点；
如果无法找到更远的support点，则说明该边就是差集最近边，算法结束；
将新的support点插入多边形中，相当于多边形向外扩展了；
跳转到步骤2。

计算步骤的分解动图:
在这里插入图片描述

2.2 构造边集

我们需要得到的是单形体多边形的边的集合，只用将support点首尾即可。为了方便后续计算，我们把每条边的法线和到原点的距离也计算出来。法线就是原点到边的垂线，取向外的方向。

有一个特殊的情况，如果GJK算法结束时，原点恰好位于单形体的某条边上。此时单形体中将会只有两个support点，且这两点的连线是经过原点的。这种情况下，无法用计算垂足的方法计算垂线，需要用到数学的方法：向量(x, y)的垂直向量为: (y, -x)，或者(-y, x)。随便用哪一个都行，因为两个support点首尾相连，会构成两条方向相反的边，恰好用数学方法求得的垂直向量方向也自然是相反的，EPA算法将会沿着两个相反的方向向外扩展。

void initEdges(Simplex simplex)
{int n = simplex.count();for (int i = 0; i < n; ++i){int iNext = (i + 1) % n;Edge edge = createEdge(simplex.get(i), simplex.get(iNext));edge.index = i;edges.Add(edge);}
}Edge createEdge(Vector2 a, Vector2 b)
{Edge e = new Edge();e.a = a;e.b = b;// 计算垂足Q。则法线向量为 OQ = Q - (0, 0) = Qe.normal = GJKTool.getPerpendicularToOrigin(a, b);e.distance = e.normal.magnitude;// 单位化边if (e.distance > float.Epsilon){e.normal *= 1.0f / e.distance;}else{// 如果距离原点太近，用数学的方法来得到直线的垂线// 方向可以随便取，刚好另外一边是反着来的Vector2 v = a - b;v.Normalize();e.normal = new Vector2(-v.y, v.x);}return e;}

2.3 查找最近边

找到距离原点最近的边即可：

Edge findClosestEdge()
{float minDistance = float.MaxValue;Edge ret = null;foreach (var e in edges){if (e.distance < minDistance){ret = e;minDistance = e.distance;}}return ret;
}

2.4 插入新的support点

将边e断开，e原来的两个端点分别与support点进行连接，然后重新插入到边的集合中。

void insertEdgePoint(Edge e, Vector2 point)
{Edge e1 = createEdge(e.a, point);// 覆盖掉原来e的位置edges[e.index] = e1;Edge e2 = createEdge(point, e.b);// 插入新的边edges.Insert(e.index + 1, e2);// 重新分配边的索引updateEdgeIndex();
}

2.5 浮点数误差问题

(增加于2021/08/22) 当GJK算法结束后，如果某个单形体的边经过了原点或者离原点非常近，则这条边的法线方向将会无法正确的计算出来，会导致后续的EPA扩张方向出现混乱，无法正确的计算出穿透向量。因此，最稳妥的方法是，进入EPA算法的时候，仅保留单形体的一条边，也就是只留下2个顶点。则EPA初始的边集只有两个有向边 ab和ba，而且两者的法线方向刚好相反，后续会沿着相反的方向进行扩张，不会产生交叉混乱的情况。

// 仅保留距离原点最近的一条边，避免浮点数误差引起原点落在了边上，造成无法计算出该边的法线方向
if (simplex.count() > 2)
{findNextDirection(); // 会自动删除一个离远点最远的点
}
...
edges.Add(createInitEdge(simplex.get(0), simplex.get(1)));
edges.Add(createInitEdge(simplex.get(1), simplex.get(0)));

3. 小结

EPA算法计算穿透向量，本质上是求得差集最近边。EPA从GJK算法结束后开始，不断的扩展单形体，直到达到边界。

本章Demo使用Unity3D引擎开发，Demo工程已上传github: https://github.com/youlanhai/learn-physics/tree/master/Assets/05-gjk-epa

4. 参考

GJK算法论文: https://ieeexplore.ieee.org/document/2083?arnumber=2083
EPA (Expanding Polytope Algorithm): http://www.dyn4j.org/2010/05/epa-expanding-polytope-algorithm

本系列文章会和我的个人公众号同步更新，感兴趣的朋友可以关注下我的公众号：游戏引擎学习。扫下面的二维码加关注：