void collision_detection::CollisionWorldFCL::constructFCLObject(const World::Object *obj, FCLObject &fcl_obj) const
{
for (std::size_t i = 0 ; i < obj->shapes_.size() ; ++i)
{
FCLGeometryConstPtr g = createCollisionGeometry(obj->shapes_[i], obj);
if (g)
{
fcl::CollisionObject *co = new fcl::CollisionObject(g->collision_geometry_, transform2fcl(obj->shape_poses_[i]));
fcl_obj.collision_objects_.push_back(boost::shared_ptr<fcl::CollisionObject>(co));
fcl_obj.collision_geometry_.push_back(g);
}
}
}
void collision_detection::CollisionRobotFCL::constructFCLObject(const planning_models::KinematicState &state, FCLObject &fcl_obj) const
{
const std::vector<planning_models::KinematicState::LinkState*> &link_states = state.getLinkStateVector();
fcl_obj.collision_objects_.reserve(geoms_.size());
for (std::size_t i = 0 ; i < geoms_.size() ; ++i)
{
if (geoms_[i] && geoms_[i]->collision_geometry_)
{
fcl::CollisionObject *collObj = new fcl::CollisionObject(geoms_[i]->collision_geometry_, transform2fcl(link_states[i]->getGlobalCollisionBodyTransform()));
fcl_obj.collision_objects_.push_back(boost::shared_ptr<fcl::CollisionObject>(collObj));
// the CollisionGeometryData is already stored in the class member geoms_, so we need not copy it
}
std::vector<const planning_models::KinematicState::AttachedBody*> ab;
link_states[i]->getAttachedBodies(ab);
for (std::size_t j = 0 ; j < ab.size() ; ++j)
{
std::vector<FCLGeometryConstPtr> objs;
getAttachedBodyObjects(ab[j], objs);
const EigenSTL::vector_Affine3d &ab_t = ab[j]->getGlobalCollisionBodyTransforms();
for (std::size_t k = 0 ; k < objs.size() ; ++k)
if (objs[k]->collision_geometry_)
{
fcl::CollisionObject *collObj = new fcl::CollisionObject(objs[k]->collision_geometry_, transform2fcl(ab_t[k]));
fcl_obj.collision_objects_.push_back(boost::shared_ptr<fcl::CollisionObject>(collObj));
// we copy the shared ptr to the CollisionGeometryData, as this is not stored by the class itself,
// and would be destroyed when objs goes out of scope.
fcl_obj.collision_geometry_.push_back(objs[k]);
}
}
}
}
