2022-3-12 fcl库的内存分配

上学期研究碰撞检测用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 指针每次更新指向新的内存区域即可。