本文解释了EPA算法及其常见实现。
EPA算法由Gino van den Bergen首次提出于其论文1。作为基于GJK的，用于找到穿透深度的扩展算法。
临近查询(Proximity Query)#
表示对两个物体进行如下查询：
- 找到最近距离和最近点对
- 判断两物体是否相交
- 找到穿透深度，以及在穿透深度下的一对可视点
- 找到一个公共点
其中1，2可以直接通过GJK得到。而3，4则需要使用EPA
EPA(Extended Polytope Algorithm)#
扩展多边形算法。意为在GJK输出的Simplex上加以扩展。
此算法十分简单。其输入和GJK一样，只需要两个物体的支撑映射。
首先，Simplex内必须包含原点（显然成立，因为相交时原点在Simplex内），然后不断地找到距离原点最近的面$F$中的最近点$\vec{v}$。用$\vec{v}$的方向作为支撑映射方向找到支撑点$S_{A - B}(\vec{v})$。然后用此支撑点扩展Simplex：
图中最外圈的是整个CSO。而Simplex从最开始的BCE逐渐“膨胀”，直到最近面已经膨胀到不能再膨胀了（$S_{A-B}(\vec{v})$落在现有面上）。此时$\vec{v}$就是分离向量，而其长度就是穿透深度。
但显然，对于二次曲面，必须像GJK一样使用容差让算法停止：
显然，EPA总是给出下界$|v|$。而上界则是真正的CSO到原点的最近距离。在每次迭代中也就是$\frac{v \cdot w}{|v|}$。于是和GJK一样，我们可以用上下界逼近的方式让算法停止：
$$ \begin{aligned} & \frac{v \cdot w}{|v|} - |v| \lt \epsilon \\ & \Rightarrow \\ & v \cdot w - |v|^2 \lt \epsilon |v| \end{aligned} $$这里$\epsilon$是用户给定的容差。
缝合(Suturing)/构造轮廓(Silhouette)#
指删除最近面后，使用剩下的面和顶点缝合成新Polytope的过程。
大部分物理引擎中这一步叫Suturing（缝合面）。Gino的论文中叫Silhouette。
这一步要注意的是，不能只利用删除的面剩下的三条边和顶点缝合。因为此时可能导致Simplex变成凹的。必须递归地看新点$\vec{w}$是否在删除面$F$连接的各个面$F_i$内（假设$F_i$法向量朝外，那么$\vec{w}$就得在$F_i$构成的负半空间内）。如果在正半空间，就必须也将$F_i$删掉，然后重复检查和其相邻的面。
而且论文中还说，有时候会存在新点位于所在面的边上，此时需要找到边距离原点最近点来让边也分裂：
但实际上Gino的实现中并没有做这个事情。如果遇到这种情况他就直接返回了。
SOLID3的实现#
bool penDepth(const DT_GJK& gjk, const DT_Convex& a, const DT_Convex& b,
MT_Vector3& v, MT_Point3& pa, MT_Point3& pb)
{
// 得到Simplex中的点。y是Simplex的点，p和q则是对应的原始点
int num_verts = gjk.getSimplex(pBuf, qBuf, yBuf);
MT_Scalar tolerance = DT_Accuracy::tol_error * gjk.maxVertex();
num_triangles = 0;
g_triangleStore.clear();
switch (num_verts)
{
case 1:
// Simplex中只有一个点。两个物体是相贴，没有穿透深度
return false;
case 2:
{
// Simplex中只有一个线段。那么尝试增加新点将其构造成四面体
MT_Vector3 dir = yBuf[1] - yBuf[0];
dir /= length(dir);
// furthestAxis：找到距离此向量最远的轴
// 其实就是找到分量最小的那个轴（分量越小，dir与此轴越垂直）
// 0 - x axis, 1 - y axis, 2 - z axis
int axis = dir.furthestAxis();
static const MT_Scalar sin_60 = sqrt(MT_Scalar(3.0)) * MT_Scalar(0.5);
// 注意：四元数的sin是半角，这里实际是绕dir旋转120度
MT_Quaternion rot(dir[0] * sin_60, dir[1] * sin_60, dir[2] * sin_60, MT_Scalar(0.5));
MT_Matrix3x3 rot_mat(rot);
// 首先得到距离此向量最远的轴，然后和dir叉乘
// 之所以要算最远的轴，是因为叉乘的结果最大，最不容易产生数值精度问题
MT_Vector3 aux1 = cross(dir, MT_Vector3(axis == 0, axis == 1, axis == 2));
// aux1绕dir旋转120度
MT_Vector3 aux2 = rot_mat * aux1;
// aux1绕dir旋转240度
MT_Vector3 aux3 = rot_mat * aux2;
// 上面三个aux其实是找到Simplex中线段垂直的平面上的三个均匀的方向
// 下面用这三个方向尝试找支撑点并鼓起Simplex为四面体
pBuf[2] = a.support(aux1);
qBuf[2] = b.support(-aux1);
yBuf[2] = pBuf[2] - qBuf[2];
pBuf[3] = a.support(aux2);
qBuf[3] = b.support(-aux2);
yBuf[3] = pBuf[3] - qBuf[3];
pBuf[4] = a.support(aux3);
qBuf[4] = b.support(-aux3);
yBuf[4] = pBuf[4] - qBuf[4];
// 检查原点是否在Simplex内（通过检查原点和各个面对面顶点是否在面同侧得到）
// 分别检查新三点与原有线段两端点构成的四面体
if (originInTetrahedron(yBuf[0], yBuf[2], yBuf[3], yBuf[4]) == 0)
{
pBuf[1] = pBuf[4];
qBuf[1] = qBuf[4];
yBuf[1] = yBuf[4];
}
else if (originInTetrahedron(yBuf[1], yBuf[2], yBuf[3], yBuf[4]) == 0)
{
pBuf[0] = pBuf[4];
qBuf[0] = qBuf[4];
yBuf[0] = yBuf[4];
}
else
{
// 原点不在Simplex内，失败
return false;
}
num_verts = 4;
}
// Fall through allowed!!
case 4:
{
// Simplex是四面体
// 先看原点是否在四面体内
// 此函数返回值：
// 0 ：原点在四面体内
// 1~4：坏的那个顶点，即和原点不在同侧的那个点。比如1就是yBuf[0]是坏的
int bad_vertex = originInTetrahedron(yBuf[0], yBuf[1], yBuf[2], yBuf[3]);
// 原点在四面体内
if (bad_vertex == 0)
{
// 构造四面体的四个面
// 当三角面退化成直线/点就会构造失败（通过Gram行列式发现线性相关时）
// 构造的同时会计算面到原点的距离，以及面上距离原点最近的点（用重心坐标表示）
Triangle *f0 = g_triangleStore.newTriangle(yBuf, 0, 1, 2);
Triangle *f1 = g_triangleStore.newTriangle(yBuf, 0, 3, 1);
Triangle *f2 = g_triangleStore.newTriangle(yBuf, 0, 2, 3);
Triangle *f3 = g_triangleStore.newTriangle(yBuf, 1, 3, 2);
// 这里有两种判断：
// 1. 面f不能是空
// 2. 原点是否刚好落在面上（f->getDist2() == MT_Scalar(0.0)，距离不会小于0）
// 如果落在面上，意味着下一步的支撑点搜索方向是0向量，从而导致EPA死循环
if (!(f0 && f0->getDist2() > MT_Scalar(0.0) &&
f1 && f1->getDist2() > MT_Scalar(0.0) &&
f2 && f2->getDist2() > MT_Scalar(0.0) &&
f3 && f3->getDist2() > MT_Scalar(0.0)))
{
return false;
}
// 构建四面体的拓扑结构：通过边将相离面连接起来
// 这里Edge的参数是面和边的其实点序号。比如Edge(f0, 0)就是取f0面的0, 1两个点构成的边
// 而Edge(f2, 2)就是取f2面的2, 0两个点构成的边
link(Edge(f0, 0), Edge(f1, 2));
link(Edge(f0, 1), Edge(f3, 2));
link(Edge(f0, 2), Edge(f2, 0));
link(Edge(f1, 0), Edge(f2, 2));
link(Edge(f1, 1), Edge(f3, 0));
link(Edge(f2, 1), Edge(f3, 1));
// 将面塞入优先队列（实现中是堆）中，以便于后面拿出距离原点最近的面
// 第二个参数是上界。当f距离原点的距离大于上界，不塞入队列
// 此外，面f到原点的最近点还必须严格在面上
addCandidate(f0, MT_INFINITY);
addCandidate(f1, MT_INFINITY);
addCandidate(f2, MT_INFINITY);
addCandidate(f3, MT_INFINITY);
break;
}
// 删除坏的点（这里是将第四个点替换坏的点），现在Simplex只含三个点
if (bad_vertex < 4)
{
pBuf[bad_vertex - 1] = pBuf[4];
qBuf[bad_vertex - 1] = qBuf[4];
yBuf[bad_vertex - 1] = yBuf[4];
}
num_verts = 3;
}
// Fall through allowed!!
case 3:
{
// 当Simplex只有三个点（只有一个三角面）时：
// 使用面的正负法线找到支撑点，以构成一个含有五个点的双四面体
// 这是一个聪明的做法，原始GJK结束时，原点在数值上应该是得在三角面内。这时如果只是构建一个四面体，
// 那么面到原点最近点向量就是0，这样就无法找到新的支撑点，算法会死循环。
// 那我不如直接构建一个六面体，原点肯定在此六面体内而且不再边上，就避免了这个问题。
MT_Vector3 v1 = yBuf[1] - yBuf[0];
MT_Vector3 v2 = yBuf[2] - yBuf[0];
MT_Vector3 vv = cross(v1, v2);
pBuf[3] = a.support(vv);
qBuf[3] = b.support(-vv);
yBuf[3] = pBuf[3] - qBuf[3];
pBuf[4] = a.support(-vv);
qBuf[4] = b.support(vv);
yBuf[4] = pBuf[4] - qBuf[4];
Triangle* f0 = g_triangleStore.newTriangle(yBuf, 0, 1, 3);
Triangle* f1 = g_triangleStore.newTriangle(yBuf, 1, 2, 3);
Triangle* f2 = g_triangleStore.newTriangle(yBuf, 2, 0, 3);
Triangle* f3 = g_triangleStore.newTriangle(yBuf, 0, 2, 4);
Triangle* f4 = g_triangleStore.newTriangle(yBuf, 2, 1, 4);
Triangle* f5 = g_triangleStore.newTriangle(yBuf, 1, 0, 4);
if (!(f0 && f0->getDist2() > MT_Scalar(0.0) &&
f1 && f1->getDist2() > MT_Scalar(0.0) &&
f2 && f2->getDist2() > MT_Scalar(0.0) &&
f3 && f3->getDist2() > MT_Scalar(0.0) &&
f4 && f4->getDist2() > MT_Scalar(0.0) &&
f5 && f5->getDist2() > MT_Scalar(0.0)))
{
return false;
}
link(Edge(f0, 1), Edge(f1, 2));
link(Edge(f1, 1), Edge(f2, 2));
link(Edge(f2, 1), Edge(f0, 2));
link(Edge(f0, 0), Edge(f5, 0));
link(Edge(f1, 0), Edge(f4, 0));
link(Edge(f2, 0), Edge(f3, 0));
link(Edge(f3, 1), Edge(f4, 2));
link(Edge(f4, 1), Edge(f5, 2));
link(Edge(f5, 1), Edge(f3, 2));
addCandidate(f0, MT_INFINITY);
addCandidate(f1, MT_INFINITY);
addCandidate(f2, MT_INFINITY);
addCandidate(f3, MT_INFINITY);
addCandidate(f4, MT_INFINITY);
addCandidate(f5, MT_INFINITY);
num_verts = 5;
}
break;
}
// Simplex中没有三角面了。失败
if (num_triangles == 0)
{
return false;
}
Triangle *triangle = 0;
MT_Scalar upper_bound2 = MT_INFINITY;
// 上面步骤构建了合法的多面体。现在开始真正执行EPA算法
do
{
// 从堆中弹出距离原点最近的面
triangle = triangleHeap[0];
std::pop_heap(&triangleHeap[0], &triangleHeap[num_triangles], triangleComp);
--num_triangles;
// 判断面是否还有效。
// TriangleStore里面有一个静态200个元素大小的数组，为了性能考虑，删除面并不是真的删除而是标记为obsolete
if (!triangle->isObsolete())
{
// 到达支持上限（200个点），失败
if (num_verts == MaxSupportPoints)
{
assert(false);
break;
}
// 得到面上最近点，以此点为方向找到新支撑点
pBuf[num_verts] = a.support( triangle->getClosest());
qBuf[num_verts] = b.support(-triangle->getClosest());
yBuf[num_verts] = pBuf[num_verts] - qBuf[num_verts];
int index = num_verts++;
// 得到穿透深度的上界
MT_Scalar far_dist = dot(yBuf[index], triangle->getClosest());
assert(far_dist > MT_Scalar(0.0));
MT_Scalar far_dist2 = far_dist * far_dist / triangle->getDist2();
// 更新upper_bound2为上界中较小的那个
GEN_set_min(upper_bound2, far_dist2);
// 算法终止条件判断：
// 1.上下界之差为 e|v|。这里为了防止舍入误差，还给了一个保底容差tolerance
// 2.新点就是三角形顶点。这是一个严格判定。可以看到源码他也用#ifdef 1圈起来了，说明这个判断不是必须的。但建议还是写上因为这是个硬兜底。
MT_Scalar error = far_dist - triangle->getDist2();
if (error <= GEN_max(DT_Accuracy::rel_error2 * far_dist, tolerance)
#ifdef 1
|| yBuf[index] == yBuf[(*triangle)[0]]
|| yBuf[index] == yBuf[(*triangle)[1]]
|| yBuf[index] == yBuf[(*triangle)[2]]
#endif
)
{
break;
}
// 获得空闲的面下标
int i = g_triangleStore.getFree();
// 使用新点缝合面
// yBuf：当前顶点集合
// index：新点在yBuf中的下标
if (!triangle->silhouette(yBuf, index, g_triangleStore))
{
break;
}
// 遍历所有新面，将面塞入堆中
while (i != g_triangleStore.getFree())
{
Triangle *newTriangle = &g_triangleStore[i];
addCandidate(newTriangle, upper_bound2);
++i;
}
}
}
while (num_triangles > 0 && triangleHeap[0]->getDist2() <= upper_bound2);
// 返回对应值
v = triangle->getClosest();
pa = triangle->getClosestPoint(pBuf);
pb = triangle->getClosestPoint(qBuf);
return true;
}这里比较复杂的可能是轮廓构造部分，可以细看一下：
bool Triangle::silhouette(const MT_Vector3 *verts, Index_t index, TriangleStore& triangleStore)
{
int first = triangleStore.getFree();
// 要缝合的面肯定是已经被遗弃的，直接删除
setObsolete(true);
// 对相邻三角面也做缝合操作（会判断相邻面是否需要缝合）
// 注意这里调用的是Edge::silhouette，不是本函数的递归
bool result = m_adjEdges[0].silhouette(verts, index, triangleStore) &&
m_adjEdges[1].silhouette(verts, index, triangleStore) &&
m_adjEdges[2].silhouette(verts, index, triangleStore);
// 再将所有需要的边和新顶点缝合起来
if (result)
{
int i, j;
for (i = first, j = triangleStore.getFree()-1; i != triangleStore.getFree(); j = i++)
{
Triangle *triangle = &triangleStore[i];
half_link(triangle->getAdjEdge(1), Edge(triangle, 1));
if (!link(Edge(triangle, 0), Edge(&triangleStore[j], 2)))
{
return false;
}
}
}
return result;
}而Edge::silhouette则是：
// 返回值表示此操作是否成功
bool Edge::silhouette(const MT_Vector3 *verts, Index_t index, TriangleStore& triangleStore) const
{
if (!m_triangle->isObsolete())
{
// 看新点是否在面外侧。这里用已经记录的面上距离原点的最近点到原点的向量作为面法线来判断
if (!m_triangle->isVisibleFrom(verts, index))
{
// 直接用边和新点缝合成新三角面
Triangle *triangle = triangleStore.newTriangle(verts, index, getTarget(), getSource());
if (triangle)
{
// half_link就是将某个面连接到某个已有边
half_link(Edge(triangle, 1), *this);
return true;
}
return false;
}
else
{
// 新点在面外侧，删除当前面，
m_triangle->setObsolete(true);
int backup = triangleStore.getFree();
// 递归地，对其另外条边都做缝合操作。这个if和后面else if都是一样的操作只是对不同的边
if (!m_triangle->getAdjEdge(circ_next(m_index)).silhouette(verts, index, triangleStore))
{
// 如果相邻边的缝合操作没成功，那得保留下当前面以防出错
m_triangle->setObsolete(false);
// 并且，本来和已删除面相连的那条边，此时得和新点缝合成新面
Triangle *triangle = triangleStore.newTriangle(verts, index, getTarget(), getSource());
if (triangle)
{
// 新面连接到此边
half_link(Edge(triangle, 1), *this);
return true;
}
return false;
}
// 对另外一条边做一样的操作
else if (!m_triangle->getAdjEdge(circ_prev(m_index)).silhouette(verts, index, triangleStore))
{
m_triangle->setObsolete(false);
triangleStore.setFree(backup);
Triangle *triangle = triangleStore.newTriangle(verts, index, getTarget(), getSource());
if (triangle)
{
half_link(Edge(triangle, 1), *this);
return true;
}
return false;
}
}
}
return true;
}在线演示#
下面是目前文章提到的各种查询方法演示。其中GJK在GJKTetrahedrons例子中，EPA则在MTDTetrahedrons中。