包围体层次结构 —— Bounding Volume Hierarchy
在上一节中，我们研究了扫描剪枝(SAP)算法。它被用来进行 Broad-Phase 阶段的碰撞检测，
检查的粒度是仿真想定中的刚体，对应着 MJCF 文件中的
<body> 标签。实际上一个刚体可能是由若干个几何体构成的。也就是说，
一个 <body> 下面可能有若干个 <geom> 子标签。
因此在 SAP 算法输出的候选碰撞物体对的基础上，我们还需要更近一步的检查构成它们的几何体，才能确认两个刚体是否真的有接触。
这就是后续 Mid-Phase 和 Narrow-Phase 阶段的工作内容。
本文中，我们将详细分析一下 MuJoCo 的 Mid-Phase 阶段所用的包围层次结构(BVH)。我们先介绍 BVH 的基本思想，再结合源码查看 MuJoCo 是如何构建 BVH 的， 最后分析如何使用 BVH 加速碰撞检测。
1. 包围体层次结构
包围体层次结构，Bounding Volume Hierarchy, 简称 BVH，是 Mid-Phase 阶段常见的一种用于加速检测的树状数据结构。
如右图(a)所示，空间中分布着 6 个不同形状的几何体，分别从 0 到 5 编号，它们都有一个 AABB 的包围盒子。 这里我们将他们分为 3 组，0-1 一组用一个能够包含它们的包围盒子 node1 框起来，2 单独一组，3-4-5 一起框在包围盒子 node2 中。在最外面，由包围盒子 node0 将它们都囊括进来。
如此，就构成了一个类似右图(b)的树状结构。最大的那个包围盒子 node0 就是树的根节点，node1 和 node2 是两个中间节点，那些直接包围几何体的盒子就是叶子节点。 做碰撞检查时，我们从根节点开始，采取深度优先的方式，逐级向下查找到叶子节点。
如右图(c)所示，假设此时我们往场景中丢了一个亮黄色的球，它砸到了五角星和五边形。此时我们就可利用 BVH 来检查。首先这个圆球一定与根节点 node0 碰撞了， 然后向下检查 node1 节点也碰撞了，接着向下查找，发现两个叶子节点都有碰撞关系。接着检查 node0 下另外两个分支，发现圆球没有与三角形接触，也没有与中间节点 node2 有碰撞关系。 所以不在对 node2 展开向下搜索了。
上述的过程中，我们一共做了6次检查，好像和穷举法没什么差别。如果场景再复杂一点，BVH 就可以更少的检查次数，输出所有可能碰撞的几何体。关于这个包围体，实际不限于 AABB 的， 这里纯粹是为了作图方便。我们也可以根据需要选择包围球(Sphere)或者定向包围盒子(OBB)，相应的在计算量和检测精度上就要做些取舍。
此外构建 BVH 时，几何体分组方式是比较灵活的。我们可以在建模的时候，根据一些语义关系，对一个复杂的装配体进行划分。比如说，一个车辆的模型，可以按照车身的零件一组， 车门的零件一组、底盘的零件一组，最后装配成的整车作为一个BVH的根节点。也可以根据几何体的位置，对空间进行划分。比如，我们可以沿着一个轴找到一个平面，使得空间中的物体大致平均地分布在平面两侧， 构成两个子节点，再以相同的方式对两个子节点进行划分，直到每个子节点都只包含一个几何体为止。
2. 寻找 BVH
在研究 MuJoCo 进行碰撞检测的总体过程的时候，
我就注意到 mid-phase 阶段的碰撞检测 是可以通过 mjDSBL_MIDPHASE 关闭的，
并且如果候选对的两个刚体/柔体没有定义 BVH 的数据结果的话，也不会进行 mid-phase 检查。当时我想当然的以为这个 BVH 是在 MJCF 文件里面描述的。
但是我在官方文档里面翻了半天也没见到哪里有说明应该怎么给 <body>
标签添加相关描述的。为了这个我纳闷了好几天，后来在代码里发现，MuJoCo 会在编译生成 mjModel 对象时，
对一个<body>下的若干个<geom>子标签，根据空间位置关系构建一个 BVH。
我看函数 mj_collision() 中是通过模型对象的字段 body_bvhadr 和 flex_bvhadr 获取刚体/柔体 BVH 首地址的，如下面的代码片段所示。 我想既然有地方读，就应该有地方写。沿着这个线索我在 MuJoCo 的 src 目录下 grep 了一下 body_bvhadr，发现在 user/user_model.cc 的1500行确实有一个赋值的动作。
int bvh1 = (isbody1 ? m->body_bvhadr[bf1] : m->flex_bvhadr[bf1-nbody]);
int bvh2 = (isbody2 ? m->body_bvhadr[bf2] : m->flex_bvhadr[bf2-nbody]);
这是函数 mjCModel::CopyTree 中的一段代码， 该函数又是在 mjCModel::TryCompile 中调用的。 在模型描述的低级数据结构 mjModel中我们曾介绍过， 函数 mjCModel::TryCompile 是实际完成从高级数据结构 mjCModel 到实际计算所用的低级数据结构 mjModel 的编译工作的。下面的这段代码正在填充模型对象中关于 BVH 的数据。
我们再沿着上图中pb->tree深挖一下。
pb是一个指向类型 mjCBody 对象的指针，在该类型的定义中有一个类型为
mjCBoundingVolumeHierarchy 的成员变量 tree。
下面左侧是 mjCBody 的成员函数 Compile 的代码片段，大意是为每个几何体 <geom> 创建一个包围体，
然后调用 tree 的成员函数 CreateBVH 构建层次结构。
 |
| 图 1. 构造 BVH |
3. 类 mjCBoundingVolumeHierarchy 与 mjCBoundingVolume
现在，我们终于找到了 MuJoCo 中关于 BVH 的数据结构，下面是类 mjCBoundingVolumeHierarchy 的定义片段，这里为排版方便，我们在不改变代码语义的前提下，对其中的一些语句块做些调整。
class mjCBoundingVolumeHierarchy {
private:
std::vector<mjCBoundingVolume> bvh_;
std::string name_;
double ipos_[3];
double iquat_[4];
左侧的这些私有的成员变量，主要是面对高级数据结构 mjCModel 的，在完成编译之后，它们就没有存在的意义了。
实际上那个 name_ 完全没有用，作为私有成员，没有公开的成员函数能够访问到它。
数组 bvh_ 中的元素与 <body> 所包含的 <geom> 几何体一一对应。
在上图1中先通过对象tree调用成员函数 AllocateBoundingVolumes() 开辟内存。然后通过几何体对象的成员函数 SetBoundingVolume() 填充具体的数据。
public:
int nbvh;
std::vector<mjtNum> bvh;
std::vector<int> child;
std::vector<int*> nodeid;
std::vector<int> level;
左侧这些公开的成员变量，面对的是低级数据结构 mjModel。它们将由成员函数 MakeBVH() 根据 bvh_ 构建包围体树的同时填充具体数据。 最终由函数 mjCBody::Compile() 将其中的数据拷贝到低级数据结构 mjModel 中。其中 nbvh 是树中节点的数量，包含根节点、中间节点和叶子节点。 数组 bvh 是长度为 nbvh × 6 的浮点数组，记录各节点的包围盒子的位置和大小。child 是长度为 nbvh × 2 的整形数组，记录各节点的子节点索引。 每个节点都有两个子节点，如果都是 -1 那么它就是叶子节点。数组 nodeid 的长度为 nbvh，记录各节点对应的几何体ID地址，如果是中间节点， 那么一般它不对应实际的几何体，所以通常为 nullptr。数组 level 记录的是节点的深度。
public:
mjCBoundingVolumeHierarchy(); // 构造函数
void CreateBVH(void); // 构造BVH
void Set(mjtNum ipos_element[3], mjtNum iquat_element[4]); // 设置BVH的位置和四元数
void AllocateBoundingVolumes(int nbvh); // 给 bvh_ 开辟内存
void RemoveInactiveVolumes(int nmax); // 移除多余的节点
mjCBoundingVolume* GetBoundingVolume(int id); // 获取 bvh_ 的第 id 个元素
private:
int MakeBVH(std::vector<const mjCBoundingVolume*>& elements, int lev = 0);
};
上面是类 mjCBoundingVolumeHierarchy 中定义的所有成员函数。那些公开的成员函数都比较短，实现的功能基本就是函数名的字面意思。 MakeBVH 是一个递归函数，它具体完成了 BVH 的构造，我们将在下文中详细分析它。
上面左图是类 mjCBoundingVolume 的定义，它记录的主要就是包围盒子(aabb)、位置(pos)、四元数(quat)。右图是几何体对象用来填充 mjCBoundingVolume 对象的实现，就是简单的赋值而已。
4. 构造 BVH
函数 mjCBoundingVolumeHierarchy::MakeBVH 把一个 <body>下的若干个<geom> 构造成一棵 BVH 二叉树。
它是一个递归函数，从根节点开始逐级向下直到叶子节点。每个 BVH 节点的构造过程大致可以总结为如下几个步骤:
- 构建一个包围盒子(AAMM)能够框住输入包围体列表中的所有元素。
- 用AAMM的位置和大小构建 BVH 节点，将其两个子节点赋予初值 -1。
- 找到AAMM中尺寸最大的那个轴，把输入包围体列表中的元素均匀的划分成两组。
- 拿着这两个组，分别递归构建该节点的两个子节点。
- 如果输入的元素为空，直接返回 -1 结束递归。
下面是该函数的代码片段，它有两个输入参数，其中 elements 是构建 BVH 节点的包围体列表，lev 是递归的深度也就是 BVH 节点的深度。
如果输入的列表为空，直接返回 -1 结束递归。如果一个几何体只是用于可视化的，就不需要参与碰撞检测。
所以有局部变量 is_visual 用于标记 elements 是不是所有的元素都对应可视化的几何体。
nelements 记录输入包围体的数量。数组 AAMM 就是该 BVH 节点的包围盒子。qinv 是<body>姿态的共轭四元数，用于表示其姿态的逆。
int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<const mjCBoundingVolume*>& elements, int lev) {
if (elements.empty())
return -1;
bool is_visual = true; int nelements = elements.size();
mjtNum AAMM[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
mjtNum qinv[4] = {iquat_[0], -iquat_[1], -iquat_[2], -iquat_[3]};
在下面的 for 循环中，遍历所有输入的包围体，构建可以框住它们的 AAMM。如果对应的几何体纯粹是可视化的，直接 continue 跳过。若所有的几何体都是纯可视化的，遍历结束之后 is_visual 将保持初值 true， 说明该节点也没有必要参与检测了，将在后面直接返回 -1。
for (int i = 0; i < nelements; i++) {
if (elements[i]->conaffinity==0 && elements[i]->contype==0)
continue;
else
is_visual = false;
mjtNum aamm[6] = {
elements[i]->aabb[0] - elements[i]->aabb[3],
elements[i]->aabb[1] - elements[i]->aabb[4],
elements[i]->aabb[2] - elements[i]->aabb[5],
elements[i]->aabb[0] + elements[i]->aabb[3],
elements[i]->aabb[1] + elements[i]->aabb[4],
elements[i]->aabb[2] + elements[i]->aabb[5]
};
左侧的代码片段中，将原 aabb 格式的包围盒子转换成 aamm 的形式。本身轴对齐包围盒子(Axis Align Bounding Box)是用位置和大小，来表示一个几何体的边界的，也就是这里的 aabb。 AAMM 则是用两个点来表示一个包围盒子，这里我称它们为最小点和最大点。最小点是包围盒子八个点中坐标最小的那个点，即\((min_x, min_y, min_z)\)， 最大点则是最大的那个点，即\((max_x, max_y, max_z)\)。估计这是变量名为 aamm 的原因。
for (int v=0; v < 8; v++) {
mjtNum vert[3], box[3];
vert[0] = (v&1 ? aamm[3] : aamm[0]);
vert[1] = (v&2 ? aamm[4] : aamm[1]);
vert[2] = (v&4 ? aamm[5] : aamm[2]);
// rotate to the body inertial frame if specified
if (elements[i]->quat) {
mju_rotVecQuat(box, vert, elements[i]->quat);
box[0] += elements[i]->pos[0] - ipos_[0];
box[1] += elements[i]->pos[1] - ipos_[1];
box[2] += elements[i]->pos[2] - ipos_[2];
mju_rotVecQuat(vert, box, qinv);
}
AAMM[0] = mjMIN(AAMM[0], vert[0]);
AAMM[1] = mjMIN(AAMM[1], vert[1]);
AAMM[2] = mjMIN(AAMM[2], vert[2]);
AAMM[3] = mjMAX(AAMM[3], vert[0]);
AAMM[4] = mjMAX(AAMM[4], vert[1]);
AAMM[5] = mjMAX(AAMM[5], vert[2]);
}
}
在左侧的代码片段中，又用了一个 for 循环，来遍历包围盒子 aamm 的 8 个顶点坐标，坐标值被保存在局部数组 vert 中。
顶点坐标的计算方法在上面的折叠代码里面，实际就是 3 个坐标轴分别取最大点或者最小点坐标的组合关系。
如果对应的几何体还声明了相对于 <body> 的位置和姿态，那么需要对 aamm 进行旋转平移，转换到<body>的坐标系下。
再然后就是更新整个 BVH 节点的最大点和最小点 AAMM，让它能够框住所有的几何体。
if (is_visual) return nbvh;
for (int i=0; i < 3; i++) {
if (mju_abs(AAMM[i]-AAMM[i+3]) < mjEPS) {
AAMM[i+0] -= mjEPS;
AAMM[i+3] += mjEPS;
} }
完成 AAMM 的计算后，还需要做一些整理的工作。先判定是否为纯可视化几何体，若是则直接返回终止递归。
有时候构造的 AAMM 可能很小，为了保证不漏检碰撞，这里对 AAMM 进行了膨胀，分别给其最大点和最小点坐标分别加上或者减去一个小数(mjEPS = 1E-14)。
在下面左侧的代码片段中，填充面向低级数据结构 mjModel 的数组。先用 index 记录下 nbvh 的值作为该节点的索引，再对 nbvh 自加完成 BVH 节点的计数。 然后给对应的两个子节点索引赋予初值 -1，nodeid 记为 nullptr。 记录节点深度后，在两个 for 循环中把刚刚计算的 AAMM 转换成 AABB 的形式填充到数组 bvh 中。 在右侧的代码片段中，检查输入的几何体数量，若为 1，说明它是一个叶子节点。这时将保持数组 child 对应的索引为 -1，同时获取几何体的 id 地址填充到数组 nodeid 中。 最后直接将节点的索引返回，由其父节点记录到 child 中。
int index = nbvh++;
child.push_back(-1);
child.push_back(-1);
nodeid.push_back(nullptr);
level.push_back(lev);
for (int i=0; i < 3; i++)
bvh.push_back((AAMM[3+i] + AAMM[i]) / 2);
for (int i=0; i < 3; i++)
bvh.push_back((AAMM[3+i] - AAMM[i]) / 2);
// leaf node, return
if (nelements==1) {
for (int i=0; i < 2; i++) {
child[2*index+i] = -1;
}
nodeid[index] = (int*)elements[0]->GetId();
return index;
}
最后，找到 AAMM 中尺寸最大的那个轴，根据坐标关系，将输入数组 elements 均匀的划分成两组。 这里省略了分组方法。如果分组非空就递归向下调用，构建子节点，将递归的返回值填充到数组 child 的对应元素中之后，将自身的索引返回。
std::vector<const mjCBoundingVolume*> left;
std::vector<const mjCBoundingVolume*> right;
// 省略分组相关代码
if (!left.empty())
child[2*index+0] = MakeBVH(left, lev+1);
if (!right.empty())
child[2*index+1] = MakeBVH(right, lev+1);
// 省略一些错误检查
return index;
}
5. 应用 BVH
实际应用 BVH 进行 mid-phase 阶段的碰撞检测是在函数 mj_collideTree 中进行的。 整个检测过程就是一个深度优先的遍历过程。下面是这个函数代码片段，它的输入参数很多，前两个 m 和 d 就是整个仿真过程中的模型和数据对象。 bf1 和 bf2 则是实际需要进行详细检查的刚体/柔体编号。最后的三个参数主要是用来防止重复检查几何体用的。
void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
int merged, int startadr, int pairadr) {
一开始，先构造了一个栈的数据结构。栈中的元素是一个结构体，分别用于记录将要检查的两个 BVH 树的节点索引，如右侧代码所示。 整个深度搜索的过程就是，不断从栈顶中取出一个检测节点对，如果存在碰撞的可能，就对该节点对向下扩展生成新的检测节点对入栈。直到栈空。 在开始遍历之前，先把两个 BVH 树的根节点也就是索引 0 入栈。
// 省略一堆局部变量声明等前置准备工作
mjCollisionTree* stack = mj_stackAllocTree(d, max_stack);
int nstack = 1;
stack[0].node1 = stack[0].node2 = 0;
struct mjCollisionTree_ {
int node1;
int node2;
};
接下来，在一个 while 循环中遍历两棵 BVH 树。检查栈中元素数量 nstack 若为 0 表示栈空，检查结束。在下面的 8,9 行中先将栈顶的数据出栈，nstack 计数自减，node1和node2分别记录两棵树的节点索引。 同时还要检查一下这两个节点是否是叶子节点，分别用 isleaf1 和 isleaf2 标记。之前已经提到叶子节点直接对应一个几何体，这里通过 bvh_nodeid 获取对应的几何体索引。
while (nstack) {
nstack--;
int node1 = stack[nstack].node1; int node2 = stack[nstack].node2;
mjtByte isleaf1 = (child1[2*node1] < 0) && (child1[2*node1+1] < 0);
mjtByte isleaf2 = (child2[2*node2] < 0) && (child2[2*node2+1] < 0);
int nodeid1 = m->bvh_nodeid[bvhadr1 + node1];
int nodeid2 = m->bvh_nodeid[bvhadr2 + node2];
我们现在只关注刚体之间的碰撞检测，柔体相关的内容将在第XXX部分中作更深入的分析。如果遍历到两个叶子节点， 那么就先用边界椭圆做一次粗筛，再用定向包围盒子(OBB)筛一遍。 如果都通过了说明真的有可能碰撞了，就会调用函数 mj_collideGeomPair 进入Narrow-Phase 完成详细的检查。
// 省略合规性检查，同体检测
if (isbody1 && isbody2) {
if (isleaf1 && isleaf2) {
mjtNum maxmargin = mju_max(m->geom_margin[nodeid1], m->geom_margin[nodeid2]);
mjtNum margin = mj_assignMargin(m, maxmargin);
if (!mj_filterSphere(m, d, nodeid1, nodeid2, margin)) {
if (mj_collideOBB(/* 省略传参 */)) {
mj_collideGeomPair(m, d, nodeid1, nodeid2, merged, startadr, pairadr);
// 省略与可视化有关的代码
} }
continue;
}
对于非叶子节点，就用 OBB 筛一遍，如果没碰撞就不再向下展开了。
mjtNum maxmargin = mju_max(m->body_margin[bf1], m->body_margin[bf2]);
mjtNum margin = mj_assignMargin(m, maxmargin);
if (!mj_collideOBB(/* 省略传参 */))
continue;
}
// 下面省略关于柔体的检测
代码运行到这里，就要展开 BVH 节点了。展开逻辑也很简单，如果一个是中间节点一个是叶子节点的情况，就展开那个中间节点。如果两个都是中间节点，就计算包围盒子的表面积， 展开那个表面积大的节点。展开的过程就是用不展开的那个节点分别和展开节点的两个子节点组合成 mjCollisionTree 的对象填充到栈顶，同时栈大小 nstack 自增。
if (!isleaf1 && isleaf2) {
for (int i=0; i < 2; i++) {
if (child1[2*node1+i] != -1) {
if (nstack >= max_stack)
mjERROR("BVH stack depth exceeded."); // SHOULD NOT OCCUR
stack[nstack].node1 = child1[2*node1+i];
stack[nstack].node2 = node2;
nstack++;
}
}
}
else if (isleaf1 && !isleaf2) {
for (int i=0; i < 2; i++) {
if (child2[2*node2+i] != -1) {
if (nstack >= max_stack)
mjERROR("BVH stack depth exceeded."); // SHOULD NOT OCCUR
stack[nstack].node1 = node1;
stack[nstack].node2 = child2[2*node2+i];
nstack++;
}
}
} else {
// compute surface areas of bounding boxes
mjtNum x1 = bvh1[6*node1+3]-bvh1[6*node1+0];
mjtNum y1 = bvh1[6*node1+4]-bvh1[6*node1+1];
mjtNum z1 = bvh1[6*node1+5]-bvh1[6*node1+2];
mjtNum x2 = bvh2[6*node2+3]-bvh2[6*node2+0];
mjtNum y2 = bvh2[6*node2+4]-bvh2[6*node2+1];
mjtNum z2 = bvh2[6*node2+5]-bvh2[6*node2+2];
mjtNum surface1 = x1*y1 + y1*z1 + z1*x1;
mjtNum surface2 = x2*y2 + y2*z2 + z2*x2;
// traverse the hierarchy whose bounding box has the larger surface area
if (surface1 > surface2) {
for (int i = 0; i < 2; i++) {
if (child1[2 * node1 + i] != -1) {
if (nstack >= max_stack)
mjERROR("BVH stack depth exceeded."); // SHOULD NOT OCCUR
stack[nstack].node1 = child1[2 * node1 + i];
stack[nstack].node2 = node2;
nstack++;
} }
} else {
for (int i = 0; i < 2; i++) {
if (child2[2 * node2 + i] != -1) {
if (nstack >= max_stack)
mjERROR("BVH stack depth exceeded."); // SHOULD NOT OCCUR
stack[nstack].node1 = node1;
stack[nstack].node2 = child2[2*node2+i];
nstack++;
} } } } }
}
mj_freeStack(d);
}
当栈为空的时候，就意味着遍历完两棵树的所有节点了。此时释放栈空间退出。