碰撞检测
Mujoco 根据模型的几何属性来进行碰撞检测。如果两个刚体之间的距离小于它们的几何边界，就认为发生了碰撞。
mjData.contact将以一个列表的形式记录发生碰撞的刚体对。碰撞检测是构建约束的雅可比矩阵求解约束力的重要参考。
碰撞检测的需求描述很简单，但是如果暴力穷举出所有可能发生碰撞的刚体对，将是一个\(O(n^2)\)计算复杂度的任务。
为了提高计算效率，Mujoco 先通过生成 + 过滤(Generation + filtering) 的方式找出潜在可能碰撞的刚体对，再进行详细的碰撞检查，极大的缩小了搜索空间。
本文我们来分析碰撞检测的入口函数 mj_collision，从总体上感受一下碰撞检测的流程以及相关的数据结构。在后续的章节中，研究刚体的碰撞检测细节， 可能也会简单讨论一下柔体的检测，但更详细的内容将有柔体模型作更深入的分析。
1. 官方文档
在 Mujoco 中，碰撞检测是对几何体(geoms)的操作，它们与刚体固连在一起。如果一个模型有 \(n\) 个几何体，那么要穷举所有的碰撞可能，则有 \(n(n-1)/2\) 个几何体对需要检查。 对于大型的系统来说，这将是一个十分耗时的工作。幸运的是，有很多几何体是不可能发生碰撞的，因此用户可以在建模的时候就把它们排除掉。 另外，还可以先用一些比较粗糙的算法压缩搜索空间。Mujoco 判定模型中哪些几何体对会发生碰撞的过程可以分为两个生成(Generation)和过滤(Filtering)两个阶段。
在生成阶段，Mujoco 首先从两个数据源中生成一个可能碰撞的候选列表。这两数据源分别是， 由用户在MJCF文件中通过标签 pair 显式声明需要碰撞检查的几何体对，以及动态生成的潜在碰撞对。 Mujoco 采用的是一种基于扫描剪枝(Sweep-And-Prune)算法的宽相位(Broad-Phase)碰撞检测技术来生成候选碰撞对。对于每个几何体，都可以用一个与xyz三轴平行的立方体包围起来， 也就是所谓的轴对齐包围盒子(Axis-Aligned Bounding Box, AABB)。SweepAndPrune 则分别在三个轴上检查这些包围盒子是否有重叠(sweep)，那些没有重叠的盒子对应的几何体是一定不会发生碰撞的(prune)。
通过扫描剪枝算法排除了一些不可能碰撞的检查之后，Mujoco 会根据 MJCF 文件中的标签 exclude 再剔除那些由用户显式声明的不参与检测的几何体对。此时的搜索空间已经比原来的 \(n(n-1)/2\) 小很多了，MuJoCo 还会进一步用四个过滤器(filter)筛选一遍。
- 候选碰撞的两个几何体必须能够找到一个碰撞函数来完成后续的碰撞检查工作，提供更详细的碰撞信息。一般情况下这个条件是能够满足的，但也有一些例外， 比如 MuJoCo 不支持平面与平面之间的碰撞。此外，有时用户还会用空指针覆盖掉碰撞函数表的特定项，来屏蔽特定类型的几何体检查。
- 再通过边界球(Bounding Sphere)，考虑碰撞边界，再做一次检测。如果其中有一个几何体是个平面，那么将进行平面-球体(plan-sphere)检测。
- 进行同体检测，即候选碰撞的两个几何体一定不能属于同一个刚体。如果多个刚体被焊接在一起，即它们之间没有带自由度的关节连接，将把这些刚体看做一个。 此外如果两个几何体所属的刚体具有父子关系也会从碰撞列表中剔除。
- 第四种滤波器要求候选几何体是兼容的，这是源自ODE 的一个特性。默认情况下都是通过的，这里我们暂不讨论它。
在完成上述筛选之后，MuJoCo 将根据几何类型进行详细的碰撞检查，支持的基础类型有：平面、球体、胶囊体、圆柱体、椭圆体、长方体等。也支持三角网格和高度场。 详细的碰撞检查假设考察的几何体都是凸的。关于这个假设，所有的基础几何体都是凸的，而高度场虽然不是凸的，但在检查时会将其看做是一堆三棱锥的集合。 只有用户定义的三角网格有可能是非凸的，此时将用先用 qhull 计算三角网格的凸包(convex hulls)， 再通过 libccd 实现的 Minkovski Portal Refinement (MPR) 算法来计算详细的碰撞信息。
2. 总体流程解析
在 engine_collision_driver.c 中提供了一个总的碰撞检测入口函数 mj_collision， 其主要的检测逻辑大体上通过调用 mj_broadphase 和 mj_collideGeoms 完成。其中 mj_broadphase 根据轴对齐包围盒子用 SweepAndPrune 算法对可能碰撞的几何体进行一遍粗筛， mj_collideGeoms 会用到上述的过滤器再筛选一遍，同时会根据几何体的类型选择碰撞函数生成详细的碰撞信息。下面我们来详细看一下这三个函数的实现。
2.1. mj_collision
下面是函数 mj_collision 的代码片段，它也是一个 MJAPI 的函数，有两个输入分别是仿真模型 mjModel 对象 m 和 仿真数据 mjData 对象 d。 其中局部变量 npair 和 nexclude 分别是用户在 MJCF 文件中显式声明的是否参与检测的几何体对数量。nbody 和 nbodyflex 则分别是模型中的刚体和刚体+柔体的数量。 为简单起见，这里我们只关注刚体的部分。关于柔体的检测挖完柔体模型之后再来详细研究。
void mj_collision(const mjModel* m, mjData* d) {
int nexclude = m->nexclude, npair = m->npair, nbody = m->nbody;
int nbodyflex = m->nbody + m->nflex;
在 mj_forward 求解正动力学问题的阶段，进行碰撞检测的目的是要建立接触约束方程的。如果系统配置关闭了约束和接触约束，也就没必要进行碰撞检测了。 模型对象 m 的字段 nconmax 则显式配置了考察的最大碰撞数量，若为零则表示不进行碰撞检测。或者几何体的数量少于 2，那么就不存在碰撞的事情。 所以下面的代码片段会检出这些条件直接退出。
// 省略清空碰撞列表和可视化相关代码
if (mjDISABLED(mjDSBL_CONSTRAINT) || mjDISABLED(mjDSBL_CONTACT) || m->nconmax == 0 || nbodyflex < 2)
return;
mj_markStack(d);
接下来的代码片段根据几何体的数量，计算了一个最大可能碰撞的数量，并通过 mj_stackAllocInt 申请了一段栈内存用来记录候选碰撞对儿。 当这一切都准备好之后，就会调用 mj_broadphase 进行一次粗筛，将可能的碰撞对添写到 broadphasepair 中。然后在一个 for 循环中遍历，进行详细检查。
int nmaxpairs = (nbodyflex*(nbodyflex - 1))/2;
int* broadphasepair = mj_stackAllocInt(d, nmaxpairs);
int nbfpair = mj_broadphase(m, d, broadphasepair, nmaxpairs);
unsigned int last_signature = -1;
int pairadr = 0;
for (int i=0; i < nbfpair; i++) {
int bf1 = (broadphasepair[i] >> 16) & 0xFFFF;
int bf2 = broadphasepair[i] & 0xFFFF;
unsigned int signature = (bf1 << 16) + bf2;
if (signature == last_signature)
continue;
last_signature = signature;
函数 mj_broadphase 输出的 broadphasepair 实际上是一个长度为 nbfpair 的32位整形数组，并且已经从小到大排序了。该数组中的每一个元素都记录着一个碰撞对， 高低16位分别对应着候选碰撞的两个几何体编号，编号小的在高16位上。我们将这种编码称为一个签名(signature)。
在左侧的 for 循环片段中，局部变量 bf1 和 bf2 分别是两个可能碰撞的几何体编号。在遍历 broadphasepair 之前， 先定义了一个局部变量 last_signature 来记录最近一次考察的候选碰撞对。因为 mj_broadphase 检出的候选集中可能有重复， 所以需要第 17 到 19 行将它们筛除。
除此之外，还定义了一个局部变量 pairadr。正如我们在前面对官方文档的解读，需要碰撞检查的候选列表的数据源有两个。一个是刚刚通过 mj_broadphase 动态生成的， 另一个则是用户在 MJCF 文件中通过标签 pair 显式声明的。这个 pairadr 就是用来遍历 pair 标签的索引。下面的代码片段中，通过一个 while 循环将用户定义在 pair 中的候选对穿插在 broadphasepair 中间进行详细检查。其中 23 行的条件语句和局部变量 merged 用于标记重复的候选对。
int merged = 0; int startadr = pairadr;
if (npair) {
while (pairadr < npair && m->pair_signature[pairadr] <= signature) {
if (m->pair_signature[pairadr] == signature)
merged = 1;
mj_collideGeoms(m, d, pairadr++, -1);
} }
if (!canCollide2(m, bf1, bf2)) {
continue;
}
接下来左侧的代码片段，通过函数 canCollide2 对候选几何体进行兼容性检查，也就是官方文档中提到的过滤器 4。该函数只是从模型对象中查询出几何体的 contype 和 conaffinity， 然后套用一下兼容性公式。
然后，在下面的代码片段中，根据签名检查候选碰撞对是否被用户显式的写在了 exclude 标签中，若是则跳过。
int exadr = 0;
if (nexclude) {
while (exadr < nexclude && m->exclude_signature[exadr] < signature)
exadr++;
if (exadr < nexclude && m->exclude_signature[exadr] == signature)
continue;
}
下面这段代码先根据 id 将几何体区分为刚体或柔体，用 isbodyX 来表示。然后从模型对象中查出几何体的层次空间分割(Bounding Volume Hierachy, BVH)， 它以一种树状的结构把几何体换分到若干个包围框中。在后面的分析中会看到它是 midphase 阶段通过函数 mj_collideTree 加速碰撞检查的重要数据结构。 对于刚体，有时用户可能会绑定多个几何体，所以还有局部变量 geomadrX 记录几何体的地址。
int isbody1 = (bf1 < nbody);
int isbody2 = (bf2 < nbody);
int bvh1 = (isbody1 ? m->body_bvhadr[bf1] : m->flex_bvhadr[bf1-nbody]);
int bvh2 = (isbody2 ? m->body_bvhadr[bf2] : m->flex_bvhadr[bf2-nbody]);
int geomadr1 = (isbody1 ? m->body_geomadr[bf1] : -1);
int geomadr2 = (isbody2 ? m->body_geomadr[bf2] : -1);
接下来就要根据上述这些几何体的信息，完成详细碰撞信息的检查。如果候选对是两个刚体，并且都只有一个几何体那么直接调用函数 mj_collideGeomPair 完成计算。 该函数就是根据局部变量 merged 来判定是否调用函数 mj_collideGeoms。如果刚刚处理 pair 时已经计算过了，就不需要再调用了。
if (isbody1 && isbody2 && m->body_geomnum[bf1] == 1 && m->body_geomnum[bf2] == 1)
mj_collideGeomPair(m, d, geomadr1, geomadr2, merged, startadr, pairadr);
如果没有关闭 midphase 检查，并且 bvhX 都查出了 BVH 的数据结构，就会调用函数 mj_collideTree 完成计算。 关于 BVH 我们将有专门的文章介绍它。
else if (!mjDISABLED(mjDSBL_MIDPHASE) && bvh1 >= 0 && bvh2 >= 0) {
int ncon_before = d->ncon;
mj_collideTree(m, d, bf1, bf2, merged, startadr, pairadr);
int ncon_after = d->ncon;
mjQUICKSORT(d->contact + ncon_before, ncon_after - ncon_before,
sizeof(mjContact), contactcompare, (void*) m);
}
如果候选对是两个刚体，并且绑定了不只一个几何体，那么将逐个调用函数 mj_collideGeomPair 完成检查。这里我们暂时不考虑柔体相关的碰撞。
else {
int geomadr_end1 = geomadr1 + m->body_geomnum[bf1];
int geomadr_end2 = geomadr2 + m->body_geomnum[bf2];
if (isbody1 && isbody2)
for (int g1 = geomadr1; g1 < geomadr_end1; g1++)
for (int g2 = geomadr2; g2 < geomadr_end2; g2++)
mj_collideGeomPair(m, d, g1, g2, merged, startadr, pairadr);
// 省略其他与柔体相关的代码
} }
最后，完成上述的遍历之后，再把剩下用户声明在 pair 标签里的候选对检查一遍。然后对柔体进行同体检查，就可以释放临时栈空间退出了。
if (npair)
while (pairadr < npair)
mj_collideGeoms(m, d, pairadr++, -1);
// 省略柔体的同体检测
mj_freeStack(d);
}
2.2. mj_broadphase
函数 mj_broadphase 主要是根据应用 Sweep-And-Prune(SAP) 算法对系统中的几何体进行一遍粗筛。 根据官方文档的说法，Mujoco 对该算法做了一点改动。 它在做检测之前，会先对所有几何体的中心位置的协方差矩阵，进行特征值分解，以特征向量的方向构建正交坐标系。在该坐标系下，将最大化几何体的分布，降低错检的概率。 除此之外，该函数还会应用前面提到的过滤器 3 对考察的几何体对进行同体检测。
下面是该函数的代码片段，它有四个参数，其中 m 和 d 分别是仿真模型和数据对象。bfpair 是一段长度为 maxpair 的缓存数组，将用来输出检出的候选碰撞对。 该函数的返回值则是检出的候选对数量。函数体一开始还定义了一堆局部变量，查询模型中有多少刚体、几何体、柔体，以及用于求解协方差矩阵和特征向量的数组。
int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair) {
int npair = 0, nbody = m->nbody, ngeom = m->ngeom;
int nvert = m->nflexvert, nflex = m->nflex, nbodyflex = m->nbody + m->nflex;
int dsbl_filterparent = mjDISABLED(mjDSBL_FILTERPARENT);
mjtNum cov[9], cen[3], eigval[3], frame[9], quat[4];
下面的这个 for 循环写的很长，简单来说就是，如果一个几何体跟 worldbody 或者一个平面焊接在一起，那么所有的几何体都需要检查与它的碰撞关系。 在这段 for 循环中还调用了 canCollide 和 filterBodyPair 两个函数。其中 canCollide 是用来判定一个几何体时候可能与其他几何体发生碰撞，其实它对应的是上述的第4个过滤器。 如果一个几何体的 contype 和 conaffinity 都是 0，那么对于其它任何几何体而言，过滤器4的兼容性条件就一定不成立的。函数 filterBodyPair 则是用来进行同体检测的。
// init with pairs involving always-colliding bodies
for (int b1=0; b1 < nbody; b1++) {
if (!canCollide(m, b1)) continue;
// b1 是与 worldbody 或者平面焊接在一起的几何体
if ((b1 == 0 && m->body_geomnum[b1] > 0) || (m->body_weldid[b1] == 0 && hasPlane(m, b1))) {
for (int b2=0; b2 < nbody; b2++) {
if (!canCollide(m, b2)) continue;
int weld2 = m->body_weldid[b2];
int parent_weld2 = m->body_weldid[m->body_parentid[weld2]];
// 进行同体检测
if (filterBodyPair(0, 0, weld2, parent_weld2, dsbl_filterparent))
continue;
add_pair(m, b1, b2, &npair, bfpair, maxpair);
}
for (int f=0; f < nflex; f++)
add_pair(m, b1, nbody+f, &npair, bfpair, maxpair);
} }
// find center of non-world geoms and flex vertices; return if none
int cnt = 0;
mju_zero3(cen); // 计算几何体中心
for (int i=0; i < ngeom; i++) {
if (m->geom_bodyid[i]) {
mju_addTo3(cen, d->geom_xpos+3*i);
cnt++;
} }
for (int i=0; i < nvert; i++) {
if (m->flex_vertbodyid[i]) {
mju_addTo3(cen, d->flexvert_xpos+3*i);
cnt++;
} }
if (cnt == 0)
return npair;
mju_scl3(cen, cen, 1.0/cnt);
mju_zero(cov, 9); // 计算协方差
for (int i=0; i < ngeom; i++) {
if (m->geom_bodyid[i])
updateCov(cov, d->geom_xpos+3*i, cen);
}
for (int i=0; i < nvert; i++) {
if (m->flex_vertbodyid[i])
updateCov(cov, d->flexvert_xpos+3*i, cen);
}
mju_scl(cov, cov, 1.0/cnt, 9);
mju_eig3(eigval, frame, quat, cov);
mj_markStack(d);
int* bfid = mj_stackAllocInt(d, nbodyflex);
int ncollide = 0;
for (int i=1; i < nbodyflex; i++)
if (canCollide(m, i))
bfid[ncollide++] = i;
if (ncollide < 2) goto endbroad;
mjtNum* aamm = mj_stackAllocNum(d, 6*ncollide);
for (int i=0; i < ncollide; i++)
makeAAMM(m, d, aamm+6*i, bfid[i], frame);
接下来计算所有非 worldbody 的几何体的中心、协方差。重点是上面第 51 行对协方差矩阵的特征值分解，数组 frame 将记录分解后的特征向量。它将作为后续构建轴对齐包围盒子的参考轴。
左侧的代码片段是在计算变更坐标系之后的轴对齐包围盒子。临时分配的数组 bfid 用于记录那些不会被兼容性检查过滤器屏蔽掉的几何体，ncollide 则统计这些几何体的数量。
如果通过兼容性检查的几何体少于 2 个，那么意味着不会有几何体发生碰撞的，所以第 58 行通过 goto 语句提前结束。
上面片段中最后的 for 循环通过函数 makeAAMM 给每个通过兼容性检查的几何体算了一个轴对齐包围盒子，估计是为了强调坐标系的变换，所以这里用 AAMM 表示包围盒子，而不是AABB。 剩下的重点就是调用函数 mj_SAP 粗筛除候选碰撞对。我们将在扫描剪枝算法 Sweep-And-Prune中详细分析这两个函数。
int maxsappair = ncollide*(ncollide-1)/2;
int* sappair = mj_stackAllocInt(d, maxsappair);
int nsappair = mj_SAP(d, aamm, ncollide, 0, sappair, maxsappair);
if (nsappair < 0) mjERROR("SAP failed");
剩下的事情就是在一个 for 循环里面遍历一下检出的候选对做个同体检测。最后就对检出的候选对排序一下就可以输出了。
for (int i=0; i < nsappair; i++) {
int bf1 = bfid[sappair[i] >> 16];
int bf2 = bfid[sappair[i] & 0xFFFF];
if (bf1 < nbody && bf2 < nbody) {
int weld1 = m->body_weldid[bf1]; int weld2 = m->body_weldid[bf2];
int parent_weld1 = m->body_weldid[m->body_parentid[weld1]];
int parent_weld2 = m->body_weldid[m->body_parentid[weld2]];
if (filterBodyPair(weld1, parent_weld1, weld2, parent_weld2, dsbl_filterparent))
continue;
}
add_pair(m, bf1, bf2, &npair, bfpair, maxpair);
}
endbroad:
if (npair)
mjQUICKSORT(bfpair, npair, sizeof(int), uintcompare, 0);
mj_freeStack(d);
return npair;
}
2.3. mj_collideGeoms
函数 mj_collideGeoms 主要用来对 broad-phase 阶段检出的候选碰撞对通过过滤器筛选一遍，同时根据几何体类型选择碰撞函数生成详细的碰撞信息。下面是该函数的代码片段， 它有四个参数，其中 m,d 分别是仿真模型和数据对象。g1 和 g2 则是候选对的两个几何体索引。
void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2) {
// 省略一些局部变量的声明
int ipair = (g2 < 0 ? g1 : -1);
if (ipair >= 0) {
g1 = m->pair_geom1[ipair];
g2 = m->pair_geom2[ipair];
}
这里 MuJoCo 通过 g2 的传参来区分碰撞对是用户显式声明的还是由 mj_broadphase 检出的。如果是用户声明的，那么 g2 为 -1，g1 将记录声明的 pair 索引。 所以左侧代码会用局部变量 ipair 标记数据来源，并从模型对象中查询声明的碰撞对。一视同仁不好吗？为啥要区别对待。
// 省略按照几何体类型排序的相关代码
type1 = m->geom_type[g1];
type2 = m->geom_type[g2];
if (!mjCOLLISIONFUNC[type1][type2])
return;
上面的代码片段先获取了几何体的类型，然后应用文档中提到的过滤器1 检查是否有对应的碰撞函数。这个 mjCOLLISIONFUNC 是定义在 engine_collision_driver.c 中的一个二维数组， 如上面右图所示，它实际上是一个函数指针的映射矩阵，如果 type1 行 type2 列的元素为 0 说明没有相关的检测函数。我们可以在运行时将某个特定的元素置零，来屏蔽特定类型的碰撞检测。
如果碰撞对不是用户声明的，就会执行下面的兼容性检查，若用户注册了回调函数 mjcb_contactfilter 将用该回调函数替换兼容性检查。 接着还要调用函数 mj_filterSphere 对两个几何体进行边界球(Bounding Sphere)检测，如果其中有一个几何体是个平面，那么将进行平面-球体(plan-sphere)检测。
if (ipair < 0) {
if (mjcb_contactfilter) {
if (mjcb_contactfilter(m, d, g1, g2))
return;
} else if (filterBitmask(m->geom_contype[g1], m->geom_conaffinity[g1],
m->geom_contype[g2], m->geom_conaffinity[g2])) {
return;
} }
if (ipair < 0)
margin = mj_assignMargin(m, mju_max(m->geom_margin[g1], m->geom_margin[g2]));
else
margin = mj_assignMargin(m, m->pair_margin[ipair]);
// Filter 2: 边界球过滤器
if (mj_filterSphere(m, d, g1, g2, margin))
return;
由于同体检测已经在 mj_broadphase 中执行了，所以经过上述三个过滤器的筛选，候选对就很可能是真的碰撞了。接下来，分配了一段 mjContact 类型的数组用于记录详细的碰撞信息， 该数据结构的字段还比较多，我们将在用到时详细研究它们。最后调用 mjCOLLISIONFUNC 中检测函数生成详细碰撞信息。
mjContact* con = (mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * mjMAXCONPAIR, _Alignof(mjContact));
if (!con) {
mj_warning(d, mjWARN_CONTACTFULL, d->ncon);
return;
}
// call collision detector to generate contacts
num = mjCOLLISIONFUNC[type1][type2](m, d, con, g1, g2, margin);
// 省略大段碰撞信息整理的代码
}