上学期研究碰撞检测用fcl就遇到过指针的问题,
2021-11-05 fcl collisiondata 的重复使用?重分配内存
这回用深度相机实时得到点云转换到fcl里面进行碰撞检测,建立的collisionobject 是指向已分配内存的指针
fcl::CollisionObjectd* pointobj = new fcl::CollisionObject<double>(pointgeometry,m_collision.pointtransform);
通过replace或者update对该object进行操作,fcl为了限制该object里面的数据不会溢出,已提示输入新的顶点数量需要与原来一致,不然就有可能内存溢出。
int BVHModel<BV>::beginReplaceModel()
{
if(build_state != BVH_BUILD_STATE_PROCESSED)
{
std::cerr << "BVH Error! Call beginReplaceModel() on a BVHModel that has no previous frame." << std::endl;
return BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME;
}
if(prev_vertices)
{
delete [] prev_vertices;
prev_vertices = nullptr;
}
num_vertex_updated = 0;
build_state = BVH_BUILD_STATE_REPLACE_BEGUN;
return BVH_OK;
}
//==============================================================================
template <typename BV>
int BVHModel<BV>::replaceVertex(const Vector3<S>& p)
{
if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN)
{
std::cerr << "BVH Warning! Call replaceVertex() in a wrong order. replaceVertex() was ignored. Must do a beginReplaceModel() for initialization." << std::endl;
return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
}
vertices[num_vertex_updated] = p;
num_vertex_updated++;
return BVH_OK;
}
//==============================================================================
template <typename BV>
int BVHModel<BV>::replaceTriangle(const Vector3<S>& p1, const Vector3<S>& p2, const Vector3<S>& p3)
{
if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN)
{
std::cerr << "BVH Warning! Call replaceTriangle() in a wrong order. replaceTriangle() was ignored. Must do a beginReplaceModel() for initialization." << std::endl;
return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
}
vertices[num_vertex_updated] = p1; num_vertex_updated++;
vertices[num_vertex_updated] = p2; num_vertex_updated++;
vertices[num_vertex_updated] = p3; num_vertex_updated++;
return BVH_OK;
}
//==============================================================================
template <typename BV>
int BVHModel<BV>::replaceSubModel(const std::vector<Vector3<S>>& ps)
{
if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN)
{
std::cerr << "BVH Warning! Call replaceSubModel() in a wrong order. replaceSubModel() was ignored. Must do a beginReplaceModel() for initialization." << std::endl;
return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
}
for(unsigned int i = 0; i < ps.size(); ++i)
{
vertices[num_vertex_updated] = ps[i];
num_vertex_updated++;
}
return BVH_OK;
}
//==============================================================================
template <typename BV>
int BVHModel<BV>::endReplaceModel(bool refit, bool bottomup)
{
if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN)
{
std::cerr << "BVH Warning! Call endReplaceModel() in a wrong order. endReplaceModel() was ignored. " << std::endl;
return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
}
if(num_vertex_updated != num_vertices)
{
std::cerr << "BVH Error! The replaced model should have the same number of vertices as the old model." << std::endl;
return BVH_ERR_INCORRECT_DATA;
}
if(refit) // refit, do not change BVH structure
{
refitTree(bottomup);
}
else // reconstruct bvh tree based on current frame data
{
buildTree();
}
build_state = BVH_BUILD_STATE_PROCESSED;
return BVH_OK;
}
但是深度相机的有效点云中点的数量每次并不一致,因此需要重新开辟一块内存建立geom 数据结构来保存新的点云碰撞模型即可解决问题。
/* update obj ? error about memory allocation */
fcl::CollisionObjectd* pointobj = new fcl::CollisionObject<double>(pointgeometry,m_collision.pointtransform);
m_collision.pointobj = pointobj;
m_collision 类里面的pointobj 指针每次更新指向新的内存区域即可。