【FCL入门教程1】实现简单的碰撞检测

案例1：两个不同位置、不同大小的Box碰撞检测

#include<Eigen/Core>
#include<ccd/ccd.h>
#define FCL_EXPORT
#include <fcl/fcl.h>
#include <fcl/narrowphase/collision.h>
#include <iostream>

void test01()
{

    cout<<"ok!"<<endl;
    
    // 创建两个不同位置的正方体
    shared_ptr<Boxd> box1 = make_shared<Boxd>(2.0, 2.0, 2.0);
    shared_ptr<Boxd> box2 = make_shared<Boxd>(4.0, 4.0, 4.0);
    
    // 创建对应的碰撞对象和碰撞组
    CollisionObjectd obj1(box1);
    CollisionObjectd obj2(box2);
    obj1.setTranslation(Vector3d(0, 0, 0)); // 第一个正方体位于原点
    obj2.setTranslation(Vector3d(5, 0, 0)); // 第二个正方体位于x轴上，距离原点为5
    
    vector<CollisionObjectd*> objects = {&obj1, &obj2};
    
    CollisionRequestd request;
    CollisionResultd result;
    
    // 进行碰撞检测
    collide(&obj1, &obj2, request, result);
    
    // 输出碰撞结果
    if (result.isCollision()) {
        cout << "Collision detected!" << endl;
    } else {
        cout << "No collision detected." << endl;
    }
    
    //距离检测
    DistanceRequestd requestd;
    DistanceResultd resultd;
    distance(&obj1, &obj2, requestd, resultd);
    cout << "min_distance:" << resultd.min_distance<<endl;

}

案例2：stl模型与Box的碰撞检测

CollisionObject有3种重载，CollisionGeometry为几何体，其他参数表示位置

CollisionObject(const std::shared_ptr<CollisionGeometry<S>>& cgeom);
CollisionObject(const std::shared_ptr<CollisionGeometry<S>>& cgeom,
                const Transform3<S>& tf);
CollisionObject(const std::shared_ptr<CollisionGeometry<S>>& cgeom,
                const Matrix3<S>& R,
                const Vector3<S>& T);

注意这里针对二进制编码的stl格式

#include<Eigen/Core>
#include<ccd/ccd.h>
#define FCL_EXPORT
#include <fcl/fcl.h>
#include <fcl/narrowphase/collision.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <string>

// 加载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;
}

/*
 在STL文件格式中，文件头部分包含80个字节的文件头信息和4个字节的三角形数量信息，因此文件总长度至少为84个字节。
因此，在loadSTLFile函数中我们首先检查文件长度是否小于84个字节，如果是则认为文件格式非法。
在STL文件中，每个三角形由12个浮点数和2个无用字节组成，因此每个三角形占用50个字节。
因此，在loadSTLFile函数中，我们通过一个循环遍历每个三角形，并从文件中读取对应的12个浮点数，最后将三角形的3个顶点存储在一个fcl::Triangle类型的变量中。
每次读取完一个三角形后，需要将读取指针向前移动50个字节，即offset += 50。由于文件头部分占用了前84个字节，因此，在开始循环前需要将读取指针初始化为offset = 84，从而跳过文件头部分，开始读取三角形信息。
**/


void test02()
{
      std::vector<fcl::Triangle> triangles;   ///创建三角片序列
      ///加载模型
      if(!loadSTLFile("D:/QQT/OpenGL/GL14 Robot Inverse/model/1.stl",triangles)){
        cout<<"Error:loadSTLFile failed!"<<endl;
      }
      ///创建mesh,并添加三角片到mesh
      /// 
      std::shared_ptr<fcl::BVHModel<fcl::OBBRSSd>> mesh_geometry(new fcl::BVHModel<fcl::OBBRSSd>());
      mesh_geometry->beginModel();
      for(const auto& triangle : triangles)
      {
          Vector3d  p1(triangle[0]),p2(triangle[1]),p3(triangle[2]);
          mesh_geometry->addTriangle(p1, p2, p3);
      }
      mesh_geometry->endModel();
  
      ///建立碰撞对象-stl ,并添加CollisionGeometry,坐标位置（0，0，0）
      CollisionObjectd obj(mesh_geometry);
       ///建立碰撞对象-box ,坐标位置（0，0，20）
      shared_ptr<Boxd> box1 = make_shared<Boxd>(2.0, 2.0, 2.0);
      CollisionObjectd obj1(box1);
      obj1.setTranslation(Vector3d(0, 0, 20));
        
      CollisionRequestd request;
      CollisionResultd result;

      /// 进行碰撞检测
      collide(&obj, &obj1, request, result);
      /// 输出碰撞结果
      if (result.isCollision()) {
          cout << "Collision detected!" << endl;
      } else {
          cout << "No collision detected." << endl;
      }
      ///距离检测
      DistanceRequestd requestd;
      DistanceResultd resultd;
      distance(&obj, &obj1, requestd, resultd);
      cout << "min_distance:" << resultd.min_distance<<endl;

}

 

 在STL文件格式中，文件头部分包含80个字节的文件头信息和4个字节的三角形数量信息，因此文件总长度至少为84个字节。因此，在loadSTLFile函数中我们首先检查文件长度是否小于84个字节，如果是则认为文件格式非法。
在STL文件中，每个三角形由12个浮点数和2个无用字节组成，因此每个三角形占用50个字节。
因此，在loadSTLFile函数中，我们通过一个循环遍历每个三角形，并从文件中读取对应的12个浮点数，最后将三角形的3个顶点存储在一个fcl::Triangle类型的变量中。
每次读取完一个三角形后，需要将读取指针向前移动50个字节，即offset += 50。由于文件头部分占用了前84个字节，因此，在开始循环前需要将读取指针初始化为offset = 84，从而跳过文件头部分，开始读取三角形信息。
 

以上代码用的double，把末尾d替换成f应该更合理。

问题记录 Qt报错：Assertion failed!

 

不知什么原因

解决方法

找到问题代码，304行

 注释掉它就好了

eigen_assert(((SizeAtCompileTime == Dynamic && (MaxSizeAtCompileTime==Dynamic || size<=MaxSizeAtCompileTime)) || SizeAtCompileTime == size) && size>=0);
      #ifdef EIGEN_INITIALIZE_COEFFS