void collision_detection::CollisionWorldFCL::updateFCLObject(const std::string &id)
{
// remove FCL objects that correspond to this object
std::map<std::string, FCLObject>::iterator jt = fcl_objs_.find(id);
if (jt != fcl_objs_.end())
{
jt->second.unregisterFrom(manager_.get());
jt->second.clear();
}
// check to see if we have this object
collision_detection::World::const_iterator it = getWorld()->find(id);
if (it != getWorld()->end())
{
// construct FCL objects that correspond to this object
if (jt != fcl_objs_.end())
{
constructFCLObject(it->second.get(), jt->second);
jt->second.registerTo(manager_.get());
}
else
{
constructFCLObject(it->second.get(), fcl_objs_[id]);
fcl_objs_[id].registerTo(manager_.get());
}
}
else
{
if (jt != fcl_objs_.end())
fcl_objs_.erase(jt);
}
// manager_->update();
}
void collision_detection::CollisionRobotFCL::allocSelfCollisionBroadPhase(const planning_models::KinematicState &state, FCLManager &manager) const
{
fcl::DynamicAABBTreeCollisionManager* m = new fcl::DynamicAABBTreeCollisionManager();
// m->tree_init_level = 2;
manager.manager_.reset(m);
constructFCLObject(state, manager.object_);
manager.object_.registerTo(manager.manager_.get());
// manager.manager_->update();
}
