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
00037 #include "collision_space_ccd/environmentBVH.h"
00038 #include <geometric_shapes/shape_operations.h>
00039 #include <ros/console.h>
00040 #include <cassert>
00041 #include <cstdio>
00042 #include <cmath>
00043 #include <algorithm>
00044 #include <map>
00045
00046 namespace collision_space_ccd
00047 {
00048 template<typename BV>
00049 EnvironmentModelBVH<BV>::EnvironmentModelBVH(void) : EnvironmentModel()
00050 {
00051 previous_set_robot_model_ = false;
00052 }
00053
00054 template<typename BV>
00055 EnvironmentModelBVH<BV>::~EnvironmentModelBVH(void)
00056 {
00057 freeMemory();
00058 }
00059
00060 template<typename BV>
00061 void EnvironmentModelBVH<BV>::freeMemory(void)
00062 {
00063 for(unsigned int j = 0; j < model_geom_.link_geom.size(); ++j)
00064 delete model_geom_.link_geom[j];
00065 model_geom_.link_geom.clear();
00066 for(typename std::map<std::string, CollisionNamespace*>::iterator it = coll_namespaces_.begin(); it != coll_namespaces_.end(); ++it)
00067 delete it->second;
00068 coll_namespaces_.clear();
00069 self_geom_manager.clear();
00070 }
00071
00072 template<typename BV>
00073 void EnvironmentModelBVH<BV>::setRobotModel(const planning_models::KinematicModel* model,
00074 const AllowedCollisionMatrix& allowed_collision_matrix,
00075 const std::map<std::string, double>& link_padding_map,
00076 double default_padding,
00077 double scale)
00078 {
00079 EnvironmentModel::setRobotModel(model, allowed_collision_matrix, link_padding_map, default_padding, scale);
00080 if(previous_set_robot_model_)
00081 {
00082 for(unsigned int j = 0; j < model_geom_.link_geom.size(); ++j)
00083 delete model_geom_.link_geom[j];
00084 model_geom_.link_geom.clear();
00085 self_geom_manager.clear();
00086 attached_bodies_in_collision_matrix_.clear();
00087 geom_lookup_map_.clear();
00088 }
00089 createBVHRobotModel();
00090 previous_set_robot_model_ = true;
00091 }
00092
00093 template<typename BV>
00094 void EnvironmentModelBVH<BV>::getAttachedBodyPoses(std::map<std::string, std::vector<btTransform> >& pose_map) const
00095 {
00096 pose_map.clear();
00097
00098 const unsigned int n = model_geom_.link_geom.size();
00099 for(unsigned int i = 0; i < n; ++i)
00100 {
00101 LinkGeom* lg = model_geom_.link_geom[i];
00102
00103
00104 const unsigned int nab = lg->att_bodies.size();
00105 std::vector<btTransform> nbt;
00106 for(unsigned int j = 0; j < nab; ++j)
00107 {
00108 for(unsigned int k = 0; k < lg->att_bodies[j]->geom.size(); k++)
00109 {
00110 nbt.push_back(lg->att_bodies[j]->geom[k]->t1);
00111 }
00112
00113 pose_map[lg->att_bodies[j]->att->getName()] = nbt;
00114 }
00115 }
00116 }
00117
00118 template<typename BV>
00119 void EnvironmentModelBVH<BV>::createBVHRobotModel()
00120 {
00121 for(unsigned int i = 0; i < robot_model_->getLinkModels().size(); ++i)
00122 {
00123
00124
00125
00126 const planning_models::KinematicModel::LinkModel* link = robot_model_->getLinkModels()[i];
00127 if (!link || !link->getLinkShape())
00128 continue;
00129
00130 LinkGeom* lg = new LinkGeom();
00131 lg->link = link;
00132 if(!default_collision_matrix_.getEntryIndex(link->getName(), lg->index))
00133 {
00134 ROS_WARN_STREAM("Link " << link->getName() << " not in provided collision matrix");
00135 }
00136
00137 double padd = default_robot_padding_;
00138 if(default_link_padding_map_.find(link->getName()) != default_link_padding_map_.end())
00139 {
00140 padd = default_link_padding_map_.find(link->getName())->second;
00141 }
00142 ROS_DEBUG_STREAM("Link " << link->getName() << " padding " << padd);
00143
00144 CollisionGeom* unpadd_g = createBVHGeom(link->getLinkShape(), 1.0, 0.0);
00145 assert(unpadd_g);
00146 lg->geom.push_back(unpadd_g);
00147 self_geom_manager.registerGeom(unpadd_g);
00148 geom_lookup_map_[unpadd_g] = std::pair<std::string, BodyType>(link->getName(), LINK);
00149
00150 CollisionGeom* padd_g = createBVHGeom(link->getLinkShape(), robot_scale_, padd);
00151 assert(padd_g);
00152 lg->padded_geom.push_back(padd_g);
00153 geom_lookup_map_[padd_g] = std::pair<std::string, BodyType>(link->getName(), LINK);
00154 const std::vector<planning_models::KinematicModel::AttachedBodyModel*>& attached_bodies = link->getAttachedBodyModels();
00155 for(unsigned int j = 0 ; j < attached_bodies.size() ; ++j)
00156 {
00157 padd = default_robot_padding_;
00158 if(default_link_padding_map_.find(attached_bodies[j]->getName()) != default_link_padding_map_.end())
00159 padd = default_link_padding_map_.find(attached_bodies[j]->getName())->second;
00160 else if(default_link_padding_map_.find("attached") != default_link_padding_map_.end())
00161 padd = default_link_padding_map_.find("attached")->second;
00162 addAttachedBody(lg, attached_bodies[j], padd);
00163 }
00164 model_geom_.link_geom.push_back(lg);
00165 }
00166 }
00167
00168 template<typename BV>
00169 CollisionGeom* EnvironmentModelBVH<BV>::createBVHGeom(const shapes::StaticShape *shape)
00170 {
00171 CollisionGeom* g = NULL;
00172 switch(shape->type)
00173 {
00174 case shapes::PLANE:
00175 {
00176
00177 ROS_WARN_STREAM("Plane is not implemented for BVH yet");
00178 }
00179 break;
00180 default:
00181 break;
00182 }
00183 return g;
00184 }
00185
00186 template<typename BV>
00187 CollisionGeom* EnvironmentModelBVH<BV>::createBVHGeom(const shapes::Shape *shape, double scale, double padding)
00188 {
00189 CollisionGeom* g = NULL;
00190 switch (shape->type)
00191 {
00192 case shapes::SPHERE:
00193 {
00194 g = makeSphere<BV>(static_cast<const shapes::Sphere*>(shape)->radius * scale + padding);
00195 }
00196 break;
00197 case shapes::BOX:
00198 {
00199 const double *size = static_cast<const shapes::Box*>(shape)->size;
00200 g = makeBox<BV>(size[0] * scale + padding * 2.0, size[1] * scale + padding * 2.0, size[2] * scale + padding * 2.0);
00201 }
00202 break;
00203 case shapes::CYLINDER:
00204 {
00205 g = makeCylinder<BV>(static_cast<const shapes::Cylinder*>(shape)->radius * scale + padding,
00206 static_cast<const shapes::Cylinder*>(shape)->length * scale + padding * 2.0);
00207 }
00208 break;
00209 case shapes::MESH:
00210 {
00211 const shapes::Mesh *mesh = static_cast<const shapes::Mesh*>(shape);
00212 if(mesh->vertexCount > 0 && mesh->triangleCount > 0)
00213 {
00214 std::vector<Triangle> tri_indices(mesh->triangleCount);
00215 for(unsigned int i = 0; i < mesh->triangleCount; ++i)
00216 tri_indices[i] = Triangle(mesh->triangles[3 * i], mesh->triangles[3 * i + 1], mesh->triangles[3 * i + 2]);
00217
00218 std::vector<Vec3f> points(mesh->vertexCount);
00219 double sx = 0.0, sy = 0.0, sz = 0.0;
00220 for(unsigned int i = 0; i < mesh->vertexCount; ++i)
00221 {
00222 points[i] = Vec3f(mesh->vertices[3 * i], mesh->vertices[3 * i + 1], mesh->vertices[3 * i + 2]);
00223 sx += points[i][0];
00224 sy += points[i][1];
00225 sz += points[i][2];
00226 }
00227
00228 sx /= (double)mesh->vertexCount;
00229 sy /= (double)mesh->vertexCount;
00230 sz /= (double)mesh->vertexCount;
00231
00232
00233 for(unsigned int i = 0; i < mesh->vertexCount; ++i)
00234 {
00235
00236 double dx = points[i][0] - sx;
00237 double dy = points[i][1] - sy;
00238 double dz = points[i][2] - sz;
00239
00240
00241
00242
00243 double ndx = ((dx > 0) ? dx+padding : dx-padding);
00244 double ndy = ((dy > 0) ? dy+padding : dy-padding);
00245 double ndz = ((dz > 0) ? dz+padding : dz-padding);
00246
00247
00248
00249 points[i] = Vec3f(sx + ndx, sy + ndy, sz + ndz);
00250 }
00251
00252 g = makeMesh<BV>(points, tri_indices);
00253 }
00254 }
00255
00256 default:
00257 break;
00258 }
00259 return g;
00260 }
00261
00262 template<typename BV>
00263 void EnvironmentModelBVH<BV>::updateGeom(CollisionGeom* geom, const btTransform &pose) const
00264 {
00265 geom->applyTransform(pose);
00266 }
00267
00268 template<typename BV>
00269 void EnvironmentModelBVH<BV>::updateAttachedBodies()
00270 {
00271 updateAttachedBodies(default_link_padding_map_);
00272 }
00273
00274
00275 template<typename BV>
00276 void EnvironmentModelBVH<BV>::updateAttachedBodies(const std::map<std::string, double>& link_padding_map)
00277 {
00278
00279 for(typename std::map<std::string, bool>::iterator it = attached_bodies_in_collision_matrix_.begin();
00280 it != attached_bodies_in_collision_matrix_.end();
00281 it++)
00282 {
00283 if(!default_collision_matrix_.removeEntry(it->first))
00284 {
00285 ROS_WARN_STREAM("No entry in default collision matrix for attached body " << it->first <<
00286 " when there really should be.");
00287 }
00288 }
00289
00290 attached_bodies_in_collision_matrix_.clear();
00291 for(unsigned int i = 0; i < model_geom_.link_geom.size(); ++i)
00292 {
00293 LinkGeom* lg = model_geom_.link_geom[i];
00294
00295 for(unsigned int j = 0; j < lg->att_bodies.size(); j++)
00296 {
00297 for(unsigned int k = 0; k < lg->att_bodies[j]->geom.size(); k++)
00298 {
00299 geom_lookup_map_.erase(lg->att_bodies[j]->geom[k]);
00300 self_geom_manager.unregisterGeom(lg->att_bodies[j]->geom[k]);
00301 }
00302 for(unsigned int k = 0; k < lg->att_bodies[j]->padded_geom.size(); k++)
00303 {
00304 geom_lookup_map_.erase(lg->att_bodies[j]->padded_geom[k]);
00305 }
00306 }
00307 lg->deleteAttachedBodies();
00308
00309
00310 const std::vector<planning_models::KinematicModel::AttachedBodyModel*>& attached_bodies = lg->link->getAttachedBodyModels();
00311 for(unsigned int j = 0; j < attached_bodies.size(); ++j)
00312 {
00313 double padd = default_robot_padding_;
00314 if(link_padding_map.find(attached_bodies[j]->getName()) != link_padding_map.end())
00315 {
00316 padd = link_padding_map.find(attached_bodies[j]->getName())->second;
00317 }
00318 else if (link_padding_map.find("attached") != link_padding_map.end())
00319 {
00320 padd = link_padding_map.find("attached")->second;
00321 }
00322 ROS_DEBUG_STREAM("Updating attached body " << attached_bodies[j]->getName());
00323 addAttachedBody(lg, attached_bodies[j], padd);
00324 }
00325 }
00326 }
00327
00328
00329 template<typename BV>
00330 void EnvironmentModelBVH<BV>::addAttachedBody(LinkGeom* lg,
00331 const planning_models::KinematicModel::AttachedBodyModel* attm,
00332 double padd)
00333 {
00334
00335 AttGeom* attg = new AttGeom();
00336 attg->att = attm;
00337
00338 if(!default_collision_matrix_.addEntry(attm->getName(), false))
00339 {
00340 ROS_WARN_STREAM("Must already have an entry in allowed collision matrix for " << attm->getName());
00341 }
00342 attached_bodies_in_collision_matrix_[attm->getName()] = true;
00343 default_collision_matrix_.getEntryIndex(attm->getName(), attg->index);
00344
00345 for(unsigned int i = 0; i < attm->getTouchLinks().size(); i++)
00346 {
00347 if(!default_collision_matrix_.changeEntry(attm->getName(), attm->getTouchLinks()[i], true))
00348 {
00349 ROS_WARN_STREAM("No entry in allowed collision matrix for " << attm->getName() << " and " << attm->getTouchLinks()[i]);
00350 }
00351 }
00352 for(unsigned int i = 0; i < attm->getShapes().size(); i++)
00353 {
00354 CollisionGeom* ga = createBVHGeom(attm->getShapes()[i], 1.0, 0.0);
00355 assert(ga);
00356 attg->geom.push_back(ga);
00357 self_geom_manager.registerGeom(ga);
00358 geom_lookup_map_[ga] = std::pair<std::string, BodyType>(attm->getName(), ATTACHED);
00359
00360 CollisionGeom* padd_ga = createBVHGeom(attm->getShapes()[i], robot_scale_, padd);
00361 assert(padd_ga);
00362 attg->padded_geom.push_back(padd_ga);
00363 geom_lookup_map_[padd_ga] = std::pair<std::string, BodyType>(attm->getName(), ATTACHED);
00364 }
00365 lg->att_bodies.push_back(attg);
00366 }
00367
00368
00369 template<typename BV>
00370 void EnvironmentModelBVH<BV>::setAttachedBodiesLinkPadding()
00371 {
00372 for(unsigned int i = 0; i < model_geom_.link_geom.size(); ++i)
00373 {
00374 LinkGeom* lg = model_geom_.link_geom[i];
00375
00376 const std::vector<planning_models::KinematicModel::AttachedBodyModel*>& attached_bodies = lg->link->getAttachedBodyModels();
00377 for(unsigned int j = 0; j < attached_bodies.size(); ++j)
00378 {
00379 double new_padd = -1.0;
00380 if(altered_link_padding_map_.find(attached_bodies[j]->getName()) != altered_link_padding_map_.end())
00381 {
00382 new_padd = altered_link_padding_map_.find(attached_bodies[j]->getName())->second;
00383 }
00384 else if(altered_link_padding_map_.find("attached") != altered_link_padding_map_.end())
00385 {
00386 new_padd = altered_link_padding_map_.find("attached")->second;
00387 }
00388 if(new_padd != -1.0)
00389 {
00390 for(unsigned int k = 0; k < attached_bodies[j]->getShapes().size(); k++)
00391 {
00392 geom_lookup_map_.erase(lg->att_bodies[j]->padded_geom[k]);
00393 delete lg->att_bodies[j]->padded_geom[k];
00394 CollisionGeom* padd_ga = createBVHGeom(attached_bodies[j]->getShapes()[k], robot_scale_, new_padd);
00395 assert(padd_ga);
00396 lg->att_bodies[j]->padded_geom[k] = padd_ga;
00397 geom_lookup_map_[padd_ga] = std::pair<std::string, BodyType>(attached_bodies[j]->getName(), ATTACHED);
00398 }
00399 }
00400 }
00401 }
00402 }
00403
00404 template<typename BV>
00405 void EnvironmentModelBVH<BV>::revertAttachedBodiesLinkPadding()
00406 {
00407 for(unsigned int i = 0; i < model_geom_.link_geom.size(); ++i)
00408 {
00409 LinkGeom* lg = model_geom_.link_geom[i];
00410
00411 const std::vector<planning_models::KinematicModel::AttachedBodyModel*>& attached_bodies = lg->link->getAttachedBodyModels();
00412 for(unsigned int j = 0; j < attached_bodies.size(); ++j)
00413 {
00414 double new_padd = -1.0;
00415 if(altered_link_padding_map_.find(attached_bodies[j]->getName()) != altered_link_padding_map_.end())
00416 {
00417 new_padd = default_link_padding_map_.find(attached_bodies[j]->getName())->second;
00418 }
00419 else if(altered_link_padding_map_.find("attached") != altered_link_padding_map_.end())
00420 {
00421 new_padd = default_link_padding_map_.find("attached")->second;
00422 }
00423 if(new_padd != -1.0)
00424 {
00425 for(unsigned int k = 0; k < attached_bodies[j]->getShapes().size(); k++)
00426 {
00427 geom_lookup_map_.erase(lg->att_bodies[j]->padded_geom[k]);
00428 delete lg->att_bodies[j]->padded_geom[k];
00429 CollisionGeom* padd_ga = createBVHGeom(attached_bodies[j]->getShapes()[k], robot_scale_, new_padd);
00430 assert(padd_ga);
00431 lg->att_bodies[j]->padded_geom[k] = padd_ga;
00432 geom_lookup_map_[padd_ga] = std::pair<std::string, BodyType>(attached_bodies[j]->getName(), ATTACHED);
00433 }
00434 }
00435 }
00436 }
00437 }
00438
00439
00440 template<typename BV>
00441 void EnvironmentModelBVH<BV>::updateRobotModel(const planning_models::KinematicState* state)
00442 {
00443 const unsigned int n = model_geom_.link_geom.size();
00444
00445 for(unsigned int i = 0; i < n; ++i)
00446 {
00447 const planning_models::KinematicState::LinkState* link_state = state->getLinkState(model_geom_.link_geom[i]->link->getName());
00448 if(link_state == NULL)
00449 {
00450 ROS_WARN_STREAM("No link state for link " << model_geom_.link_geom[i]->link->getName());
00451 continue;
00452 }
00453 updateGeom(model_geom_.link_geom[i]->geom[0], link_state->getGlobalCollisionBodyTransform());
00454 updateGeom(model_geom_.link_geom[i]->padded_geom[0], link_state->getGlobalCollisionBodyTransform());
00455 const std::vector<planning_models::KinematicState::AttachedBodyState*>& attached_bodies = link_state->getAttachedBodyStateVector();
00456 for(unsigned int j = 0; j < attached_bodies.size(); ++j)
00457 {
00458 for(unsigned int k = 0; k < attached_bodies[j]->getGlobalCollisionBodyTransforms().size(); k++)
00459 {
00460 updateGeom(model_geom_.link_geom[i]->att_bodies[j]->geom[k], attached_bodies[j]->getGlobalCollisionBodyTransforms()[k]);
00461 updateGeom(model_geom_.link_geom[i]->att_bodies[j]->padded_geom[k], attached_bodies[j]->getGlobalCollisionBodyTransforms()[k]);
00462 }
00463 }
00464 }
00465
00466 self_geom_manager.update();
00467 }
00468
00469 template<typename BV>
00470 void EnvironmentModelBVH<BV>::setAlteredLinkPadding(const std::map<std::string, double>& new_link_padding)
00471 {
00472
00473
00474 EnvironmentModel::setAlteredLinkPadding(new_link_padding);
00475
00476 for(unsigned int i = 0; i < model_geom_.link_geom.size(); i++)
00477 {
00478 LinkGeom* lg = model_geom_.link_geom[i];
00479
00480 if(altered_link_padding_map_.find(lg->link->getName()) != altered_link_padding_map_.end())
00481 {
00482 double new_padding = altered_link_padding_map_.find(lg->link->getName())->second;
00483 const planning_models::KinematicModel::LinkModel* link = lg->link;
00484 if (!link || !link->getLinkShape())
00485 {
00486 ROS_WARN_STREAM("Can't get kinematic model for link " << link->getName() << " to make new padding");
00487 continue;
00488 }
00489 ROS_DEBUG_STREAM("Setting padding for link " << lg->link->getName() << " from "
00490 << default_link_padding_map_[lg->link->getName()]
00491 << " to " << new_padding);
00492
00493 for(unsigned int j = 0; j < lg->padded_geom.size() ; ++j)
00494 {
00495 geom_lookup_map_.erase(lg->padded_geom[j]);
00496 delete lg->padded_geom[j];
00497 }
00498 lg->padded_geom.clear();
00499 CollisionGeom* g = createBVHGeom(link->getLinkShape(), robot_scale_, new_padding);
00500 assert(g);
00501 lg->padded_geom.push_back(g);
00502 geom_lookup_map_[g] = std::pair<std::string, BodyType>(link->getName(), LINK);
00503 }
00504 }
00505
00506 setAttachedBodiesLinkPadding();
00507 }
00508
00509 template<typename BV>
00510 void EnvironmentModelBVH<BV>::revertAlteredLinkPadding()
00511 {
00512 for(unsigned int i = 0; i < model_geom_.link_geom.size(); i++)
00513 {
00514 LinkGeom* lg = model_geom_.link_geom[i];
00515
00516 if(altered_link_padding_map_.find(lg->link->getName()) != altered_link_padding_map_.end())
00517 {
00518 double old_padding = default_link_padding_map_.find(lg->link->getName())->second;
00519 const planning_models::KinematicModel::LinkModel* link = lg->link;
00520 if (!link || !link->getLinkShape())
00521 {
00522 ROS_WARN_STREAM("Can't get kinematic model for link " << link->getName() << " to revert to old padding");
00523 continue;
00524 }
00525
00526 for(unsigned int j = 0; j < lg->padded_geom.size(); ++j)
00527 {
00528 geom_lookup_map_.erase(lg->padded_geom[j]);
00529 delete lg->padded_geom[j];
00530 }
00531 ROS_DEBUG_STREAM("Reverting padding for link " << lg->link->getName() << " from " << altered_link_padding_map_[lg->link->getName()]
00532 << " to " << old_padding);
00533 lg->padded_geom.clear();
00534 CollisionGeom* g = createBVHGeom(link->getLinkShape(), robot_scale_, old_padding);
00535 assert(g);
00536 lg->padded_geom.push_back(g);
00537 geom_lookup_map_[g] = std::pair<std::string, BodyType>(link->getName(), LINK);
00538 }
00539 }
00540 revertAttachedBodiesLinkPadding();
00541
00542
00543 EnvironmentModel::revertAlteredLinkPadding();
00544 }
00545
00546 template<typename BV>
00547 bool EnvironmentModelBVH<BV>::getCollisionContacts(const std::vector<AllowedContact>& allowedContacts, std::vector<Contact>& contacts, unsigned int max_count) const
00548 {
00549 contacts.clear();
00550 CollisionData cdata;
00551 cdata.allowed_collision_matrix = &getCurrentAllowedCollisionMatrix();
00552 cdata.geom_lookup_map = &geom_lookup_map_;
00553 cdata.contacts = &contacts;
00554 cdata.max_contacts = max_count;
00555 if (!allowedContacts.empty())
00556 cdata.allowed = &allowedContacts;
00557 contacts.clear();
00558 testCollision(&cdata);
00559 return cdata.collides;
00560 }
00561
00562 template<typename BV>
00563 bool EnvironmentModelBVH<BV>::getAllCollisionContacts(const std::vector<AllowedContact>& allowedContacts, std::vector<Contact>& contacts, unsigned int num_contacts_per_pair) const
00564 {
00565 contacts.clear();
00566 CollisionData cdata;
00567 cdata.geom_lookup_map = &geom_lookup_map_;
00568 cdata.allowed_collision_matrix = &getCurrentAllowedCollisionMatrix();
00569 cdata.contacts = &contacts;
00570 cdata.max_contacts = num_contacts_per_pair;
00571 if (!allowedContacts.empty())
00572 cdata.allowed = &allowedContacts;
00573 cdata.exhaustive = true;
00574 contacts.clear();
00575 testCollision(&cdata);
00576 return cdata.collides;
00577 }
00578
00579
00580 template<typename BV>
00581 bool EnvironmentModelBVH<BV>::isCollision(void) const
00582 {
00583 CollisionData cdata;
00584 cdata.allowed_collision_matrix = &getCurrentAllowedCollisionMatrix();
00585 cdata.geom_lookup_map = &geom_lookup_map_;
00586 testCollision(&cdata);
00587 return cdata.collides;
00588 }
00589
00590 template<typename BV>
00591 bool EnvironmentModelBVH<BV>::isSelfCollision(void) const
00592 {
00593 CollisionData cdata;
00594 cdata.geom_lookup_map = &geom_lookup_map_;
00595 cdata.allowed_collision_matrix = &getCurrentAllowedCollisionMatrix();
00596 testSelfCollision(&cdata);
00597 return cdata.collides;
00598 }
00599
00600 template<typename BV>
00601 bool EnvironmentModelBVH<BV>::isEnvironmentCollision(void) const
00602 {
00603 CollisionData cdata;
00604 cdata.geom_lookup_map = &geom_lookup_map_;
00605 cdata.allowed_collision_matrix = &getCurrentAllowedCollisionMatrix();
00606 testEnvironmentCollision(&cdata);
00607 return cdata.collides;
00608 }
00609
00610 template<typename BV>
00611 void EnvironmentModelBVH<BV>::testObjectCollision(CollisionNamespace *cn, CollisionData *cdata) const
00612 {
00613 cn->env_geom_manager.setup();
00614 for(int i = model_geom_.link_geom.size() - 1; i >= 0 && (!cdata->done || cdata->exhaustive); --i)
00615 {
00616 LinkGeom* lg = model_geom_.link_geom[i];
00617
00618 bool allowed = false;
00619 if(cdata->allowed_collision_matrix)
00620 {
00621 if(!cdata->allowed_collision_matrix->getAllowedCollision(cn->name, lg->link->getName(), allowed))
00622 {
00623 ROS_WARN_STREAM("No entry in cdata allowed collision matrix for " << cn->name << " and " << lg->link->getName());
00624 return;
00625 }
00626 }
00627
00628
00629 if(!allowed)
00630 {
00631 for(unsigned int j = 0; j < lg->padded_geom.size(); j++)
00632 {
00633
00634 unsigned int current_contacts_size = 0;
00635 if(cdata->contacts)
00636 {
00637 current_contacts_size = cdata->contacts->size();
00638 }
00639
00640
00641 cn->env_geom_manager.collide(lg->padded_geom[j], cdata);
00642
00643 if(cdata->contacts && cdata->contacts->size() > current_contacts_size)
00644 {
00645
00646 for(unsigned int k = current_contacts_size; k < cdata->contacts->size(); k++)
00647 {
00648 if(cdata->contacts->at(k).body_type_1 == OBJECT)
00649 {
00650 cdata->contacts->at(k).body_name_1 = cn->name;
00651 }
00652 else if(cdata->contacts->at(k).body_type_2 == OBJECT)
00653 {
00654 cdata->contacts->at(k).body_name_2 = cn->name;
00655 }
00656 else
00657 {
00658 ROS_WARN_STREAM("New contacts really should have an object as one of the bodies");
00659 }
00660 }
00661 }
00662 if(cdata->done)
00663 {
00664 return;
00665 }
00666 }
00667 }
00668
00669 for(unsigned int j = 0; j < lg->att_bodies.size(); j++)
00670 {
00671 std::string att_name = lg->att_bodies[j]->att->getName();
00672 allowed = false;
00673 if(cdata->allowed_collision_matrix)
00674 {
00675 if(!cdata->allowed_collision_matrix->getAllowedCollision(cn->name, att_name, allowed))
00676 {
00677 ROS_WARN_STREAM("No entry in current allowed collision matrix for " << cn->name << " and " << att_name);
00678 return;
00679 }
00680 }
00681 if(!allowed)
00682 {
00683 for(unsigned int k = 0; k < lg->att_bodies[j]->padded_geom.size(); k++)
00684 {
00685
00686 unsigned int current_contacts_size = 0;
00687 if(cdata->contacts)
00688 {
00689 current_contacts_size = cdata->contacts->size();
00690 }
00691
00692
00693 cn->env_geom_manager.collide(lg->att_bodies[j]->padded_geom[k], cdata);
00694
00695
00696 if(cdata->contacts && cdata->contacts->size() > current_contacts_size)
00697 {
00698
00699 for(unsigned int l = current_contacts_size; l < cdata->contacts->size(); l++)
00700 {
00701 if(cdata->contacts->at(l).body_type_1 == OBJECT)
00702 {
00703 cdata->contacts->at(l).body_name_1 = cn->name;
00704 }
00705 else if(cdata->contacts->at(l).body_type_2 == OBJECT)
00706 {
00707 cdata->contacts->at(l).body_name_2 = cn->name;
00708 }
00709 else
00710 {
00711 ROS_WARN_STREAM("New contacts really should have an object as one of the bodys");
00712 }
00713 }
00714 }
00715 if(cdata->done)
00716 {
00717 return;
00718 }
00719 }
00720 }
00721 }
00722 }
00723 }
00724
00725
00726 template<typename BV>
00727 void EnvironmentModelBVH<BV>::testGeomCollision(CollisionData* cdata, CollisionGeom* o1, CollisionGeom* o2)
00728 {
00729 if(cdata->done && !cdata->exhaustive)
00730 {
00731 return;
00732 }
00733
00734
00735 bool check_in_allowed_collision_matrix = true;
00736
00737 std::map<CollisionGeom*, std::pair<std::string, EnvironmentModelBVH::BodyType> >::const_iterator it1 = cdata->geom_lookup_map->find(o1);
00738 std::map<CollisionGeom*, std::pair<std::string, EnvironmentModelBVH::BodyType> >::const_iterator it2 = cdata->geom_lookup_map->find(o2);
00739
00740 if(it1 != cdata->geom_lookup_map->end())
00741 {
00742 cdata->body_name_1 = it1->second.first;
00743 cdata->body_type_1 = it1->second.second;
00744 }
00745 else
00746 {
00747 cdata->body_name_1 = "";
00748 cdata->body_type_1 = EnvironmentModelBVH::OBJECT;
00749 check_in_allowed_collision_matrix = false;
00750 }
00751
00752 if(it2 != cdata->geom_lookup_map->end())
00753 {
00754 cdata->body_name_2 = it2->second.first;
00755 cdata->body_type_2 = it2->second.second;
00756 }
00757 else
00758 {
00759 cdata->body_name_2 = "";
00760 cdata->body_type_2 = EnvironmentModelBVH::OBJECT;
00761 check_in_allowed_collision_matrix = false;
00762 }
00763
00764
00765 if (cdata->allowed_collision_matrix && check_in_allowed_collision_matrix)
00766 {
00767 bool allowed;
00768 if(!cdata->allowed_collision_matrix->getAllowedCollision(cdata->body_name_1, cdata->body_name_2, allowed))
00769 {
00770 ROS_WARN_STREAM("No entry in allowed collision matrix for " << cdata->body_name_1 << " and " << cdata->body_name_2);
00771 return;
00772 }
00773 if(allowed)
00774 {
00775 ROS_DEBUG_STREAM("Collision but allowed touch between " << cdata->body_name_1 << " and " << cdata->body_name_2);
00776 return;
00777 }
00778 else
00779 {
00780 ROS_DEBUG_STREAM("Collision and no allowed touch between " << cdata->body_name_1 << " and " << cdata->body_name_2);
00781 }
00782 }
00783
00784
00785 unsigned int num_contacts = 1;
00786 if(cdata->contacts)
00787 {
00788 num_contacts = cdata->max_contacts;
00789 }
00790 num_contacts = std::max(num_contacts, (unsigned int)1);
00791
00792 if(!cdata->contacts)
00793 {
00794 BVH_CollideResult res = o1->collide(o2);
00795 int num_c = res.numPairs();
00796
00797
00798 if(num_c)
00799 {
00800 cdata->collides = true;
00801 cdata->done = true;
00802 }
00803 }
00804 else
00805 {
00806 BVH_CollideResult res = o1->collide(o2, num_contacts);
00807 int num_c = res.numPairs();
00808
00809 BVHCollisionPair* pairs = res.collidePairs();
00810 for(int i = 0; i < num_c; ++i)
00811 {
00812
00813 if(!cdata->exhaustive && cdata->max_contacts > 0 && cdata->contacts->size() >= cdata->max_contacts)
00814 {
00815 break;
00816 }
00817
00818 Vec3f normal = pairs[i].normal;
00819 BVH_REAL depth = pairs[i].penetration_depth;
00820 Vec3f contact = pairs[i].contact_point;
00821
00822 ROS_DEBUG_STREAM("Contact at " << contact[0] << " " << contact[1] << " " << contact[2]);
00823 btVector3 pos(contact[0], contact[1], contact[2]);
00824
00825
00826
00827 if(cdata->allowed && (cdata->body_type_1 == EnvironmentModelBVH::OBJECT || cdata->body_type_2 == EnvironmentModelBVH::OBJECT))
00828 {
00829 std::string body_name;
00830 if(cdata->body_type_1 != EnvironmentModelBVH::OBJECT)
00831 {
00832 body_name = cdata->body_name_1;
00833 }
00834 else
00835 {
00836 body_name = cdata->body_name_2;
00837 }
00838
00839 bool allow = false;
00840 for(unsigned int j = 0; !allow && j < cdata->allowed->size(); ++j)
00841 {
00842 if(cdata->allowed->at(j).bound->containsPoint(pos) && cdata->allowed->at(j).depth > fabs(depth))
00843 {
00844 for(unsigned int k = 0; k < cdata->allowed->at(j).links.size(); ++k)
00845 {
00846 if(cdata->allowed->at(j).links[k] == body_name)
00847 {
00848 allow = true;
00849 break;
00850 }
00851 }
00852 }
00853 }
00854
00855 if (allow)
00856 continue;
00857 }
00858
00859 cdata->collides = true;
00860
00861 EnvironmentModelBVH::Contact add;
00862 add.pos = pos;
00863 add.normal = btVector3(normal[0], normal[1], normal[2]);
00864 add.depth = depth;
00865 add.body_name_1 = cdata->body_name_1;
00866 add.body_name_2 = cdata->body_name_2;
00867 add.body_type_1 = cdata->body_type_1;
00868 add.body_type_2 = cdata->body_type_2;
00869
00870 cdata->contacts->push_back(add);
00871 }
00872
00873 if (!cdata->exhaustive && cdata->max_contacts > 0 && cdata->contacts->size() >= cdata->max_contacts)
00874 cdata->done = true;
00875 }
00876 }
00877
00878 template<typename BV>
00879 void EnvironmentModelBVH<BV>::testCollision(CollisionData *cdata) const
00880 {
00881 testSelfCollision(cdata);
00882 testEnvironmentCollision(cdata);
00883 }
00884
00885 template<typename BV>
00886 void EnvironmentModelBVH<BV>::testSelfCollision(CollisionData *cdata) const
00887 {
00888 self_geom_manager.setup();
00889
00890 self_geom_manager.collide(cdata);
00891 }
00892
00893 template<typename BV>
00894 void EnvironmentModelBVH<BV>::testEnvironmentCollision(CollisionData *cdata) const
00895 {
00896
00897 for(typename std::map<std::string, CollisionNamespace*>::const_iterator it = coll_namespaces_.begin() ; it != coll_namespaces_.end() && !cdata->done ; ++it)
00898 {
00899 testObjectCollision(it->second, cdata);
00900 }
00901 }
00902
00903 template<typename BV>
00904 bool EnvironmentModelBVH<BV>::hasObject(const std::string& ns) const
00905 {
00906 if(coll_namespaces_.find(ns) != coll_namespaces_.end())
00907 {
00908 return true;
00909 }
00910 return false;
00911 }
00912
00913 template<typename BV>
00914 void EnvironmentModelBVH<BV>::addObjects(const std::string& ns, const std::vector<shapes::Shape*>& shapes, const std::vector<btTransform>& poses)
00915 {
00916 assert(shapes.size() == poses.size());
00917 typename std::map<std::string, CollisionNamespace*>::iterator it = coll_namespaces_.find(ns);
00918 CollisionNamespace* cn = NULL;
00919 if(it == coll_namespaces_.end())
00920 {
00921 cn = new CollisionNamespace(ns);
00922 coll_namespaces_[ns] = cn;
00923 default_collision_matrix_.addEntry(ns, false);
00924 }
00925 else
00926 {
00927 cn = it->second;
00928 }
00929
00930
00931 objects_->addObjectNamespace(ns);
00932
00933 unsigned int n = shapes.size();
00934 for(unsigned int i = 0; i < n; ++i)
00935 {
00936 CollisionGeom* g = createBVHGeom(shapes[i], 1.0, 0.0);
00937 assert(g);
00938 updateGeom(g, poses[i]);
00939 cn->geoms.push_back(g);
00940 cn->env_geom_manager.registerGeom(g);
00941 objects_->addObject(ns, shapes[i], poses[i]);
00942 }
00943 }
00944
00945 template<typename BV>
00946 void EnvironmentModelBVH<BV>::addObject(const std::string& ns, shapes::Shape* shape, const btTransform& pose)
00947 {
00948 typename std::map<std::string, CollisionNamespace*>::iterator it = coll_namespaces_.find(ns);
00949 CollisionNamespace* cn = NULL;
00950 if(it == coll_namespaces_.end())
00951 {
00952 cn = new CollisionNamespace(ns);
00953 coll_namespaces_[ns] = cn;
00954 default_collision_matrix_.addEntry(ns, false);
00955 }
00956 else
00957 cn = it->second;
00958
00959 CollisionGeom* g = createBVHGeom(shape, 1.0, 0.0);
00960 assert(g);
00961
00962 default_collision_matrix_.addEntry(ns, false);
00963
00964 updateGeom(g, pose);
00965 cn->geoms.push_back(g);
00966 objects_->addObject(ns, shape, pose);
00967 }
00968
00969 template<typename BV>
00970 void EnvironmentModelBVH<BV>::addObject(const std::string& ns, shapes::StaticShape* shape)
00971 {
00972 typename std::map<std::string, CollisionNamespace*>::iterator it = coll_namespaces_.find(ns);
00973 CollisionNamespace* cn = NULL;
00974 if(it == coll_namespaces_.end())
00975 {
00976 cn = new CollisionNamespace(ns);
00977 coll_namespaces_[ns] = cn;
00978 default_collision_matrix_.addEntry(ns, false);
00979 }
00980 else
00981 cn = it->second;
00982
00983 CollisionGeom* g = createBVHGeom(shape);
00984 assert(g);
00985 cn->geoms.push_back(g);
00986 objects_->addObject(ns, shape);
00987 }
00988
00989 template<typename BV>
00990 void EnvironmentModelBVH<BV>::clearObjects(void)
00991 {
00992 for(typename std::map<std::string, CollisionNamespace*>::iterator it = coll_namespaces_.begin(); it != coll_namespaces_.end(); ++it)
00993 {
00994 default_collision_matrix_.removeEntry(it->first);
00995 delete it->second;
00996 }
00997 coll_namespaces_.clear();
00998 objects_->clearObjects();
00999 }
01000
01001 template<typename BV>
01002 void EnvironmentModelBVH<BV>::clearObjects(const std::string &ns)
01003 {
01004 typename std::map<std::string, CollisionNamespace*>::iterator it = coll_namespaces_.find(ns);
01005 if(it != coll_namespaces_.end())
01006 {
01007 default_collision_matrix_.removeEntry(ns);
01008 delete it->second;
01009 coll_namespaces_.erase(ns);
01010 }
01011 objects_->clearObjects(ns);
01012 }
01013
01014 template<typename BV>
01015 CollisionGeom* EnvironmentModelBVH<BV>::copyGeom(CollisionGeom* geom) const
01016 {
01017
01018 return NULL;
01019 }
01020
01021 template<typename BV>
01022 EnvironmentModel* EnvironmentModelBVH<BV>::clone(void) const
01023 {
01024
01025 return NULL;
01026 }
01027
01028
01029 template<typename BV>
01030 void EnvironmentModelBVH<BV>::SAPManager::unregisterGeom(CollisionGeom* geom)
01031 {
01032 setup();
01033 CollisionGeom* found = NULL;
01034 std::vector<CollisionGeom*>::iterator pos_start1 = std::lower_bound(geoms_x.begin(), geoms_x.end(), geom, SortByXTest());
01035 std::vector<CollisionGeom*>::iterator pos_end1 = std::upper_bound(pos_start1, geoms_x.end(), geom, SortByXTest());
01036 while(pos_start1 < pos_end1)
01037 {
01038 if(*pos_start1 == geom)
01039 {
01040 found = *pos_start1;
01041 geoms_x.erase(pos_start1);
01042 break;
01043 }
01044 ++pos_start1;
01045 }
01046
01047 std::vector<CollisionGeom*>::iterator pos_start2 = std::lower_bound(geoms_y.begin(), geoms_y.end(), geom, SortByYTest());
01048 std::vector<CollisionGeom*>::iterator pos_end2 = std::upper_bound(pos_start2, geoms_y.end(), geom, SortByYTest());
01049 while(pos_start2 < pos_end2)
01050 {
01051 if(*pos_start2 == geom)
01052 {
01053 assert(found == *pos_start2);
01054 geoms_y.erase(pos_start2);
01055 break;
01056 }
01057 ++pos_start2;
01058 }
01059
01060 std::vector<CollisionGeom*>::iterator pos_start3 = std::lower_bound(geoms_z.begin(), geoms_z.end(), geom, SortByZTest());
01061 std::vector<CollisionGeom*>::iterator pos_end3 = std::upper_bound(pos_start3, geoms_z.end(), geom, SortByZTest());
01062 while(pos_start3 < pos_end3)
01063 {
01064 if(*pos_start3 == geom)
01065 {
01066 assert(found == *pos_start3);
01067 geoms_z.erase(pos_start3);
01068 break;
01069 }
01070 ++pos_start3;
01071 }
01072 }
01073
01074
01075
01076
01077 template<typename BV>
01078 void EnvironmentModelBVH<BV>::SAPManager::registerGeom(CollisionGeom* geom)
01079 {
01080 geoms_x.push_back(geom);
01081 geoms_y.push_back(geom);
01082 geoms_z.push_back(geom);
01083 setup_ = false;
01084 }
01085
01086 template<typename BV>
01087 void EnvironmentModelBVH<BV>::SAPManager::setup()
01088 {
01089 if(!setup_)
01090 {
01091 std::sort(geoms_x.begin(), geoms_x.end(), SortByXLow());
01092 std::sort(geoms_y.begin(), geoms_y.end(), SortByYLow());
01093 std::sort(geoms_z.begin(), geoms_z.end(), SortByZLow());
01094 setup_ = true;
01095 }
01096 }
01097
01098 template<typename BV>
01099 void EnvironmentModelBVH<BV>::SAPManager::update()
01100 {
01101 setup();
01102 }
01103
01104 template<typename BV>
01105 void EnvironmentModelBVH<BV>::SAPManager::clear()
01106 {
01107 geoms_x.clear();
01108 geoms_y.clear();
01109 geoms_z.clear();
01110 setup_ = false;
01111 }
01112
01113 template<typename BV>
01114 void EnvironmentModelBVH<BV>::SAPManager::getGeoms(std::vector<CollisionGeom*>& geoms) const
01115 {
01116 geoms.resize(geoms_x.size());
01117 for(unsigned int i = 0; i < geoms.size(); ++i)
01118 geoms[i] = geoms_x[i];
01119 }
01120
01121 template<typename BV>
01122 void EnvironmentModelBVH<BV>::SAPManager::collide(CollisionGeom* geom, CollisionData* cdata) const
01123 {
01124 static const unsigned int CUTOFF = 100;
01125
01126 assert(setup_);
01127
01128 std::vector<CollisionGeom*>::const_iterator pos_start1 = std::lower_bound(geoms_x.begin(), geoms_x.end(), geom, SortByXTest());
01129 if(pos_start1 != geoms_x.end())
01130 {
01131 std::vector<CollisionGeom*>::const_iterator pos_end1 = std::upper_bound(pos_start1, geoms_x.end(), geom, SortByXTest());
01132 unsigned int d1 = pos_end1 - pos_start1;
01133
01134 if(d1 > CUTOFF)
01135 {
01136 std::vector<CollisionGeom*>::const_iterator pos_start2 = std::lower_bound(geoms_y.begin(), geoms_y.end(), geom, SortByYTest());
01137 if(pos_start2 != geoms_y.end())
01138 {
01139 std::vector<CollisionGeom*>::const_iterator pos_end2 = std::upper_bound(pos_start2, geoms_y.end(), geom, SortByYTest());
01140 unsigned int d2 = pos_end2 - pos_start2;
01141
01142 if(d2 > CUTOFF)
01143 {
01144 std::vector<CollisionGeom*>::const_iterator pos_start3 = std::lower_bound(geoms_z.begin(), geoms_z.end(), geom, SortByZTest());
01145 if(pos_start3 != geoms_z.end())
01146 {
01147 std::vector<CollisionGeom*>::const_iterator pos_end3 = std::upper_bound(pos_start3, geoms_z.end(), geom, SortByZTest());
01148 unsigned int d3 = pos_end3 - pos_start3;
01149
01150 if(d3 > CUTOFF)
01151 {
01152 if(d3 <= d2 && d3 <= d1)
01153 checkColl(pos_start3, pos_end3, geom, cdata);
01154 else
01155 {
01156 if(d2 <= d3 && d2 <= d1)
01157 checkColl(pos_start2, pos_end2, geom, cdata);
01158 else
01159 checkColl(pos_start1, pos_end1, geom, cdata);
01160 }
01161 }
01162 else
01163 checkColl(pos_start3, pos_end3, geom, cdata);
01164 }
01165 }
01166 else
01167 checkColl(pos_start2, pos_end2, geom, cdata);
01168 }
01169 }
01170 else
01171 checkColl(pos_start1, pos_end1, geom, cdata);
01172 }
01173 }
01174
01175
01176 template<typename BV>
01177 void EnvironmentModelBVH<BV>::SAPManager::checkColl(std::vector<CollisionGeom*>::const_iterator pos_start, std::vector<CollisionGeom*>::const_iterator pos_end,
01178 CollisionGeom* geom, CollisionData* cdata) const
01179 {
01180 while(pos_start < pos_end)
01181 {
01182 if((*pos_start)->aabb.overlap(geom->aabb))
01183 {
01184 EnvironmentModelBVH<BV>::testGeomCollision(cdata, *pos_start, geom);
01185 if (cdata->done && !cdata->exhaustive)
01186 return;
01187 }
01188 pos_start++;
01189 }
01190 }
01191
01192 template<typename BV>
01193 void EnvironmentModelBVH<BV>::SAPManager::collide(CollisionData* cdata) const
01194 {
01195
01196
01197
01198 double delta_x = (geoms_x[geoms_x.size() - 1])->aabb.min_[0] - (geoms_x[0])->aabb.min_[0];
01199 double delta_y = (geoms_x[geoms_y.size() - 1])->aabb.min_[1] - (geoms_y[0])->aabb.min_[1];
01200 double delta_z = (geoms_z[geoms_z.size() - 1])->aabb.min_[2] - (geoms_z[0])->aabb.min_[2];
01201
01202 int axis = 0;
01203 if(delta_y > delta_x && delta_y > delta_z)
01204 axis = 1;
01205 else if(delta_z > delta_y && delta_z > delta_x)
01206 axis = 2;
01207
01208 int axis2 = (axis + 1 > 2) ? 0 : (axis + 1);
01209 int axis3 = (axis2 + 1 > 2) ? 0 : (axis2 + 1);
01210
01211 std::vector<CollisionGeom*>::const_iterator pos, run_pos, pos_end;
01212
01213 switch(axis)
01214 {
01215 case 0:
01216 pos = geoms_x.begin();
01217 pos_end = geoms_x.end();
01218 break;
01219 case 1:
01220 pos = geoms_y.begin();
01221 pos_end = geoms_y.end();
01222 break;
01223 case 2:
01224 pos = geoms_z.begin();
01225 pos_end = geoms_z.end();
01226 break;
01227 }
01228 run_pos = pos;
01229
01230 while((run_pos != pos_end) && (pos != pos_end))
01231 {
01232 CollisionGeom* geom = *(pos++);
01233
01234 while(1)
01235 {
01236 if((*run_pos)->aabb.min_[axis] < geom->aabb.min_[axis])
01237 {
01238 run_pos++;
01239 if(run_pos == pos_end) break;
01240 continue;
01241 }
01242 else
01243 {
01244 run_pos++;
01245 break;
01246 }
01247 }
01248
01249 if(run_pos != pos_end)
01250 {
01251 std::vector<CollisionGeom*>::const_iterator run_pos2 = run_pos;
01252
01253 while((*run_pos2)->aabb.min_[axis] <= geom->aabb.max_[axis])
01254 {
01255 CollisionGeom* geom2 = *run_pos2;
01256 run_pos2++;
01257
01258 if((geom->aabb.max_[axis2] >= geom2->aabb.min_[axis2]) && (geom2->aabb.max_[axis2] >= geom->aabb.min_[axis2]))
01259 {
01260 if((geom->aabb.max_[axis3] >= geom2->aabb.min_[axis3]) && (geom2->aabb.max_[axis3] >= geom->aabb.min_[axis3]))
01261 {
01262 EnvironmentModelBVH<BV>::testGeomCollision(cdata, geom, geom2);
01263 if (cdata->done && !cdata->exhaustive)
01264 return;
01265 }
01266 }
01267
01268 if(run_pos2 == pos_end) break;
01269 }
01270 }
01271 }
01272 }
01273
01274 template<typename BV>
01275 bool EnvironmentModelBVH<BV>::SAPManager::empty() const
01276 {
01277 return geoms_x.empty();
01278 }
01279
01280
01281 template class EnvironmentModelBVH<KDOP<16> >;
01282 template class EnvironmentModelBVH<KDOP<18> >;
01283 template class EnvironmentModelBVH<KDOP<24> >;
01284 template class EnvironmentModelBVH<OBB>;
01285 template class EnvironmentModelBVH<AABB>;
01286
01287
01288 }
