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(); }