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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193

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})
1
2
3
4
5
6