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_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})