00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <moveit/collision_detection_fcl/collision_common.h>
00038 #include <fcl/BVH/BVH_model.h>
00039 #include <fcl/shape/geometric_shapes.h>
00040 #include <fcl/octree.h>
00041 #include <boost/thread/mutex.hpp>
00042
00043 namespace collision_detection
00044 {
00045 bool collisionCallback(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* data)
00046 {
00047 CollisionData* cdata = reinterpret_cast<CollisionData*>(data);
00048 if (cdata->done_)
00049 return true;
00050 const CollisionGeometryData* cd1 = static_cast<const CollisionGeometryData*>(o1->collisionGeometry()->getUserData());
00051 const CollisionGeometryData* cd2 = static_cast<const CollisionGeometryData*>(o2->collisionGeometry()->getUserData());
00052
00053
00054 if (cd1->sameObject(*cd2))
00055 return false;
00056
00057
00058 if (cdata->active_components_only_)
00059 {
00060 const robot_model::LinkModel* l1 =
00061 cd1->type == BodyTypes::ROBOT_LINK ?
00062 cd1->ptr.link :
00063 (cd1->type == BodyTypes::ROBOT_ATTACHED ? cd1->ptr.ab->getAttachedLink() : NULL);
00064 const robot_model::LinkModel* l2 =
00065 cd2->type == BodyTypes::ROBOT_LINK ?
00066 cd2->ptr.link :
00067 (cd2->type == BodyTypes::ROBOT_ATTACHED ? cd2->ptr.ab->getAttachedLink() : NULL);
00068
00069
00070 if ((!l1 || cdata->active_components_only_->find(l1) == cdata->active_components_only_->end()) &&
00071 (!l2 || cdata->active_components_only_->find(l2) == cdata->active_components_only_->end()))
00072 return false;
00073 }
00074
00075
00076 DecideContactFn dcf;
00077 bool always_allow_collision = false;
00078 if (cdata->acm_)
00079 {
00080 AllowedCollision::Type type;
00081 bool found = cdata->acm_->getAllowedCollision(cd1->getID(), cd2->getID(), type);
00082 if (found)
00083 {
00084
00085 if (type == AllowedCollision::ALWAYS)
00086 {
00087 always_allow_collision = true;
00088 if (cdata->req_->verbose)
00089 logDebug(
00090 "Collision between '%s' (type '%s') and '%s' (type '%s') is always allowed. No contacts are computed.",
00091 cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(), cd2->getTypeString().c_str());
00092 }
00093 else if (type == AllowedCollision::CONDITIONAL)
00094 {
00095 cdata->acm_->getAllowedCollision(cd1->getID(), cd2->getID(), dcf);
00096 if (cdata->req_->verbose)
00097 logDebug("Collision between '%s' and '%s' is conditionally allowed", cd1->getID().c_str(),
00098 cd2->getID().c_str());
00099 }
00100 }
00101 }
00102
00103
00104 if (cd1->type == BodyTypes::ROBOT_LINK && cd2->type == BodyTypes::ROBOT_ATTACHED)
00105 {
00106 const std::set<std::string>& tl = cd2->ptr.ab->getTouchLinks();
00107 if (tl.find(cd1->getID()) != tl.end())
00108 {
00109 always_allow_collision = true;
00110 if (cdata->req_->verbose)
00111 logDebug("Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.",
00112 cd1->getID().c_str(), cd2->getID().c_str());
00113 }
00114 }
00115 else if (cd2->type == BodyTypes::ROBOT_LINK && cd1->type == BodyTypes::ROBOT_ATTACHED)
00116 {
00117 const std::set<std::string>& tl = cd1->ptr.ab->getTouchLinks();
00118 if (tl.find(cd2->getID()) != tl.end())
00119 {
00120 always_allow_collision = true;
00121 if (cdata->req_->verbose)
00122 logDebug("Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.",
00123 cd2->getID().c_str(), cd1->getID().c_str());
00124 }
00125 }
00126
00127 if (cd1->type == BodyTypes::ROBOT_ATTACHED && cd2->type == BodyTypes::ROBOT_ATTACHED)
00128 {
00129 if (cd1->ptr.ab->getAttachedLink() == cd2->ptr.ab->getAttachedLink())
00130 always_allow_collision = true;
00131 }
00132
00133
00134 if (always_allow_collision)
00135 return false;
00136
00137 if (cdata->req_->verbose)
00138 logDebug("Actually checking collisions between %s and %s", cd1->getID().c_str(), cd2->getID().c_str());
00139
00140
00141 std::size_t want_contact_count = 0;
00142 if (cdata->req_->contacts)
00143 if (cdata->res_->contact_count < cdata->req_->max_contacts)
00144 {
00145 std::size_t have;
00146 if (cd1->getID() < cd2->getID())
00147 {
00148 std::pair<std::string, std::string> cp(cd1->getID(), cd2->getID());
00149 have = cdata->res_->contacts.find(cp) != cdata->res_->contacts.end() ? cdata->res_->contacts[cp].size() : 0;
00150 }
00151 else
00152 {
00153 std::pair<std::string, std::string> cp(cd2->getID(), cd1->getID());
00154 have = cdata->res_->contacts.find(cp) != cdata->res_->contacts.end() ? cdata->res_->contacts[cp].size() : 0;
00155 }
00156 if (have < cdata->req_->max_contacts_per_pair)
00157 want_contact_count =
00158 std::min(cdata->req_->max_contacts_per_pair - have, cdata->req_->max_contacts - cdata->res_->contact_count);
00159 }
00160
00161 if (dcf)
00162 {
00163
00164 bool enable_cost = cdata->req_->cost;
00165 std::size_t num_max_cost_sources = cdata->req_->max_cost_sources;
00166 bool enable_contact = true;
00167 fcl::CollisionResult col_result;
00168 int num_contacts = fcl::collide(o1, o2, fcl::CollisionRequest(std::numeric_limits<size_t>::max(), enable_contact,
00169 num_max_cost_sources, enable_cost),
00170 col_result);
00171 if (num_contacts > 0)
00172 {
00173 if (cdata->req_->verbose)
00174 logInform("Found %d contacts between '%s' and '%s'. These contacts will be evaluated to check if they are "
00175 "accepted or not",
00176 num_contacts, cd1->getID().c_str(), cd2->getID().c_str());
00177 Contact c;
00178 const std::pair<std::string, std::string>& pc = cd1->getID() < cd2->getID() ?
00179 std::make_pair(cd1->getID(), cd2->getID()) :
00180 std::make_pair(cd2->getID(), cd1->getID());
00181 for (int i = 0; i < num_contacts; ++i)
00182 {
00183 fcl2contact(col_result.getContact(i), c);
00184
00185 if (dcf(c) == false)
00186 {
00187
00188 if (want_contact_count > 0)
00189 {
00190 --want_contact_count;
00191 cdata->res_->contacts[pc].push_back(c);
00192 cdata->res_->contact_count++;
00193 if (cdata->req_->verbose)
00194 logInform("Found unacceptable contact between '%s' and '%s'. Contact was stored.", cd1->getID().c_str(),
00195 cd2->getID().c_str());
00196 }
00197 else if (cdata->req_->verbose)
00198 logInform("Found unacceptable contact between '%s' (type '%s') and '%s' (type '%s'). Contact was stored.",
00199 cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(),
00200 cd2->getTypeString().c_str());
00201 cdata->res_->collision = true;
00202 if (want_contact_count == 0)
00203 break;
00204 }
00205 }
00206 }
00207
00208 if (enable_cost)
00209 {
00210 std::vector<fcl::CostSource> cost_sources;
00211 col_result.getCostSources(cost_sources);
00212
00213 CostSource cs;
00214 for (std::size_t i = 0; i < cost_sources.size(); ++i)
00215 {
00216 fcl2costsource(cost_sources[i], cs);
00217 cdata->res_->cost_sources.insert(cs);
00218 while (cdata->res_->cost_sources.size() > cdata->req_->max_cost_sources)
00219 cdata->res_->cost_sources.erase(--cdata->res_->cost_sources.end());
00220 }
00221 }
00222 }
00223 else
00224 {
00225 if (want_contact_count > 0)
00226 {
00227
00228 bool enable_cost = cdata->req_->cost;
00229 std::size_t num_max_cost_sources = cdata->req_->max_cost_sources;
00230 bool enable_contact = true;
00231
00232 fcl::CollisionResult col_result;
00233 int num_contacts = fcl::collide(
00234 o1, o2, fcl::CollisionRequest(want_contact_count, enable_contact, num_max_cost_sources, enable_cost),
00235 col_result);
00236 if (num_contacts > 0)
00237 {
00238 int num_contacts_initial = num_contacts;
00239
00240
00241 if (want_contact_count >= (std::size_t)num_contacts)
00242 want_contact_count -= num_contacts;
00243 else
00244 {
00245 num_contacts = want_contact_count;
00246 want_contact_count = 0;
00247 }
00248
00249 if (cdata->req_->verbose)
00250 logInform("Found %d contacts between '%s' (type '%s') and '%s' (type '%s'), which constitute a collision. %d "
00251 "contacts will be stored",
00252 num_contacts_initial, cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(),
00253 cd2->getTypeString().c_str(), num_contacts);
00254
00255 const std::pair<std::string, std::string>& pc = cd1->getID() < cd2->getID() ?
00256 std::make_pair(cd1->getID(), cd2->getID()) :
00257 std::make_pair(cd2->getID(), cd1->getID());
00258 cdata->res_->collision = true;
00259 for (int i = 0; i < num_contacts; ++i)
00260 {
00261 Contact c;
00262 fcl2contact(col_result.getContact(i), c);
00263 cdata->res_->contacts[pc].push_back(c);
00264 cdata->res_->contact_count++;
00265 }
00266 }
00267
00268 if (enable_cost)
00269 {
00270 std::vector<fcl::CostSource> cost_sources;
00271 col_result.getCostSources(cost_sources);
00272
00273 CostSource cs;
00274 for (std::size_t i = 0; i < cost_sources.size(); ++i)
00275 {
00276 fcl2costsource(cost_sources[i], cs);
00277 cdata->res_->cost_sources.insert(cs);
00278 while (cdata->res_->cost_sources.size() > cdata->req_->max_cost_sources)
00279 cdata->res_->cost_sources.erase(--cdata->res_->cost_sources.end());
00280 }
00281 }
00282 }
00283 else
00284 {
00285 bool enable_cost = cdata->req_->cost;
00286 std::size_t num_max_cost_sources = cdata->req_->max_cost_sources;
00287 bool enable_contact = false;
00288 fcl::CollisionResult col_result;
00289 int num_contacts =
00290 fcl::collide(o1, o2, fcl::CollisionRequest(1, enable_contact, num_max_cost_sources, enable_cost), col_result);
00291 if (num_contacts > 0)
00292 {
00293 cdata->res_->collision = true;
00294 if (cdata->req_->verbose)
00295 logInform("Found a contact between '%s' (type '%s') and '%s' (type '%s'), which constitutes a collision. "
00296 "Contact information is not stored.",
00297 cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(),
00298 cd2->getTypeString().c_str());
00299 }
00300
00301 if (enable_cost)
00302 {
00303 std::vector<fcl::CostSource> cost_sources;
00304 col_result.getCostSources(cost_sources);
00305
00306 CostSource cs;
00307 for (std::size_t i = 0; i < cost_sources.size(); ++i)
00308 {
00309 fcl2costsource(cost_sources[i], cs);
00310 cdata->res_->cost_sources.insert(cs);
00311 while (cdata->res_->cost_sources.size() > cdata->req_->max_cost_sources)
00312 cdata->res_->cost_sources.erase(--cdata->res_->cost_sources.end());
00313 }
00314 }
00315 }
00316 }
00317
00318 if (cdata->res_->collision)
00319 if (!cdata->req_->contacts || cdata->res_->contact_count >= cdata->req_->max_contacts)
00320 {
00321 if (!cdata->req_->cost)
00322 cdata->done_ = true;
00323 if (cdata->req_->verbose)
00324 logInform("Collision checking is considered complete (collision was found and %u contacts are stored)",
00325 (unsigned int)cdata->res_->contact_count);
00326 }
00327
00328 if (!cdata->done_ && cdata->req_->is_done)
00329 {
00330 cdata->done_ = cdata->req_->is_done(*cdata->res_);
00331 if (cdata->done_ && cdata->req_->verbose)
00332 logInform("Collision checking is considered complete due to external callback. %s was found. %u contacts are "
00333 "stored.",
00334 cdata->res_->collision ? "Collision" : "No collision", (unsigned int)cdata->res_->contact_count);
00335 }
00336
00337 return cdata->done_;
00338 }
00339
00340 struct FCLShapeCache
00341 {
00342 FCLShapeCache() : clean_count_(0)
00343 {
00344 }
00345
00346 void bumpUseCount(bool force = false)
00347 {
00348 clean_count_++;
00349
00350
00351 if (clean_count_ > MAX_CLEAN_COUNT || force)
00352 {
00353 clean_count_ = 0;
00354 unsigned int from = map_.size();
00355 for (std::map<boost::weak_ptr<const shapes::Shape>, FCLGeometryConstPtr>::iterator it = map_.begin();
00356 it != map_.end();)
00357 {
00358 std::map<boost::weak_ptr<const shapes::Shape>, FCLGeometryConstPtr>::iterator nit = it;
00359 ++nit;
00360 if (it->first.expired())
00361 map_.erase(it);
00362 it = nit;
00363 }
00364
00365
00366 }
00367 }
00368
00369 static const unsigned int MAX_CLEAN_COUNT = 100;
00370
00371 std::map<boost::weak_ptr<const shapes::Shape>, FCLGeometryConstPtr> map_;
00372 unsigned int clean_count_;
00373 boost::mutex lock_;
00374 };
00375
00376 bool distanceCallback(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* data, double& min_dist)
00377 {
00378 CollisionData* cdata = reinterpret_cast<CollisionData*>(data);
00379
00380 const CollisionGeometryData* cd1 = static_cast<const CollisionGeometryData*>(o1->collisionGeometry()->getUserData());
00381 const CollisionGeometryData* cd2 = static_cast<const CollisionGeometryData*>(o2->collisionGeometry()->getUserData());
00382
00383
00384 if (cd1->sameObject(*cd2))
00385 return false;
00386
00387
00388 if (cdata->active_components_only_)
00389 {
00390 const robot_model::LinkModel* l1 =
00391 cd1->type == BodyTypes::ROBOT_LINK ?
00392 cd1->ptr.link :
00393 (cd1->type == BodyTypes::ROBOT_ATTACHED ? cd1->ptr.ab->getAttachedLink() : NULL);
00394 const robot_model::LinkModel* l2 =
00395 cd2->type == BodyTypes::ROBOT_LINK ?
00396 cd2->ptr.link :
00397 (cd2->type == BodyTypes::ROBOT_ATTACHED ? cd2->ptr.ab->getAttachedLink() : NULL);
00398
00399
00400 if ((!l1 || cdata->active_components_only_->find(l1) == cdata->active_components_only_->end()) &&
00401 (!l2 || cdata->active_components_only_->find(l2) == cdata->active_components_only_->end()))
00402 {
00403 min_dist = cdata->res_->distance;
00404 return cdata->done_;
00405 }
00406 }
00407
00408
00409 bool always_allow_collision = false;
00410 if (cdata->acm_)
00411 {
00412 AllowedCollision::Type type;
00413
00414 bool found = cdata->acm_->getAllowedCollision(cd1->getID(), cd2->getID(), type);
00415 if (found)
00416 {
00417
00418 if (type == AllowedCollision::ALWAYS)
00419 {
00420 always_allow_collision = true;
00421 if (cdata->req_->verbose)
00422 logDebug("Collision between '%s' and '%s' is always allowed. No contacts are computed.", cd1->getID().c_str(),
00423 cd2->getID().c_str());
00424 }
00425 }
00426 }
00427
00428
00429 if (cd1->type == BodyTypes::ROBOT_LINK && cd2->type == BodyTypes::ROBOT_ATTACHED)
00430 {
00431 const std::set<std::string>& tl = cd2->ptr.ab->getTouchLinks();
00432 if (tl.find(cd1->getID()) != tl.end())
00433 {
00434 always_allow_collision = true;
00435 if (cdata->req_->verbose)
00436 logDebug("Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.",
00437 cd1->getID().c_str(), cd2->getID().c_str());
00438 }
00439 }
00440 else
00441 {
00442 if (cd2->type == BodyTypes::ROBOT_LINK && cd1->type == BodyTypes::ROBOT_ATTACHED)
00443 {
00444 const std::set<std::string>& tl = cd1->ptr.ab->getTouchLinks();
00445 if (tl.find(cd2->getID()) != tl.end())
00446 {
00447 always_allow_collision = true;
00448 if (cdata->req_->verbose)
00449 logDebug("Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.",
00450 cd2->getID().c_str(), cd1->getID().c_str());
00451 }
00452 }
00453 }
00454
00455 if (always_allow_collision)
00456 {
00457 min_dist = cdata->res_->distance;
00458 return cdata->done_;
00459 }
00460
00461 if (cdata->req_->verbose)
00462 logDebug("Actually checking collisions between %s and %s", cd1->getID().c_str(), cd2->getID().c_str());
00463
00464 fcl::DistanceResult dist_result;
00465 dist_result.update(cdata->res_->distance, NULL, NULL, fcl::DistanceResult::NONE,
00466 fcl::DistanceResult::NONE);
00467 double d = fcl::distance(o1, o2, fcl::DistanceRequest(), dist_result);
00468
00469 if (d < 0)
00470 {
00471 cdata->done_ = true;
00472 cdata->res_->distance = -1;
00473 }
00474 else
00475 {
00476 if (cdata->res_->distance > d)
00477 cdata->res_->distance = d;
00478 }
00479
00480 min_dist = cdata->res_->distance;
00481
00482 return cdata->done_;
00483 }
00484
00485
00486 template <typename BV, typename T>
00487 FCLShapeCache& GetShapeCache()
00488 {
00489 static FCLShapeCache cache;
00490 return cache;
00491 }
00492
00493 template <typename T1, typename T2>
00494 struct IfSameType
00495 {
00496 enum
00497 {
00498 value = 0
00499 };
00500 };
00501
00502 template <typename T>
00503 struct IfSameType<T, T>
00504 {
00505 enum
00506 {
00507 value = 1
00508 };
00509 };
00510
00511 template <typename BV, typename T>
00512 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const T* data, int shape_index)
00513 {
00514 FCLShapeCache& cache = GetShapeCache<BV, T>();
00515
00516 boost::weak_ptr<const shapes::Shape> wptr(shape);
00517 {
00518 boost::mutex::scoped_lock slock(cache.lock_);
00519 std::map<boost::weak_ptr<const shapes::Shape>, FCLGeometryConstPtr>::const_iterator cache_it =
00520 cache.map_.find(wptr);
00521 if (cache_it != cache.map_.end())
00522 {
00523 if (cache_it->second->collision_geometry_data_->ptr.raw == (void*)data)
00524 {
00525
00526
00527 return cache_it->second;
00528 }
00529 else if (cache_it->second.unique())
00530 {
00531 const_cast<FCLGeometry*>(cache_it->second.get())->updateCollisionGeometryData(data, shape_index, false);
00532
00533
00534 return cache_it->second;
00535 }
00536 }
00537 }
00538
00539
00540
00541
00542 if (IfSameType<T, robot_state::AttachedBody>::value == 1)
00543 {
00544
00545 FCLShapeCache& othercache = GetShapeCache<BV, World::Object>();
00546
00547
00548 othercache.lock_.lock();
00549 std::map<boost::weak_ptr<const shapes::Shape>, FCLGeometryConstPtr>::iterator cache_it = othercache.map_.find(wptr);
00550 if (cache_it != othercache.map_.end())
00551 {
00552 if (cache_it->second.unique())
00553 {
00554
00555 FCLGeometryConstPtr obj_cache = cache_it->second;
00556 othercache.map_.erase(cache_it);
00557 othercache.lock_.unlock();
00558
00559
00560 const_cast<FCLGeometry*>(obj_cache.get())->updateCollisionGeometryData(data, shape_index, true);
00561
00562
00563
00564
00565
00566 boost::mutex::scoped_lock slock(cache.lock_);
00567 cache.map_[wptr] = obj_cache;
00568 cache.bumpUseCount();
00569 return obj_cache;
00570 }
00571 }
00572 othercache.lock_.unlock();
00573 }
00574 else
00575
00576
00577
00578 if (IfSameType<T, World::Object>::value == 1)
00579 {
00580
00581 FCLShapeCache& othercache = GetShapeCache<BV, robot_state::AttachedBody>();
00582
00583
00584 othercache.lock_.lock();
00585 std::map<boost::weak_ptr<const shapes::Shape>, FCLGeometryConstPtr>::iterator cache_it = othercache.map_.find(wptr);
00586 if (cache_it != othercache.map_.end())
00587 {
00588 if (cache_it->second.unique())
00589 {
00590
00591 FCLGeometryConstPtr obj_cache = cache_it->second;
00592 othercache.map_.erase(cache_it);
00593 othercache.lock_.unlock();
00594
00595
00596 const_cast<FCLGeometry*>(obj_cache.get())->updateCollisionGeometryData(data, shape_index, true);
00597
00598
00599
00600
00601
00602
00603 boost::mutex::scoped_lock slock(cache.lock_);
00604 cache.map_[wptr] = obj_cache;
00605 cache.bumpUseCount();
00606 return obj_cache;
00607 }
00608 }
00609 othercache.lock_.unlock();
00610 }
00611
00612 fcl::CollisionGeometry* cg_g = NULL;
00613 if (shape->type == shapes::PLANE)
00614 {
00615
00616 switch (shape->type)
00617 {
00618 case shapes::PLANE:
00619 {
00620 const shapes::Plane* p = static_cast<const shapes::Plane*>(shape.get());
00621 cg_g = new fcl::Plane(p->a, p->b, p->c, p->d);
00622 }
00623 break;
00624 default:
00625 break;
00626 }
00627 }
00628 else
00629 {
00630 switch (shape->type)
00631 {
00632 case shapes::SPHERE:
00633 {
00634 const shapes::Sphere* s = static_cast<const shapes::Sphere*>(shape.get());
00635 cg_g = new fcl::Sphere(s->radius);
00636 }
00637 break;
00638 case shapes::BOX:
00639 {
00640 const shapes::Box* s = static_cast<const shapes::Box*>(shape.get());
00641 const double* size = s->size;
00642 cg_g = new fcl::Box(size[0], size[1], size[2]);
00643 }
00644 break;
00645 case shapes::CYLINDER:
00646 {
00647 const shapes::Cylinder* s = static_cast<const shapes::Cylinder*>(shape.get());
00648 cg_g = new fcl::Cylinder(s->radius, s->length);
00649 }
00650 break;
00651 case shapes::CONE:
00652 {
00653 const shapes::Cone* s = static_cast<const shapes::Cone*>(shape.get());
00654 cg_g = new fcl::Cone(s->radius, s->length);
00655 }
00656 break;
00657 case shapes::MESH:
00658 {
00659 fcl::BVHModel<BV>* g = new fcl::BVHModel<BV>();
00660 const shapes::Mesh* mesh = static_cast<const shapes::Mesh*>(shape.get());
00661 if (mesh->vertex_count > 0 && mesh->triangle_count > 0)
00662 {
00663 std::vector<fcl::Triangle> tri_indices(mesh->triangle_count);
00664 for (unsigned int i = 0; i < mesh->triangle_count; ++i)
00665 tri_indices[i] =
00666 fcl::Triangle(mesh->triangles[3 * i], mesh->triangles[3 * i + 1], mesh->triangles[3 * i + 2]);
00667
00668 std::vector<fcl::Vec3f> points(mesh->vertex_count);
00669 for (unsigned int i = 0; i < mesh->vertex_count; ++i)
00670 points[i] = fcl::Vec3f(mesh->vertices[3 * i], mesh->vertices[3 * i + 1], mesh->vertices[3 * i + 2]);
00671
00672 g->beginModel();
00673 g->addSubModel(points, tri_indices);
00674 g->endModel();
00675 }
00676 cg_g = g;
00677 }
00678 break;
00679 case shapes::OCTREE:
00680 {
00681 const shapes::OcTree* g = static_cast<const shapes::OcTree*>(shape.get());
00682 cg_g = new fcl::OcTree(g->octree);
00683 }
00684 break;
00685 default:
00686 logError("This shape type (%d) is not supported using FCL yet", (int)shape->type);
00687 cg_g = NULL;
00688 }
00689 }
00690 if (cg_g)
00691 {
00692 cg_g->computeLocalAABB();
00693 FCLGeometryConstPtr res(new FCLGeometry(cg_g, data, shape_index));
00694 boost::mutex::scoped_lock slock(cache.lock_);
00695 cache.map_[wptr] = res;
00696 cache.bumpUseCount();
00697 return res;
00698 }
00699 return FCLGeometryConstPtr();
00700 }
00701
00703 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const robot_model::LinkModel* link,
00704 int shape_index)
00705 {
00706 return createCollisionGeometry<fcl::OBBRSS, robot_model::LinkModel>(shape, link, shape_index);
00707 }
00708
00709 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const robot_state::AttachedBody* ab,
00710 int shape_index)
00711 {
00712 return createCollisionGeometry<fcl::OBBRSS, robot_state::AttachedBody>(shape, ab, shape_index);
00713 }
00714
00715 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const World::Object* obj)
00716 {
00717 return createCollisionGeometry<fcl::OBBRSS, World::Object>(shape, obj, 0);
00718 }
00719
00720 template <typename BV, typename T>
00721 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
00722 const T* data, int shape_index)
00723 {
00724 if (fabs(scale - 1.0) <= std::numeric_limits<double>::epsilon() &&
00725 fabs(padding) <= std::numeric_limits<double>::epsilon())
00726 return createCollisionGeometry<BV, T>(shape, data, shape_index);
00727 else
00728 {
00729 shapes::ShapePtr scaled_shape(shape->clone());
00730 scaled_shape->scaleAndPadd(scale, padding);
00731 return createCollisionGeometry<BV, T>(scaled_shape, data, shape_index);
00732 }
00733 }
00734
00735 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
00736 const robot_model::LinkModel* link, int shape_index)
00737 {
00738 return createCollisionGeometry<fcl::OBBRSS, robot_model::LinkModel>(shape, scale, padding, link, shape_index);
00739 }
00740
00741 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
00742 const robot_state::AttachedBody* ab, int shape_index)
00743 {
00744 return createCollisionGeometry<fcl::OBBRSS, robot_state::AttachedBody>(shape, scale, padding, ab, shape_index);
00745 }
00746
00747 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
00748 const World::Object* obj)
00749 {
00750 return createCollisionGeometry<fcl::OBBRSS, World::Object>(shape, scale, padding, obj, 0);
00751 }
00752
00753 void cleanCollisionGeometryCache()
00754 {
00755 FCLShapeCache& cache1 = GetShapeCache<fcl::OBBRSS, World::Object>();
00756 {
00757 boost::mutex::scoped_lock slock(cache1.lock_);
00758 cache1.bumpUseCount(true);
00759 }
00760 FCLShapeCache& cache2 = GetShapeCache<fcl::OBBRSS, robot_state::AttachedBody>();
00761 {
00762 boost::mutex::scoped_lock slock(cache2.lock_);
00763 cache2.bumpUseCount(true);
00764 }
00765 }
00766 }
00767
00768 void collision_detection::CollisionData::enableGroup(const robot_model::RobotModelConstPtr& kmodel)
00769 {
00770 if (kmodel->hasJointModelGroup(req_->group_name))
00771 active_components_only_ = &kmodel->getJointModelGroup(req_->group_name)->getUpdatedLinkModelsSet();
00772 else
00773 active_components_only_ = NULL;
00774 }
00775
00776 void collision_detection::FCLObject::registerTo(fcl::BroadPhaseCollisionManager* manager)
00777 {
00778 std::vector<fcl::CollisionObject*> collision_objects(collision_objects_.size());
00779 for (std::size_t i = 0; i < collision_objects_.size(); ++i)
00780 collision_objects[i] = collision_objects_[i].get();
00781 if (!collision_objects.empty())
00782 manager->registerObjects(collision_objects);
00783 }
00784
00785 void collision_detection::FCLObject::unregisterFrom(fcl::BroadPhaseCollisionManager* manager)
00786 {
00787 for (std::size_t i = 0; i < collision_objects_.size(); ++i)
00788 manager->unregisterObject(collision_objects_[i].get());
00789 }
00790
00791 void collision_detection::FCLObject::clear()
00792 {
00793 collision_objects_.clear();
00794 collision_geometry_.clear();
00795 }