windows下使用FCL（Flexible-collision-library）

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版本以及屏蔽掉测试程序。具体操作如下：

1. 使用VS studio打开FCO源码工程，如图1所示。

图1

2. 通过改CMakeList.txt文件，屏蔽测试程序，如图2所示。

图2

3. 点击“项目”，再点击“fcl的CMake配置”，将编译设置成Release版本，如图3所示。

图3

4. 点击“生成”，再点击“全部重新生成”对FCL源码进行编译。

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_LIBCCDfcl::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})