windows下使用FCL（The Flexible-collision-library）
FCL做为一款开源的碰撞检测库，支持多种基础的几何体，及支持C++和python，在windows和linux平台均可以使用。是一款计算高效的碰撞检测工具。在机械臂规划控制框架moveit中做为基础的碰撞检测算法。
FCL支持的几何体类型：
- box （长方体）
- sphere（球）
- ellipsoid（椭球）
- capsule（胶囊体）
- cone（锥体）
- cylinder（圆柱）
- convex（凸包）
- half-space（半空间）
- plane（平面）
- mesh（面片）
- octree （八叉树）
FCL库（The Flexible Collision Library）主要的功能有：
1、碰撞检测：检测两个模型是否重叠，以及（可选）所有重叠的三角形。
2、距离计算：计算一对模型之间的最小距离，即最近的一对点之间的距离。
3、公差验证：确定两个模型是否比公差距离更近或更远。
4、连续碰撞检测：检测两个运动模型在运动过程中是否重叠，以及可选的接触时间。
5、接触信息：对于碰撞检测和连续碰撞检测，可以选择返回接触信息（包括接触法线和接触点）。
源码下载及编译
FCL 源码github
在windows环境下，使用VS studio直接编译FCL存在问题，需要将CMake设置成Release版本以及屏蔽掉测试程序。具体操作如下：
- 使用VS studio打开FCO源码工程，如图1所示。
图1
图2
图3
FCL碰撞测试demo
测试程序如下所示：
//main.cpp
#include "fcl/math/constants.h"
#include "fcl/narrowphase/collision.h"
#include "fcl/narrowphase/collision_object.h"
#include "fcl/narrowphase/distance.h"
/**
* @brief 两个相互碰撞的Box碰撞检测测试
*/
void test1() {
std::shared_ptr<fcl::CollisionGeometry<double>> box1(
new fcl::Box<double>(3, 3, 3));
std::shared_ptr<fcl::CollisionGeometry<double>> box2(
new fcl::Box<double>(1, 1, 1));
fcl::Transform3d tf1 = fcl::Transform3d::Identity();
fcl::CollisionObjectd obj1(box1, tf1);
fcl::Transform3d tf2 = fcl::Transform3d::Identity();
fcl::CollisionObjectd obj2(box2, tf2);
fcl::CollisionRequestd request;
fcl::CollisionResultd result;
request.gjk_solver_type =
fcl::GJKSolverType::GST_INDEP; // specify solver type with the default
// type is GST_LIBCCD
fcl::collide(&obj1, &obj2, request, result);
std::cout << "test1 collide result:" << result.isCollision() << std::endl;
}
/**
* @brief 两个无碰撞的Box碰撞检测测试
*/
void test2() {
std::shared_ptr<fcl::CollisionGeometry<double>> box1(
new fcl::Box<double>(3, 3, 3));
std::shared_ptr<fcl::CollisionGeometry<double>> box2(
new fcl::Box<double>(1, 1, 1));
fcl::Transform3d tf1 = fcl::Transform3d::Identity();
fcl::CollisionObjectd obj1(box1, tf1);
fcl::Transform3d tf2 = fcl::Transform3d::Identity();
tf2.translation() = fcl::Vector3d{3, 0, 0};
fcl::CollisionObjectd obj2(box2, tf2);
fcl::CollisionRequestd request;
fcl::CollisionResultd result;
fcl::collide(&obj1, &obj2, request, result);
std::cout << "test2 collide result:" << result.isCollision() << std::endl;
}
/**
* @brief 两个无碰撞的Box碰撞检测测试，并计算最短距离
*/
void test3() {
std::shared_ptr<fcl::CollisionGeometry<double>> box1(
new fcl::Box<double>(3, 3, 3));
std::shared_ptr<fcl::CollisionGeometry<double>> box2(
new fcl::Box<double>(1, 1, 1));
fcl::Transform3d tf1 = fcl::Transform3d::Identity();
fcl::CollisionObjectd obj1(box1, tf1);
fcl::Transform3d tf2 = fcl::Transform3d::Identity();
tf2.translation() = fcl::Vector3d{3, 0, 0};
fcl::CollisionObjectd obj2(box2, tf2);
fcl::CollisionRequestd request;
fcl::CollisionResultd result;
// fcl::collide(&obj1,&obj2,request,result);
std::cout << "test3 collide result:" << result.isCollision() << std::endl;
fcl::DistanceRequestd dist_request(true);
dist_request.distance_tolerance = 1e-4;
fcl::DistanceResultd dist_result;
fcl::distance(&obj1, &obj2, dist_request, dist_result);
std::cout << "test3 collide distance:" << dist_result.min_distance
<< std::endl;
std::cout << "test3 collide point 0:" << dist_result.nearest_points[0]
<< std::endl;
std::cout << "test3 collide point 1:" << dist_result.nearest_points[1]
<< std::endl;
}
/**
* @brief 加载STL模型
*/
bool loadSTLFile(const std::string& filename,
std::vector<fcl::Triangle>& triangles) {
std::ifstream file(filename, std::ios::in | std::ios::binary);
if (!file) {
std::cerr << "Failed to open STL file: " << filename << std::endl;
return false;
}
file.seekg(0, std::ios::end); /// 定位到流末尾的位置，0偏移
std::streampos length = file.tellg(); /// 记录当前指针位置
file.seekg(0, std::ios::beg); /// 定位到流开头的位置，0偏移
std::vector<char> buffer(length);
file.read(&buffer[0], length);
file.close();
if (length < 84) {
std::cerr << "Invalid STL file: " << filename << std::endl;
return false;
}
unsigned int num_triangles = *(unsigned int*)&buffer[80];
triangles.resize(num_triangles);
unsigned int offset = 84;
for (unsigned int i = 0; i < num_triangles; ++i) {
for (unsigned int j = 0; j < 3; ++j) {
// 3顶点构成三角形
float* vertex = (float*)&buffer[offset + j * 12];
triangles[i][j] = (vertex[0], vertex[1], vertex[2]);
}
offset += 50;
}
return true;
}
/**
* @brief 在STL文件格式中，文件头部分包含80个字节的文件头信息和4个字节的三角形数量信息，因此文件总长度至少为84个字节。
因此，在loadSTLFile函数中我们首先检查文件长度是否小于84个字节，如果是则认为文件格式非法。
在STL文件中，每个三角形由12个浮点数和2个无用字节组成，因此每个三角形占用50个字节。
因此，在loadSTLFile函数中，我们通过一个循环遍历每个三角形，并从文件中读取对应的12个浮点数，最后将三角形的3个顶点存储在一个fcl::Triangle类型的变量中。
每次读取完一个三角形后，需要将读取指针向前移动50个字节，即offset += 50。由于文件头部分占用了前84个字节，因此，在开始循环前需要将读取指针初始化为offset= 84，从而跳过文件头部分，开始读取三角形信息。
*/
void test4() {
std::vector<fcl::Triangle> triangles; /// 创建三角片序列
/// 加载模型
if (!loadSTLFile("C:/test0.STL", triangles)) {
std::cout << "Error:loadSTLFile failed!" << std::endl;
return;
}
/// 创建mesh,并添加三角片到mesh
///
std::shared_ptr<fcl::BVHModel<fcl::OBBRSSd>> mesh_geometry(
new fcl::BVHModel<fcl::OBBRSSd>());
mesh_geometry->beginModel();
for (const auto& triangle : triangles) {
Eigen::Vector3d p1(triangle[0]), p2(triangle[1]), p3(triangle[2]);
mesh_geometry->addTriangle(p1, p2, p3);
}
mesh_geometry->endModel();
/// 建立碰撞对象-stl ,并添加CollisionGeometry,坐标位置（0，0，0）
fcl::CollisionObjectd obj(mesh_geometry);
/// 建立碰撞对象-box ,坐标位置（0，0，20）
std::shared_ptr<fcl::Boxd> box1 = std::make_shared<fcl::Boxd>(2.0, 2.0, 2.0);
fcl::CollisionObjectd obj1(box1);
obj1.setTranslation(Eigen::Vector3d(0, 0, 0));
fcl::CollisionRequestd request;
fcl::CollisionResultd result;
/// 进行碰撞检测
fcl::collide(&obj, &obj1, request, result);
/// 输出碰撞结果
if (result.isCollision()) {
std::cout << "Collision detected!" << std::endl;
} else {
std::cout << "No collision detected." << std::endl;
}
/// 距离检测
fcl::DistanceRequestd requestd;
fcl::DistanceResultd resultd;
fcl::distance(&obj, &obj1, requestd, resultd);
std::cout << "min_distance:" << resultd.min_distance << std::endl;
}
int main(int argc, char** argv) {
std::cout << "FCL test" << std::endl;
test1();
test2();
test3();
test4();
std::cout << "end test" << std::endl;
return 0;
}
CMakeList.txt文件如下所示：
cmake_minimum_required(VERSION 3.14)
find_package(Eigen3 REQUIRED)
find_package(FCL REQUIRED)
add_executable(use_fcl main.cpp)
target_link_libraries(use_fcl fcl Eigen3::Eigen)
target_include_directories(use_fcl PUBLIC ${EIGEN3_INCLUDE_DIRS} ${FCL_INCLUDE_DIRS})
