扫描剪枝算法 —— Sweep And Prune
当仿真系统中的物体很多时，暴力的两两对比，穷举出所有的可能，显然是一个十分低效的事情。对于一些实时性要求比较高的场合，很有可能这种方式是不可接受的。 虽然我们很难快速判断两个物体是否发生了碰撞，但想快速判定它们不可能碰撞还是一件比较简单的事情。 扫描剪枝算法用，一种被称为轴对齐包围盒子(Axis Align Bounding Box)，的立方体来代替复杂的几何形状，如果两个立方体没有相交，那么对应的物体就一定没有碰撞。
扫描剪枝算法之所以选用轴对齐包围盒子，是因为这样它可以将 3D 的物体投影到一个 1D 的数轴上，充分利用数据的排序关系完成筛选。 如果物体的空间分布比较均匀，那么筛选出不可能碰撞的几何体几率就会大一些。所以 MuJoCo 对朴素的扫描剪枝算法做了一些改进，先通过主成分分析，找到能最大化分布的坐标轴， 再通过扫描剪枝算法完成 Broad-Phase 的粗检。
本文我们从 AABB 模型开始介绍扫描剪枝算法的基本思想，最后分析 MuJoCo 中的实现函数 mj_SAP。
1. 轴对齐包围盒子 AABB
所谓的轴对齐包围盒子，就是一个各边与坐标轴平行，包围着几何体的最小外接立方体。下面就是一个 AABB 的示意图，为简单起见，这里用 2D 平面的矩形来代替。 之所以选择立方体或者矩形，是因为判定它们是否相交的方法很简单。
 |
在上图中，我们可以用区间 \([sx0, ex0]\) 和 \([sy0, ey0]\) 分别表示五角星的包围盒子在 \(x\) 轴和 \(y\)轴上的区间，五边形则是\([sx1, ex1]\) 和 \([sy1, ey1]\)。 左图中两个图形的包围盒子相交，我们可以直观看到 \(x\) 轴和 \(y\) 的区间都存在交集，即 \(\emptyset \neq [sx0, ex0] \cap [sx1, ex1]\) 并且 \(\emptyset \neq [sy0, ey0] \cap [sy1, ey1]\)。若有一个轴的区间不存在交集，意味着存在一条与该轴平行的平面/直线，可以将两个包围盒子完全分开，所以他们不可能相交。
判定两个区间不存在交集的算法也很简单，我们只需要检查一个区间的起点，是否大于另一个区间的终点即可，即：
sx0 > ex1 || sx1 > ex0 || sy0 > ex1 || sy1 > ey0 || sz0 > ez1 || sz1 > ez0
对于一个 2D 的图形，我们只要保证前面 4 个关于 x,y 轴的不等式有一个成立即可，对于 3D 的图形还可以再检查后两个关于 z 轴的不等式。 显然这种求交的算法复杂度只有 \(O(1)\)。上面左图中，我们特意画了一个包围盒子相交，但是几何体并未碰撞的例子。主要想强调一下 Broad-Phase 的基本思想，筛除那些明显不可能发生碰撞的几何体， 而不是找出碰撞。
2. 扫描剪枝算法
虽然判定两个 AABB 盒子不相交的计算很少，但是完全穷举下来时间复杂度还是 \(O(n^2)\)。实际上根据几何体的空间分布，很多几何体之间都不需要检查 AABB 是否相交。 比如下图中的这个场景，其中一共有 6 个不同的几何体，我们可以一眼看出来其中有些几何体是不可能相交的。扫描剪枝算法，就可以帮助我们对 AABB 盒子进行一些筛选， 进一步减少计算量。
 |
扫描剪枝算法的关键在于一个扫描队列。它将所有 AABB 盒子投影到一个指定的坐标轴上，然后从小到大的扫描投影点。遇到一个 AABB 的起点就将之添加进队列， 遇到终点旧将对应对的几何体从队列中移除。只有那些同时出现在队列里的 AABB 有可能相交。大体上来说，该算法的流程如下:
指定一个坐标轴，将所有 AABB 盒子投影到该轴上，得到对应的起点和终点。比如上图中我们选择了 x 轴。
将所有的起点和终点放到一个数组 sortbuf 中排序。
遍历 sortbuf 完成如下扫描任务:
若遇到第 i 个 AABB 盒子的起点 s(i)，则:
检查 AABB(i) 与扫描队列 checkbuf 中其它包围盒子的相交关系
将 i 加入 checkbuf 中
若遇到第 i 个 AABB 盒子的终点 e(i)，则:
将 i 从 checkbuf 移除
该算法最耗时的地方在于对起止投影点的排序，一般排序算法的时间复杂度为 \(O(n \log n)\)，这个复杂度相比于穷举的 \(O(n^2)\) 而言要快很多了。 上图中我们只需要检查 0-1, 3-4, 3-5, 4-5 四对 AABB 盒子就可以了，而穷举法需要检查 15 对。
坐标轴的选取，对算法的性能是很大影响的。 上图中如果我们选择 y 轴投影，那么我们几乎需要检查所有的组合情况。所以，几何体在投影轴上的分布越分散，算法的性能就越好。 在上一节中我们了解到，MuJoCo 在调用 mj_SAP 进行扫描剪枝之前， 先通过主成分分析法(PCA)，对几何体坐标的协方差矩阵进行特征值分解，找到使得几何体分布最分散的坐标系。其背后的设计思想就来源于此。
3. MuJoCo 的实现 mj_SAP
下面是 MuJoCo 实现的扫描剪枝算法函数 mj_SAP 的代码片段。它有六个参数。其中d 是仿真数据对象。数组 aamm 中记录了各个几何体的轴对齐盒子， 每个元素对应 6 个数值以 (min[3], max[3]) 的形式记录着各轴的起止点。n 输入几何体数量。axis 选择了算法的投影轴，它只有三个可能得取值 0/1/2 分别对应这个三个坐标轴。 pair 和 maxpair 分别传参输出候选几何对列表的缓存地址和大小。函数返回的是候选几何对的数量，一开始先对输入的参数进行了合法性检查。
static int mj_SAP(mjData* d, const mjtNum* aamm, int n, int axis, int* pair, int maxpair) {
if (n >= 0x10000 || axis < 0 || axis > 2 || maxpair < 1)
return -1;
接下来开辟了一段空间 sortbuf，记录下各个 AABB 对应的几何索引和起止点在 axis 轴上的坐标。
mjtSAP* sortbuf = (mjtSAP*) mj_stackAllocByte(d, 2*n*sizeof(mjtSAP), _Alignof(mjtSAP));
for (int i = 0; i < n; i++) {
sortbuf[2*i ].id_ismax = i; sortbuf[2*i ].value = (float)aamm[6*i + axis];
sortbuf[2*i+1].id_ismax = i + 0x10000; sortbuf[2*i+1].value = (float)aamm[6*i+3+axis];
}
mjQUICKSORT(sortbuf, 2*n, sizeof(mjtSAP), SAPcompare, 0);
因为，对投影轴的扫描和剪枝，已经是在 axis 轴上进行了求交运算，所以我们后续只需要对另外两轴求交，就可以完成两个 AABB 盒子的碰撞检测。 下面的代码片段穷举了 axis 的三种取值，找到另外两个坐标轴。
int axisA, axisB;
if (axis == 0) { axisA = 1; axisB = 2; }
else if (axis == 1) { axisA = 0; axisB = 2; }
else { axisA = 0; axisB = 1; }
在开始扫描 sortbuf 之前，我们开辟了一段 activebuf 的空间，作为扫描队列。局部变量 cnt 和 npair 分别表示扫描队列中的几何体数量， 以及相交的几何对数量。
mjtSAP* activebuf = (mjtSAP*) mj_stackAllocByte(d, 2*n*sizeof(mjtSAP), _Alignof(mjtSAP));
int cnt = 0; int npair = 0;
扫描过程中，第 17 行通过 id_ismax 判定是否遇到了投影起始点。并在 18 行的 for 循环中，先通过另外两轴确认 id2 没有与扫描队列中的 AABB 盒子相交。
for (int i=0; i < 2*n; i++) {
if (!(sortbuf[i].id_ismax & 0x10000)) {
for (int j=0; j < cnt; j++) {
int id1 = activebuf[j].id_ismax; int id2 = sortbuf[i].id_ismax;
if (aamm[6*id1+axisA] > aamm[6*id2+axisA+3] || aamm[6*id1+axisB] > aamm[6*id2+axisB+3] ||
aamm[6*id2+axisA] > aamm[6*id1+axisA+3] || aamm[6*id2+axisB] > aamm[6*id1+axisB+3])
continue;
确实相交了，就添加到输出候选列表 pair 中。检查完扫描队列之后，将 id2 添加到扫描队列中。
pair[npair++] = (id1<<16) + id2;
if (npair >= maxpair)
return maxpair;
}
activebuf[cnt++] = sortbuf[i];
}
若扫描到投影终止点，将对应几何体从扫描队列中移除。最后返回可能碰撞的几何体对数。
else {
int toremove = sortbuf[i].id_ismax & 0xFFFF;
for (int j=0; j < cnt; j++) {
if (activebuf[j].id_ismax == toremove) {
if (j < cnt-1)
memmove(activebuf+j, activebuf+j+1, sizeof(mjtSAP)*(cnt-1-j));
cnt--; break;
} } } }
return npair;
}