Flexible Collision Library（FCL）简介及使用流程
1 FCL简介
FCL是一个碰撞检测库，针对刚体模型、可变体模型、连杆类模型和点云图等碰撞检测模型提供一个系统框架，FCL使用物体的位置、角度、包围盒和表面网格来检测两个物体之间的碰撞，提供离散碰撞检测、连续的碰撞检测、距离计算、穿透深度估算四种功能。主要适用于以下场景：
| 刚体模型 | 可变体模型 | 点云图 | 连杆类模型 | |
| 碰撞检测 | √ | √ | √ | √ |
| 连续碰撞检测 | √ | √ | X | √ |
| 自碰撞检测 | √ | √ | √ | √ |
| 渗透估计 | √ | X | X | X |
| 距离计算 | √ | √ | X | √(接口推导) |
| 宽相碰撞 | √ | √ | √ | √(接口推导) |
| 名词解释： 碰撞检测：检测两个模型是否重叠（以及可选的位置）。 自碰撞检测：检测模型自身各组成部分是否重叠。 连续碰撞检测（Continuous Collision Detection，CCD）：检测两个模型在运动过程中是否重叠（以及可选的接触时间）。 距离计算：计算一对模型之间的最小距离。 宽相碰撞：对象组之间碰撞检测和距离计算，并且可以避免成对计算带来的n^2的复杂度。 | ||||
表1 FCL适用场景
FCL的碰撞检测查询是一个多层次的遍历过程，不同的查询使用相同的遍历框架，但数据结构和遍历策略有所不同，碰撞检测和距离计算的遍历过程由如下三步组成：
1）目标表示：却定目标包围体层级结构(Bounding Volume Hierarchy，BVH)，例如，基本几何形状（例如，锥、圆柱体、球体）用一个相应的单层的包围盒所表示并作为一个独立的节点。任意几何目标用层次包围盒表示，层次结构和目标的配置构型信息被存储在fcl::CollisionObject的结构体中。
2）遍历节点初始化：遍历节点是用来存储用于执行特定查询的遍历所需的完整信息的结构。这样的信息在不同对象表示的各种类型的查询中可能是不同的。例如，对于连续碰撞查询，需要为前一帧存储对象坐标和形状表示。遍历节点还决定给定查询的遍历策略。例如，如果碰撞查询只需要一个“是”或“否”的结论，那么一旦发现发生碰撞，遍历就可以停止。
3）层次遍历：在遍历节点初始化步骤之后，遍历层次结构来执行特定的碰撞检测。
通常情况下，对于基本的几何体用一个单层包围盒描述，而随机几何体用多层次包围盒描述，不同的包围盒描述有不同的查询方法，首先对包围盒之间的碰撞进行粗判断，如果不相交则没有发生碰撞；反之相交则对物体细分进行下一步查询，经过一定层次的逐步求精，可以得出比较精确的碰撞位置。
针对CCD场景，FCL使用保守进步算法（Conservative Advancement，CA）。CA算法的原理是以增量的方式按固定时间步前进物体同时避免碰撞。为了确定保守时间步的长度，它需要计算物体之间的最小间隔距离并使用它来估计保守运动边界。BVH数据结构存储了基础物体的顶点和三角网格。为了执行CCD计算，BVH数据结构也从上一时间步跟踪顶点的位置。其结果是，BVH结构可以使用一致的方式处理三角网格和顶点云。为了执行CCD请求，BVH被用来计算在给定的时间间隔内物体的保守波及体积的边界。因此，FCL更新BVH为了考虑运动信息和给CCD获取有效的BVH。当物体的底层拓扑更改时，FCL清空BVH然后重新构建。
2 FCL实现流程
2.1 实例化碰撞对象
碰撞对象类型的可以分为几何包围体和包围盒（bounding box/volume，BV），通常情况下，只有当包围体有碰撞时，才有必要进一步进行更复杂的几何体测试。
在FCL库中，提供了以下关于几何包围体的接口，分别为：
立方体：fcl::BOX
胶囊体：fcl::Capsule
椎体：fcl::Cone
凸面体fcl::Convex
圆柱体：fcl::Cylinder
椭球体：fcl::Ellipsoid
平面：fcl::Plane
球体：fcl::Sphere
三角形fcl::Triangle
实例化后，建立以坐标轴原点为中心的几何体。用户可以通过fcl::ShapeBase接口（或者各自几何体重写fcl::ShapeBase后）的computeLocalAABB()计算出轴对齐包围盒(Axis-aligned Bounding Box，AABB)的数据、getNodeType()获取节点类型、computeVolume()获取体积数据、computeMomentofInertia()获取惯性矩阵（inertia matrix）数据。各包围体均派生于fcl::ShapeBase类，fcl::ShapeBase派生于fcl::CollisionGeometry，fcl::CollisionGeometry接口是所有实例化碰撞对象的基类接口。
2.2 声明碰撞对象在空间的位姿
几何体实例化后，可以通过fcl::Transform确定其空间坐标系的位置，平移是3元素一维向量，而旋转可以由3x3旋转矩阵或四元数指定。
2.3 构造碰撞对象
实物在空间的位置声明后，一个fcl::CollisionGeometry和一个fcl::Transform，可以构造一个碰撞对象fcl::CollisionObject，可使用fcl::ContinuousCollisionObject构造连续构造对象。fcl::DummyCollisionObject是派生于fcl::CollisionObject的虚拟碰撞对象，可以通过computeLocalAABB()获取实例的AABB信息。碰撞对象创建完后，依然可以通过fcl::CollisionObject的接口对其内部的实物及位姿属性进行修改。
FCL还支持对象组之间的碰撞/距离查询fcl::BroadPhaseCollisionManager，可以避免成对计算带来的n^2的复杂度。具体地，在执行碰撞/距离检查之前，将fcl::CollisionObject各项注册到AABB管理器fcl::DynamicAABBTreeCollisionManager中。
AABB管理器提供三种类型的检查：
- 一对多: 单独的 CollisionObject与AABB管理器中所有对象之间的碰撞检测、距离检查。
- AABB管理器内的多对多:AABB管理器中所有对象对之间的成对的碰撞检测、距离检查。
- AABB管理器间的多对多: 来自两个AABB管理器之间的成对碰撞检测、距离检查。
通常，碰撞检查可以返回所有接触对象，而距离检查仅返回任何一对对象之间的最接近距离。此外，还可以通过fcl::SortByXLow、fcl::SortByYLow、fcl::SortByZLow可以根据实物的AABB边框进行排序。
2.4 碰撞操作
针对构造的实体或者AABB虚体对象。支持三种查询类型，分别是：碰撞检测、距离检测、连续碰撞检测，每一个对象在进行碰撞查询时遵循单线程的规则。
首先，需要初始化一个碰撞请求的数据结构，将创建fcl::CollisionGeometry和一个fcl::Transform参数代入fcl::CollisionRequest接口中。第二步，使用fcl::CollisionResult初始化一个空的相应查询结构。使用两个fcl::CollisionObject，请求结构和响应结构作为参数调用查询函数。调用fcl::collide后,f返回结果result 包含有关碰撞和接触的信息。
2.5 距离检查
首先，需要初始化一个碰撞请求的数据结构，将创建CollisionGeometry和一个Transform参数代入fcl::DistanceReques接口中。第二步，使用fcl:::DistanceResult初始化一个空的相应查询结构。使用两个CollisionObject，请求结构和响应结构作为参数调用查询函数。调用fcl::distance后，返回结果result 包含有关碰撞和接触的信息。
2.6 连续碰撞检查
首先，需要初始化一个碰撞请求的数据结构，将创建CollisionGeometry和一个Transform参数代入fcl::ContinuousCollisionRequest接口中。第二步，使用fcl:::ContinuousCollisionResult初始化一个空的相应查询结构。使用两个CollisionObject，请求结构和响应结构作为参数调用查询函数。调用fcl::continuousCollide后，result 包含有关碰撞时间和状态的信息，以及前后两时间点的碰撞结果。