fcl - 高效灵活的三维碰撞检测和距离计算库

FCL -- 灵活碰撞库

Linux / OS X Windows 覆盖率

FCL是一个用于对由三角形组成的几何模型对执行三种类型邻近查询的库。

碰撞检测：检测两个模型是否重叠，并可选择性地返回所有重叠的三角形。
距离计算：计算一对模型之间的最小距离，即最近点对之间的距离。
容差验证：确定两个模型是否比容差距离更近或更远。
连续碰撞检测：检测两个移动模型在运动过程中是否重叠，并可选择性地返回接触时间。
接触信息：对于碰撞检测和连续碰撞检测，可以选择性地返回接触信息（包括接触法线和接触点）。

FCL具有以下特点：

C++接口
可编译用于Linux或Windows（可以使用cmake生成makefile或Microsoft Visual Studio项目）
输入模型不需要特殊的拓扑约束或邻接信息 - 只需要模型的三角形列表
支持不同的物体形状：

盒子
球体
椭球体
胶囊体
圆锥体
圆柱体
凸面体
半空间
平面
网格
八叉树（可选，八叉树使用octomap库表示，http://octomap.github.com）

安装

在编译FCL之前，请确保已安装Eigen和libccd（用于凸对象之间的碰撞检查，可在https://github.com/danfis/libccd获取）。对于libccd，请确保从github版本编译，而不是网页上的zip文件，因为压缩版本中不包含一个bug修复。

FCL的一些可选功能需要安装一些可选库。对于八叉树碰撞，请从https://octomap.github.io/安装octomap库。

CMakeLists.txt用于在Linux中生成makefile或在Windows中生成Visual Studio项目。在命令行中运行：

mkdir build
cd build
cmake ..

接下来，在Linux中，使用make编译代码。

在Windows中，将生成Visual Studio项目，然后您可以编译代码。

接口

在开始邻近计算之前，我们首先需要为参与计算的对象设置几何形状和变换。对象的几何形状表示为网格汤，可以按如下方式设置：

// 设置网格三角形和顶点索引
std::vector<Vector3f> vertices;
std::vector<Triangle> triangles;
// 设置顶点和三角形的代码
...
// BVHModel是网格几何的模板类，默认使用OBBRSS模板
typedef BVHModel<OBBRSSf> Model;
std::shared_ptr<Model> geom = std::make_shared<Model>();
// 将网格数据添加到BVHModel结构中
geom->beginModel();
geom->addSubModel(vertices, triangles);
geom->endModel();

对象的变换包括旋转和平移：

// R和T分别是旋转矩阵和平移向量
Matrix3f R;
Vector3f T;
// 设置R和T的代码
...
// 根据R和T配置变换
Transform3f pose = Transform3f::Identity();
pose.linear() = R;
pose.translation() = T;

给定几何形状和变换，我们还可以将它们组合在一起以获得碰撞对象实例，这里是一个例子：

//geom和tf是对象的几何形状和变换
std::shared_ptr<BVHModel<OBBRSSf>> geom = ...
Transform3f tf = ...
//将它们组合在一起
CollisionObjectf* obj = new CollisionObjectf(geom, tf);

设置好对象后，我们可以对它们进行邻近计算。FCL中的所有邻近查询都遵循一个通用流程：首先，设置查询请求数据结构，然后使用请求作为输入运行查询函数。结果返回在查询结果数据结构中。例如，对于碰撞检查，我们首先设置CollisionRequest数据结构，然后运行碰撞函数：

// 给定两个对象o1和o2
CollisionObjectf* o1 = ...
CollisionObjectf* o2 = ...
// 设置碰撞请求结构，这里我们只使用默认设置
CollisionRequest request;
// 结果将通过碰撞结果结构返回
CollisionResult result;
// 执行碰撞测试
collide(o1, o2, request, result);

通过设置碰撞请求，用户可以轻松选择是返回接触信息（速度较慢）还是只返回二进制碰撞结果（速度较快）。对于距离计算，流程几乎相同：

// 给定两个对象 o1 和 o2
CollisionObjectf* o1 = ...
CollisionObjectf* o2 = ...
// 设置距离请求结构，这里我们只使用默认设置
DistanceRequest request;
// 结果将通过碰撞结果结构返回
DistanceResult result;
// 执行距离测试
distance(o1, o2, request, result);

对于连续碰撞，FCL 需要提供目标变换（初始变换包含在碰撞对象数据结构中）。除此之外，流程几乎与距离/碰撞相同：

// 给定两个对象 o1 和 o2
CollisionObjectf* o1 = ...
CollisionObjectf* o2 = ...
// o1 和 o2 的目标变换
Transform3f tf_goal_o1 = ...
Transform3f tf_goal_o2 = ...
// 设置连续碰撞请求结构，这里我们只使用默认设置
ContinuousCollisionRequest request;
// 结果将通过连续碰撞结果结构返回
ContinuousCollisionResult result;
// 执行连续碰撞测试
continuousCollide(o1, tf_goal_o1, o2, tf_goal_o2, request, result);

FCL 支持两组对象之间的宽相碰撞/距离计算，可以避免 n 平方的复杂度。对于碰撞，宽相算法可以返回所有碰撞对。对于距离，它可以返回最小距离的对。FCL 使用 CollisionManager 结构来管理涉及碰撞或距离操作的所有对象。

// 为第一组对象初始化碰撞管理器。
// FCL 提供了 CollisionManager 的各种不同实现。
// 通常，DynamicAABBTreeCollisionManager 会提供最佳性能。
BroadPhaseCollisionManagerf* manager1 = new DynamicAABBTreeCollisionManagerf(); 
// 为第二组对象初始化碰撞管理器。
BroadPhaseCollisionManagerf* manager2 = new DynamicAABBTreeCollisionManagerf();
// 要将对象添加到碰撞管理器中，使用
// BroadPhaseCollisionManager::registerObject() 函数添加一个对象
std::vector<CollisionObjectf*> objects1 = ...
for(std::size_t i = 0; i < objects1.size(); ++i)
manager1->registerObject(objects1[i]);
// 另一种选择是使用 BroadPhaseCollisionManager::registerObjects()
// 函数添加一组对象
std::vector<CollisionObjectf*> objects2 = ...
manager2->registerObjects(objects2);
// 为了在宽相阶段收集信息，CollisionManager 需要两个设置：
// a) 碰撞或距离的回调函数；
// b) 用于存储宽相计算过程中生成的信息的中间数据。
// 为方便起见，FCL 为碰撞和距离查询提供了默认的回调函数来满足 a)，
// 以及相应的回调数据来满足 b)。对于碰撞，使用 DefaultCollisionCallback 和 DefaultCollisionData，
// 对于距离，使用 DefaultDistanceCallback 和 DefaultDistanceData。
// 默认的碰撞/距离数据结构只是简单的容器，包含上面提到的每种查询类型的请求和距离结构。
DefaultCollisionData collision_data;
DefaultDistanceData distance_data;
// 设置管理器，这与根据输入对象初始化宽相加速结构有关
manager1->setup();
manager2->setup();
// 各种查询的示例
// 1. 两个对象组之间的碰撞查询，并获取碰撞数量
manager2->collide(manager1, &collision_data, DefaultCollisionFunction);
int n_contact_num = collision_data.result.numContacts(); 
// 2. 两个对象组之间的距离查询，并获取最小距离
manager2->distance(manager1, &distance_data, DefaultDistanceFunction);
double min_distance = distance_data.result.min_distance;
// 3. 组 1 的自碰撞查询
manager1->collide(&collision_data, DefaultCollisionFunction);
// 4. 组 1 的自距离查询
manager1->distance(&distance_data, DefaultDistanceFunction);
// 5. 组 1 中的一个对象与整个组 2 之间的碰撞查询
manager2->collide(objects1[0], &collision_data, DefaultCollisionFunction);
// 6. 组 1 中的一个对象与整个组 2 之间的距离查询
manager2->distance(objects1[0], &distance_data, DefaultDistanceFunction);

更多示例，请参考测试文件夹：

test_fcl_collision.cpp：提供碰撞测试的示例
test_fcl_distance.cpp：提供距离测试的示例
test_fcl_broadphase.cpp：提供宽相碰撞/距离测试的示例
test_fcl_frontlist.cpp：提供前列表碰撞加速的示例
test_fcl_octomap.cpp：提供八叉树数据与其他数据类型之间碰撞/距离计算的示例