案例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