从0开始制作游戏物理引擎（五）

本文解释了GJK算法及其常见实现。

GJK算法来源于两份论文：

1. GJK原始论文^[1]：由Gilbert, Johnson, Keerth三人编写。对GJK算法的正确性进行了证明，并给出了算法。论文偏理论，对数值问题没有一个性能较好的解决方法。
2. GinoGJK^[2]：由Gino van den Bergen编写。在原有GJK基础上解决了很多痛点，让算法性能更高以便于更好的用于实时环境。论文偏工程。此文章中的算法和其实现^[3]是现代游戏物理引擎中用的最多的。

本文会将上述三个文章中的知识做总结，述说现在成熟的GJK方案并阅读Gino的GJK源码。

现在大多数文章介绍的都是Ericson的GJK算法，他首次显式地提出在GJK中使用重心坐标并推广到游戏编程圈内。但从论文本身来说，和Ericson的方法（至少形式上）并不一样。

多篇文章的符号不一样，但由于Gino的实现和其文章一致，所以我们用Gino文章中的符号。

GJK算法

预备知识

GJK算法能做很多事情：

• 判断两个凸体是否相交
• 如果相交，获得穿透深度（GinoGJK）
• 如果不相交，算得两个物体之间的距离已经最近点（原始GJK）

首先是一些符号定义。

距离

定义两个物体$A$和$B$间的距离为

GJK中使用欧几里得距离。

半空间

半空间为使用平面分割的一半的空间。定义平面为

其法向量$\vec{n}$指定了平面正向。平面正向的空间记为$H^+$，而反向的记为$H^-$。

仿射集和凸集

定义仿射集为：

定义凸集为：

凸集是比较重要也是常用的概念。显然，凸集是仿射集的扩展，也是所有凸体的描述（满足任意集合内任意两点连线在集合内）。

一般用$\operatorname{co}X$表示$X$的凸包。显然，这也是GJK首要研究的对象。

重心坐标是$|X| = 3$的集合的特例，所以也符合凸集的公式。

再定义仿射无关(Affinely Independent)：

对于集合$X = \{x_1, x_2, \cdots, x_n\}$，如果集合

线性无关，那么就叫$X$仿射无关。

仿射无关的几何意义是，集合中没有多余的点落在此集合构成的凸包内（面上可以）。也就是说，集合内所有点共同支棱起一个凸包。

支撑映射(Support Mapping)

对于集合$X$，其某个方向$d$的支撑距离为

即沿着这个方向在$X$上的最远距离。而满足$h_X(d)$中$x$的这个点就叫支撑点，记为$s_X(d)$。

显然有：

而在GJK算法中，一般研究离原点最近的点：

而由凸集的定义，显然有：

接下来是一些常见几何体的支撑映射

常见几何体的支撑映射

Box

可以利用其三个互相垂直的轴加速计算。

假设此Box由三个轴上的半长$\eta_x, \eta_y, \eta_z$表示。那么支撑点函数就是：

Sphere

显然为：

Capped Cone

意指有界圆锥，即只有一个顶点和一个底面的圆锥。

假设此圆锥轴心位于$y$轴，朝上（顶点在上底面在下），底面半径为$\rho$，顶点在$y = \eta$处，底面中心在$y = -\eta$处。顶点处的半角为$\alpha$满足$\sin(\alpha) = \frac{\rho}{\sqrt{\rho^2 + (2\eta)^2}}$。

令$\sigma = \sqrt{x^2 + z^2}$为任意点到$y$轴的距离，那么支撑点函数为：

Capped Cylinder

有界圆柱。轴为$y$轴，中心在原点。半高为$\eta$，半径为$\rho$。

令$\sigma = \sqrt{x^2 + z^2}$为任意点到$y$轴的距离，那么支撑点函数为：

其支撑点函数为：

仿射变换后的几何体

定义仿射变换为$T(\vec{x}) = B\vec{x} + \vec{c}$。那么对于一个物体$A$经过仿射变换之后的物体$T(A)$，其支撑点函数为：

很易证。原论文有证明这里不写了。

尖锐边角的处理

圆柱和圆锥的底边边角即带有弧又很尖锐，圆锥的顶角也同理。在我的实践中发现这很容易导致数值精度误差，会导致GJK算法在还未真正收敛的情况下退出。

JoltPhysics给出了一个解决方法我觉得很好用：将这些地方变圆。比如对于圆柱体，其SupportMapping的代码为：

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13

 Vector3 CylinderSupportMapping(Vector3 dir) const {
	// m_convex_radius就是变圆的半径
	Vector3 d = dir.normalized();
	real dn = d.dot(m_axis);
	Vector3 radial = d - m_axis * dn;
	real hh = m_half_height - m_convex_radius;
	real rr = m_radius - m_convex_radius;
	Vector3 p = m_center + m_axis * (dn >= 0 ? hh : -hh);
	if (radial.squaredNorm() > 0) {
		p += radial.normalized() * rr;
	}
	return p + d * m_convex_radius;
}

Minkowski差和CSO

在Gino的论文²中，将两个物体$A$和$B$的Minkowski差称为CSO(Configuration Space Obstacle)：

由于会反复出现$A$和$B$的Minkowsi差，我们用$K = A - B$来表示。

现在可以证明：

1. 两物体相交等价于原点在CSO内：显然，两物体相交区域内必有两个位置一样的点$a \in A, b \in B$，那么显然$a - b = \vec{0}$，那么原点在CSO内。
2. 计算两物体的距离等价于计算原点到Minkowski差的距离：显然，原点到Minkowski差的距离为$d(\vec{0}, K) = \min \{|k - \vec{0}|: k \in K\}$，而 $x = a - b, a \in A, b \in B $正是两物体之间距离。

接下来介绍GJK原始论文中的算法。而这个算法着重于求两物体间的距离。那么这个问题就转换为求$v(K)$。

算法

由于GJK是对任意维度$R^m$都成立的算法，那么对其的证明自然围绕$R^m$。

定理1

令$K = A - B, K \in R^m$是紧致和凸的。那么定义$g_K :R^m \rightarrow R$为：

$$ g_K(x) = |x|^2 + h_K(-x) $$

所谓“紧致(compact)”是拓扑学中的概念，意为有界且闭的。这里紧致+凸就是指一般的凸体或者说凸的刚体。

如果假设$x \in K$，那么定理1有四条性质：

1. $g_K(x) \ge 0$恒成立。这是显然的，因为$x\cdot s_K(-x) = \min\{a: a \cdot x, a \in K \} \cdot x \le x \cdot x = |x| ^ 2$，而$g_K(x) = |x|^2 -x\cdot s_K(-x) \ge 0$显然就成立了。

2. 如果$g_K(x) \gt 0$，那么在线段$\operatorname{co} \{x, s_K(-x)\}$上一定有一点$z$满足$|z| \lt |x|$（即$z$到原点的距离比$x$小）

   证明：

   1. 先证$z$的存在性：

      • 显然在$|s_K(-x)| \lt |x|$时，取$z = s_K(-x)$即可。

      • 那么考虑$|s_K(-x)| \ge |x|$的情况：

        由于$z$是线段上一点，那么可以用凸集表示为：

        $$ \begin{aligned} & z = (1 - \lambda) x + \lambda s_K(-x) = x + \lambda(s_K(-x) - x) \\ & |z|^2 = (x + \lambda(s_K(-x) - x))^2 = |x|^2 + 2\lambda x(s_K(-x) - x) + \lambda^2|s_K(-x) -x|^2 \end{aligned} $$

        我们要求得最小的$|z|$，所以对$|z|^2$求导数，并令导数为0：

        $$ \begin{aligned} & \frac{d|z|^2}{d|z|} = 2x(s_K(-x) - x) + 2\lambda|s_K(-x) -x|^2 = 0 \\ & \Rightarrow \\ & \lambda = \frac{x(x - s_K(-x))}{|s_K(-x) - x|^2} & (1) \end{aligned} $$

        那么问题就变成找到一个合格的$\lambda$。

        而我们可以对$g_K(x)$变形得到：

        $$ g_K(x) = |x|^2 + h_K(-x) = |x|^2 +(-x)s_K(-x) = |x|^2 - xs_K(-x) = x(x - s_K(-x)) $$

        正是上面$(1)$式的分子。所以有：

        $$ \lambda = \frac{g_K(x)}{|s_K(-x) - x|^2} $$

        而：

        $$ \begin{aligned} & |x - s_K(-x)|^2 = |x|^2 + |s_K(-x)|^2 - 2x\cdot s_K(-x) \\ & 2g_K(x) = 2(|x|^2 - x \cdot s_K(-x)) = 2|x|^2 - 2x\cdot s_K(-x) \\ & \text{上述两式相减得:} \\ & |x - s_K(-x)|^2 - 2g_K(x) = |s_K(-x)|^2 - |x|^2 \ge 0 &(*) \\ & \Rightarrow \\ & |x - s_K(-x)|^2 \ge 2g_K(x) \Rightarrow \frac{g_K(x)}{|x - s_K(-x)|^2} \le \frac{1}{2} \Rightarrow \lambda \le \frac{1}{2} \end{aligned} $$

        $(*)$式是因为，$s_K(-x) = \{y : \max\{y \cdot (-x)\}\} \ge x \cdot (-x)$。那么左右两边平方就有$|s_K(-x)|^2 \ge |x|^2$。

        而又根据$\lambda$的定义有$\lambda \gt 0$。所以综上，总能找到一个$\lambda \in [0, \frac{1}{2}]$。那么也就总存在一个$z$了。

      综上，$z$的存在性证明完毕。

   2. 然后证明$|z| \lt |x|$，只需将$g_K(x) = \lambda|x - s_K(-x)|^2$带入$|z|^2$式子中即可：

      $$ \begin{aligned} |z|^2 & = |x|^2 + 2\lambda x(s_K(-x) - x) + \lambda^2|s_K(-x) - x|^2 \\ & = |x|^2 + 2\lambda(-g_K(x)) + \lambda(\lambda|x - s_K(-x)|^2) \\ & = |x|^2 - 2\lambda g_K(x) + \lambda g_K(x) \\ & = |x|^2 - \lambda g_K(x) \end{aligned} $$

      因为$\lambda \gt 0$, $g_K(x) \gt 0$（根据题设），所以有$|z|^2 \lt |x|^2$成立。

3. $x = v(K)$当且仅当$g_K(x) = 0$。也就是说$x$为整个Minkowski差中距离原点最近的点，当且仅当$g_K(x) = 0$。

   证明：

   1. 先证必要性：

      $$ g_K(x) = 0 \Rightarrow |x|^2 +h_K(-x) = 0 \Rightarrow |x|^2 = -h_K(-x) = \min \{a \cdot x: a \in K\} $$

      那么有

      $$ |x|^2 \le |x|^2 + |a - x|^2 = |a|^2 + 2(|x|^2 - a \cdot x) \le |a|^2, a \in K $$

      这样就证明了$|x|^2 \le |a|^2$小于$K$中任意数的模的平方。那么$x$就是距离原点的最近点。

   2. 而充分性可以用反证法：

      假设$g_K(x) \gt 0$。那么根据性质2，一定有一个距离原点更小的$z$，违背条件$x = v(K)$。所以矛盾。

4. $|x - v(K)|^2 \le g_K(x)$

   证明：

   将左式展开：

   $$ \begin{aligned} |x - v(K)|^2 & = |x|^2 - 2x\cdot v(K) + v(K)^2 \\ & \le |x|^2 - 2x \cdot v(K) + x \cdot v(K) \\ & = |x|^2 - x \cdot v(K) \\ & \le |x|^2 - x \cdot s_K(-x) \\ & = g_K(x) \end{aligned} $$

   证毕。

距离算法

通过定理1可知，当$g_K(x) = 0$时，$x = v(K)$就能找到要求的距离原点最近点。这个是GJK算法的终止条件。

先给出通过GJK求距离的算法：

给出紧致的凸集$K \in R^m, K = \{y_1, y_2, \cdots, y_v \}, 1 \le v \le m+1$，算法步骤如下：

1. 令$V_0 = \{y_1, \cdots, y_v\}$，令$k = 0$
2. 找到$v_k = v(\operatorname{co} V_k)$
3. 如果$g_K(v_k) = 0$，那么显然$v(K) = v_k$，算法终止返回$|v_k|$作为距离
4. 令$V_{k+1} = \hat{V_k} \cup \{s_K(-v_k)\}$，这里 $\hat{V_k} \subset V_k$，并且$|\hat{V_k}| \le m, v_k \in \operatorname{co}\hat{V_k}$。然后$k = k+1$自增$k$，然后回到步骤2

翻译成人话：

• 初始集合$V_0$是整个凸体的点集

• $k$其实是当前迭代次数，而$v_k$是第$k$步下距离原点最近点

• 每一次迭代都算$V_k$中距离原点的最小值

• 如果算法没终止，这里$\hat{V_k}$的要求有三个：

  1. 包含$v_k$
  2. 元素个数不大于$m$
  3. $\hat{V_k} \subset V_k$

  意思是每次都要在$V_k$中找到一个子集，这子集得包含$v_k$且元素个数不大于$m$。

  那么根据$V_{k+1}$的构造有$|V_{k+1}| \le m$。并且$V_{k+1}$一定包含$s_K(-v_k)$。也就是说$V_{k+1}$一定包含两个点：

  • $s_K(-v_k)$
  • $v_k$

我们知道大部分GJK文章中说的做法都是：

1. 每步找Simplex距离原点最近的点$v_k$
2. 如果点在Simplex内，停止算法。否则，如果Simplex点数已满，删除$v_k$对面的点，然后将$v_k$加入Simplex

这里的Simplex其实就类似于$V_k$。而删点其实就类似于找到$\hat{V_k}$。所以算法每一步都在构造新Simplex。

我们后面可以看到，这就是对$R^3$下的算法进行的特化。数学上来说，此方法和论文中的方式是等价的。只是GJK本身能够处理$R^m$空间，所以论文中写了个更通用的算法。

这里有几点要注意：

• 第二步的$v_k = v(\operatorname{co}V_k)$说明$v_k$可以在$\operatorname{co}V_k$的面上或体内
• 那么第三步的意思其实是：原点在$\operatorname{co}V_k$上，也就是原点在单纯形内，那么算法立刻退出

接下来要证明此方法是正确的（能够找到$v(K)$的）：

1. 首先证明此算法每一步都是可达的。主要是步骤4：

   在步骤4中，显然$g_K(x) \gt 0$，那么根据定理1的性质3，$v_k \ne \vec{0}$（说明原点不在单纯形上或内）。那么$s_K(-v_k)$就一定能找到点，那么$\hat{V_k}$就一定可构造了。

2. 然后证明$|v_{k+1}| \lt |v_k|$，即每次找到的一定是离原点更近的点：

   根据定理1的性质2有：

   $$ |v_{k+1}| = |v(\operatorname{co}V_{k+1})| \le |v(\operatorname{co}\{v_k, s_K(-v_k)\})| \lt |v_k| $$

然后证明此算法是可以在有限步骤内完成的。这被称为定理2：

定理2

假设$Z \subset R^m$是有限集。$K = \operatorname{co}Z$。假设$\forall \eta \in R^m, s_K(\eta) \in Z$，那么距离算法可以在有限步内找到$v(K)$

证明：

简单来说，因为每一步的$V_k$和之前的任何$V_k$都是不相同的（因为上面的证明2，$|v_k|$的大小是严格递减的，所以每个$v_k$肯定不一样，而$V_k$又一定要包含$v_{k-1}$，所以显然每个$V_k$也是不一样的）。而$K$又是有限集，也就是说他的子集个数是有限的。那么算法最坏情况下会遍历完其所有子集。而由之前Minkowski差转为寻找最近点问题的证明可知，一定存在$v(K)$。所以$v(K)$一定会在某个子集进入算法循环时被找到。

这就证明了算法可在有限步（最坏情况下是$K$子集个数）中完成。

距离子算法(Distance Subalgorithm)

距离算法中的第二步和第四步中的$\hat{V_k}$的选取并没有明确说明要怎么做。距离子算法就是来做这个事情的。

距离子算法由于是Johnson发明的，所以又叫Johnson算法。

我们假定每一步的$V_k$都是仿射无关的（如果某一步仿射相关，那整个GJK就崩了，算法执行失败）。为了方便记忆，后用$Y$表示：$Y = V_k$。

令$Y$的非空集合个数为$\sigma$个，定义数列$s = 1, \cdots, \sigma$为其集合个数的序列。而$|Y| = v$，定义元素在$Y$中的下标序列为$I = \{1, \cdots, v\}$。对于$Y$的任意的子集$Y_s$，我们使用$I_s \subset I$表示$Y_s$中元素下标的集合，而$I_s^{'}$为$I_s$在$I$中的补集（即不在$Y_s$中的元素下标）。

首先，有$v(\operatorname{aff}Y_s) = \sum_{i = 2}^{r}\lambda_ix_i$，且$\lambda_1 = 1 - \sum_{i = 2}^r \lambda_i$。那么我们要找$v(\operatorname{co}Y_s)$就是找到一系列$\lambda_i$，使得$|v(\operatorname{aff}Y_s)|$最小。那么定义函数$f(\lambda_2, \cdots, \lambda_r)$为:

我们要求这个$r$元函数的最小值。根据分析学的理论：

• 函数导数为0的点可能是函数的最小值点（需要通过Hessian矩阵辅助判断）

但这里$f$是凸函数，这意味导数为0的点一定是最小值点。那么可以通过Jacobian矩阵表示其导数为：

通过Cramer法则^[4]可以解出所有$\lambda_i = \frac{\Delta_i(Y_s)}{\Delta(Y_s)}$。其中$\Delta_i(Y_s)$是第$i$行替换$b$得到的矩阵，而$\Delta(Y_s) = |A_s|$。

当然这里可以硬算行列式。但论文中给出了一种更加快速的，利用旧行列式缓存的方式，以递增的方式去计算。

具体的公式是：

即：

• 含有单个元素的Cache为1
• 在原有行列式基础上增加一个点，可以通过增量形式计算这个新行列式
• 分母由所有$\Delta_i(Y_s)$相加得到

我们可以举例说明这个算法的细节：

$\lambda_i$的增量计算过程

假设我们在$R^3$下运行此算法。并且此时$Y$里面没有点。所以我们先往里面填充点：

第一步：

加入$y_1$。此时$Y = Y_s = \{y_1\}$，由定义$\Delta_1(Y_s) = \Delta_i(\{y_1\}) = 1$，$\Delta(Y_s) = 1$。

第二步：

加入$y_2$，此时$Y = \{y_1, y_2\}$。注意$Y_s \subset Y$按照$I_s$中的下标选取元素。所以这里有三种$Y_s$：

这里，由于$y_2$是新增点，所以$I_s^{'} = \{2\}$。那么就是：

那么有

可这为何能成立？因为这就是$|A_i|$的Laplace展开。当有两个点时：

将$|A_2|$按第二列展开就得到了。

那么新问题来了。公式里有$\Delta(Y_s) = \sum_{i \in I} \Delta_i(Y_s)$。我们这里是$\Delta(Y_3) = \Delta_1(Y_3) + \Delta_2(Y_3)$。我们有$\Delta_2(Y_3)$，但是$\Delta_1(Y_3)$从何而来？其实一样的，只是组成的集合不一样。

对于$\Delta_1(Y_3)$，$j = 1$，也就是说此时$I_s^{'} = \{1\}$。那么为了凑齐$I = \{1, 2\}$显然$I_s = \{2\}$。

也就是说，$\Delta_1(Y_3)$是视为从$\{y_2\}$中增加$\{y_1\}$点得到的结果。那么也是一样计算，会得到：

那么为什么$\Delta(Y_s) = |A_s| = \sum_{i \in I} \Delta_i(Y_s)$成立呢？这其实是$|A_s|$按第一行展开的结果。

所以这里你就可以看到，使用旧的$\Delta_i(Y_s)$去加速计算新$\Delta_j(Y_s)$的过程了。

第三步：

为了更好地看到这种加速，我们再加一个点。将$y_3$加入$Y$中得$Y = \{y_1, y_2, y_3\}$。那么此时有6个$Y_s$：

那么新增的$Y_4 \sim Y_6$是否能用$Y_1 \sim Y_3$加速计算呢？显然是可以的。首先根据定义$\Delta(Y_4) = 1$。然后

而$Y_6$则是：

可以看到确实可以通过旧值加速计算新值。这里只有$\Delta_2(Y_6)$中的$\Delta_i(\{y_1, y_3\})$没有旧值，但仍旧可以使用递推公式算得旧值。

当$Y$内点到达上限$m$时，此时新增点前需要删除点。新增的点仍旧可以用旧值加速计算（不再举例赘述）。

这个递推公式的好处就是可以快速计算$\lambda$，只需要使用旧值计算$\Delta_i(Y_s)$，然后将各个$\Delta_i(Y_s)$相加即可得到$\Delta(Y_s)$，然后就可以用Cramer法则得到最后的$\lambda$。免去了每次都要从头展开行列式带来的性能问题。

定理3

$Y_s$有效，当且仅当

1. $\Delta(Y_s) \gt 0$
2. $\Delta_i(Y_s) \gt 0, i \in I_s$
3. $\Delta_j(Y_s \cup \{y_j\}) \le 0, j \in I_s^{'}$

最后，$\lambda_i = \frac{\Delta_i(Y_s)}{\Delta(Y_s)}$

这三条分别说明：

1. $Y_s$是仿射无关的。

   这是引理1：$Y_s$仿射无关当且仅当$\Delta(Y_s) \gt 0$

   证明：

   ​ 因为$\Delta(Y_s) = Q_s^TQ_s$，而$Q_s$则是：

   $$ Q_s = \begin{bmatrix} (x_2 - x_1) \cdots (x_r - x_1) \end{bmatrix} $$

   而形如$Q_s^TQ_s$的矩阵叫做Gramian矩阵^[5]。而只有当他正定时，$Q_s$中元素线性无关。而$Q_s$元素线性无关正是$Y_s$仿射无关的定义。

2. $v(\operatorname{co}Y_s)$是$\operatorname{co}Y_s$的相对内点（即在$\operatorname{co}Y_s$内部，不包含边但包含面。或者换句话说，$\lambda_i \ne 0$）。这意味着此$v(\operatorname{co}Y_s)$无需再在$Y_s$的子集内搜索（因为如果点落在边上，说明还存在更小的子集需要搜索）

   这其实是个凸优化里面的结论：$Y_s$仿射无关时，$\operatorname{co}(Y_s) = \{ \sum\lambda_i y_i : \lambda_i \gt 0, \sum \lambda_i = 1 \}$。这里不再证明。

3. $v(\operatorname{co}Y_s) = v(\operatorname{co}Y)$，即局部最小值就是全局最小值。

距离子算法步骤

给出有限集$Y = \{y_1, \cdots, y_v \} \subset R^m$和其所有子集$Y_s, s = 1, \cdots, \sigma$。算法步骤如下：

1. 令 $s = 1$
2. 如果$\Delta(Y_s) \gt 0$且 $\Delta_i(Y_s) \gt 0, i \in I_s$ 且 $\Delta_j(Y_s \cup \{y_j\}) \le 0, j \in I_s^{'}$。那么我们就找到了$v(\operatorname{co}Y)$（使用$\lambda_i$可计算出来）。算法终止。
3. 如果$s \lt \sigma$，$s = s+1$返回步骤1
4. 如果算法停止，返回失败

这里简单来说就是遍历$Y_s$所有子集，试图找满足条件2的$v(\operatorname{co}Y)$。

现在我们可以看一下，论文中的距离子算法和重心坐标的联系。

以Simplex包含三个点为例（也就是三角形，这样简单一些）。假设现在Simplex中有三个点$Y = \{ y_1, y_2, y_3 \}$。那么其一共有7个子集。分别是：

本质上，含有两个点的集合就是边（线段），含有三个点的集合就是三角面。

而对每个子集判断$\Delta_i(Y_s) \gt 0, \Delta(Y_s) \gt 0$其实就等价于看原点是否在当前边/面的Voronoi域内。

$\Delta(Y_s)$我们知道是Gramian行列式，这个行列式在数学上就是计算由$y_i$个点组成的仿射无关的体的体积。

$\Delta(Y_s) \gt 0$的判断，其实就是在说这$y_i$个点仿射无关，确定了Simplex的有效性。并且$\Delta(Y_s)$本质上是平方量，不会出现小于0的情况。

而$\Delta_i(Y_s)$其实就是计算Simplex中子区域（以原点在Simplex上的最近点为顶点）的体积。所以两者的比值才是重心坐标。其实和重心坐标计算公式是完全等价的。

然后$\Delta_i(Y) \gt 0$的判断，这里$\Delta_i(Y)$是带符号的。等于在说离原点的最近点必须在$\operatorname{co}(Y_s)$上。

而$\Delta_j(Y_s \cup \{y_j\}) \lt 0$的判断则是说明，原点与新点$y_j$必须在$\operatorname{co}Y_s$的异侧（等价于Ericson GJK中，说新点要“越过”原点）。

所以可以看到，原始GJK在数学上的表述和Ericson GJK是完全等价的。

距离子算法的工程实现

GJK原始论文¹的理论已搞懂，但工程上到底要如何实现呢？Gino在他的论文²中给出了答案。

我们只考虑$R^3$下的情况（但理论上Gino的方法可以推广到$R^m$）。由于Simplex最多只有4个点（这里Simplex指$Y$），而含有4个点的集合最多只有16个，所以可以用一个16x4的矩阵存储$\Delta_i(Y_s)$。

那么我们可以用一个4位的位掩码来表示这16个集合。甚至可以根据此掩码的1的位置表示此集合中包含的顶点。比如mask = 0100就是第4个子集，此子集包含第三个顶点。而mask = 0101就是第5个子集，此子集包含两个顶点（第一个和第三个）。

有了这个表示法，$\Delta_i(Y_s)$的增量计算就好算了，直接套论文中的公式即可。

而在实际编码中³，Gino甚至对$\Delta_j(Y_s \cup \{y_j\}) = \sum_{i \in I_s}\Delta_i(Y_s)(y_k - y_j) \cdot y_i$中反复用到的$y_k - y_j$也做了缓存（将其正负值各存一份）用于加速计算。

从CSO中反向计算最近点

在整个GJK算法结束后，我们可以找到CSO上距离原点的最近点$v(K)$。此时，我们可以通过此点反向计算出在$A$和$B$上的最近点对$a, b$。只需要使用重心坐标$\lambda_i$计算即可。因为：

那么可以用$\sum \lambda_i a_i$得到$A$中最近点，$\sum \lambda_i b_i$得到$B$中最近点。

所以算法中不仅要记录CSO上的点，还得记录这个点的原始两个点。

适用于二次曲面的GJK

原始的GJK论文只讨论了凸多面体。但后人发现可以应用于任何的凸体，包括含有二次曲线的（比如球，球扫略体等）。并且还证明了GJK算法只需要两个物体的支撑映射，而不需要全部的顶点信息。

那么有一个问题：对于含有二次曲面的GJK，其曲面上的顶点是无数个。这有可能导致GJK不停地逼近最近点，或者由于数值精度问题在最近点疯狂摇摆。此时必须增加容差。

记每次迭代中距离原点的最近点为$\vec{v_k}$，在此次迭代中新加入Simplex的点为$\vec{w_k}$（使用$\vec{v_k}$找到）。那么这两个点构成一个支撑平面：

GJK算法本身只会给出$d(A, B)$的上界$|v(A - B)|$（因为GJK一直在尝试减少$|v(A - B)|$所以是上界）。但并没有给出下界。

而根据刚才的支撑平面可知，$|v(A - B)|$的下界就是点$\vec{w_i}$到原点的距离，即：

那么当上界界逐渐逼近的时候，显然GJK算法会越来越准确。那么我们认为，当

的时候，GJK算法就找到了正确的最近点。这里$\epsilon$是一个用户给定值。

但这里有个问题：此算法不一定能终止，虽然上界$|v_k|$是单调递减的，但下界$\delta_k$并不是一个单调递增的数值，所以可能导致算法一直无法满足。

解决方法是找到所有$\delta_k$中的最大的那个：

那么$\mu_k$显然是递增的，那么总是有：

这就是最后的终止条件。

这里的夹逼其实就是原始条件$g_K(v(K)) = 0$的相对容差版本：

其实左边是$g_K(v_k) = |v_k|^2 - s(-x)$的近似版本，所以所以这个式子其实就是$g_K(x)$的容差版本，只是容差不太一样而已（$|v_k|^2$和$|v_k|\max \{ |v_k| \}$之间的差距可以通过选取适当的$\epsilon$抹去）。

Gino发现，在物体特别大的时候这个终止条件会有浮点数精度问题，所以需要一个相对容差而非绝对容差：

而当物体特别小的时候，会出现浮点数下溢导致$\vec{v}$变成0向量，导致算法死循环。所以他的建议是再给一个和0相关的容差$\omega$：

而在实际代码中，他使用的是相对容差：

当满足这个容差时，算法也终止，同时认为当前$\vec{v}$就是最近点。

而Gino在论文里说过，原式有$|v_k|$，为了在工程上不进行昂贵的开方操作，不如将等式左右两边再乘上$\max \{|v_k|\}$变成：

这也和上面说到的公式一模一样，反向证明了GinoGJK的终止条件等价于原始GJK的终止条件。

数值精度问题以及Gino的鲁棒性优化

1. 如果几何体偏离原点太远，可能由于大浮点数运算问题导致精度问题。原始论文的方法是利用其几何中心连线的中点$\rho$进行偏移：

   $$ \begin{aligned} & z_1 = \frac{\sum_i^N{z_{1i}}}{N} \\ & z_2 = \frac{\sum_i^N{z_{2i}}}{N} \\ & \rho = \frac{z_1 + z_2}{2} \end{aligned} $$

2. 在判断$g_K(x) = 0$的时候，可以使用一个相对容差进行判断：

   $$ \begin{aligned} & g_K(x) \le \epsilon(D(K)^2) \\ & D(K) = \max(|z| : z \in K) \end{aligned} $$

   其中$\epsilon$是一个用户给定的极小值。

   而如果你使用Gino的终止条件，则无需做任何调整。

3. 在做Johnson距离子算法时，可能会由于反复的浮点数计算导致所有子集中$\Delta(Y_s) \gt 0, \Delta_i(Y_s) \gt 0,\Delta_j(Y_s \cup \{y_j\}) < 0$的判断失效，从而导致算法异常退出。原始论文解决的方法是采用一个兜底程序（Backup Procedure）：暴力遍历所有子集，找到满足$\Delta(Y_s) \gt 0, \Delta_i(Y_s) \gt 0$的距离原点的最近点作为返回值。这总是可以找到的（比如只含有一个点的集合）。这个方法叫Johnson Robust。

   但Gino在他的论文中说，他发现每次触发兜底程序返回的$v(K)$，几乎就是上一帧找到的$v_k$。所以他建议直接就返回上一帧的$v_k$不要再跑这个笨重的兜底程序。

   或者直接报错（但在我个人的实践中，这种情况在刚体不旋转的情况下还是较为常见的，所以我不建议直接报错）

4. 病态情况(ill-condition)：当大小相差几个数量级的多面体彼此非常接近的时候，这些对象的CSO可能产生及其狭长的面从而导致算法无限循环（详见Gino论文）。这时，新算出的$\vec{w_k}$其实是Simplex中已有的点。这样会导致算法死循环。

   解决方法就是每次都判断$\vec{w_k} \in W_{k-1} \cup \{\vec{w_{k-1}}\}$是否成立。如果成立说明遇到了病态情况，那么算法直接结束走兜底程序。

利用GJK进行相交测试

上面说了如何利用GJK计算得到两物体之间的距离已经最近点对。接下来再上述算法的基础上稍加改动即可利用GJK进行相交测试。此方法记录在GinoGJK论文²中。

只要$|\vec{v_k}|$的下界$\delta_k \gt 0$，那么显然两物体就是相离的。否则就是相交/相贴的。

等价于：

并且这里的$\vec{v_k}$就是分离轴。所以此时算法就变成：

注意这里的$\vec{v}$不再是CSO上的任意顶点的方向，而是任意方向。这很有利于帧间一致性。

帧间一致性(Frame coherence)/热启动(Warm start)

指实时模拟中相邻两帧之间物体只移动/旋转了一点点，几何配置变化很小——所以上一帧的结果（分离轴、支撑点、最近特征等）在本帧大概率仍然有效或非常接近，可以作为本帧迭代的热启动起点。

在游戏物理引擎中也叫做热启动。

GinoGJK中就提出了帧间一致性：当Polytope使用爬山法构造时（爬山法详见“从0开始制作游戏物理引擎（二）”），可以利用上一帧最后找支撑点的方向作为下一帧的开始方向查找，一般会大大减少爬山法搜索的时长。

而在使用GJK进行相交测试时，由于$\vec{v}$的初值可以是任意值。那么我们可以利用上一帧找到的分离轴作为方向来加速查找。这样也能加快GJK的执行。

SOLID3中的GJK实现

接下来阅读Gino的GJK实现³。主要是intersect和closest_points两个函数。其他的函数需要EPA算法支持。

着眼于closest_points函数。intersect函数大体上差不多。

剔除多余的代码，intersect的大体流程如下：

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91

// max_dist2 表示感兴趣的距离的平方，超过这个距离就不给精确值或直接返回函数了。这个是用于空间划分技术中过滤过于远的物体的。
MT_Scalar closest_points(const DT_Convex& a, const DT_Convex& b, MT_Scalar max_dist2,
                         MT_Point3& pa, MT_Point3& pb) 
{
    // v初始值是0，表示选取a, b中的第一个点（所有点的SupportMapping都是0）。
	MT_Vector3 v(MT_Scalar(0.0), MT_Scalar(0.0), MT_Scalar(0.0));
    DT_GJK gjk;
	MT_Scalar dist2 = MT_INFINITY;

    do
	{
        // 计算SupportMapping
		MT_Point3  p = a.support(-v);	
		MT_Point3  q = b.support(v);
        // 得到Minkowski差
		MT_Vector3 w = p - q; 

		MT_Scalar delta = v.dot(w);
        
        // 两物体相离
        // delta > MT_Scalar(0)是“利用GJK进行相交测试”中的下界>0的情况
        // 而后半部分则是下界超出max_dist2的情况。
        // 这里其实是 delta * delta / dist2 > max_dist2，即下界v·w/|v|^2 > 感兴趣距离的平方
		if (delta > MT_Scalar(0.0) && delta * delta > dist2 * max_dist2) 
		{
            // 直接返回无效值表示根本不相交
			return MT_INFINITY;
		}

        // ill-condition判断，或上界-下界的差小于给定值（|v|^2 - w·v <= e|v|^2)
		if (gjk.inSimplex(w) || dist2 - delta <= dist2 * DT_Accuracy::rel_error2) 
		{
            break;
		}

        // 将新点加入Simplex
		gjk.addVertex(w, p, q);
        // 如果GJK此时仿射相关，那直接退出
        if (gjk.isAffinelyDependent())
        {
            break;
        }

        // Johnson距离子算法，在当前Simplex中找到距离原点的最近点v
        // 这里v是输出参数不参与计算
        if (!gjk.closest(v)) 
		{
            break;
        }

        // 用于安全退出
#ifdef SAFE_EXIT
		MT_Scalar prev_dist2 = dist2;
#endif

		dist2 = v.length2();

#ifdef SAFE_EXIT
        // 这里是Gino的判断浮点数舍入的手段：用之前的距离平方-当前距离平方
        // 因为GJK确保dist2每一步下是单调递减的。但存在极端情况，每次都递减一点点，那么最后可能由于舍入误差
        // 导致在某个步骤反而变成递增的了。这就可能导致算法错误。
        // 这里这个式子就是当递减量过小时，直接启用备用程序返回一个v和dist2。
        // 这里MT_EPSILON就是std::numeric_traits<float>::epsilon()是机器浮点数精度。
		if (prev_dist2 - dist2 <= MT_EPSILON * prev_dist2) 
		{
            // 为什么这里出错就要调用backup程序而之前的不需要？因为之前的计算没有污染上一帧留下来的v。
            // 而这里，由于使用gjk.closest(v)计算过v了，此时这个v可能并不可靠，所以必须跑backup程序。
            
            // backup程序即原始GJK论文中说的那个backup程序
            gjk.backup_closest(v);
            dist2 = v.length2();
			break;
		}
#endif
    }
    // 退出条件：GJK中Simplex存在四个点，或者dist2是否等于0
    // 这里后半部分是 |v|^2 <= e·w^2 的判断。这里gjk.maxVertex()返回的是w^2
    while (!gjk.fullSimplex() && dist2 > DT_Accuracy::tol_error * gjk.maxVertex()); 
    
    // Simplex中必须存在点
	assert(!gjk.emptySimplex());
	
    // 如果距离在感兴趣距离内，那我们计算最近点
	if (dist2 <= max_dist2)
	{
		gjk.compute_points(pa, pb);
	}
	
    // 算法退出后，总是用最近后算得的最近点的距离(dist2)作为返回值，就算是算法出错也用这个兜底
	return dist2;
}

而GJK结构体中就是一些和论文一模一样的GJK算法。不再赘述。