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
00038 #include "fcl/broadphase/broadphase_dynamic_AABB_tree.h"
00039
00040 namespace fcl
00041 {
00042
00043 void DynamicAABBTreeCollisionManager::registerObjects(const std::vector<CollisionObject*>& other_objs)
00044 {
00045 if(size() > 0)
00046 {
00047 BroadPhaseCollisionManager::registerObjects(other_objs);
00048 }
00049 else
00050 {
00051 std::vector<DynamicAABBNode*> leaves(other_objs.size());
00052 table.rehash(other_objs.size());
00053 for(size_t i = 0, size = other_objs.size(); i < size; ++i)
00054 {
00055 DynamicAABBNode* node = new DynamicAABBNode;
00056 node->bv = other_objs[i]->getAABB();
00057 node->parent = NULL;
00058 node->children[1] = NULL;
00059 node->data = other_objs[i];
00060 table[other_objs[i]] = node;
00061 leaves[i] = node;
00062 }
00063
00064 dtree.init(leaves, tree_init_level);
00065
00066 setup_ = true;
00067 }
00068 }
00069
00070 void DynamicAABBTreeCollisionManager::registerObject(CollisionObject* obj)
00071 {
00072 DynamicAABBNode* node = dtree.insert(obj->getAABB(), obj);
00073 table[obj] = node;
00074 }
00075
00076 void DynamicAABBTreeCollisionManager::unregisterObject(CollisionObject* obj)
00077 {
00078 DynamicAABBNode* node = table[obj];
00079 table.erase(obj);
00080 dtree.remove(node);
00081 }
00082
00083 void DynamicAABBTreeCollisionManager::setup()
00084 {
00085 if(!setup_)
00086 {
00087 int num = dtree.size();
00088 if(num == 0)
00089 {
00090 setup_ = true;
00091 return;
00092 }
00093
00094 int height = dtree.getMaxHeight();
00095
00096
00097 if(height - std::log((FCL_REAL)num) / std::log(2.0) < max_tree_nonbalanced_level)
00098 dtree.balanceIncremental(tree_incremental_balance_pass);
00099 else
00100 dtree.balanceTopdown();
00101
00102 setup_ = true;
00103 }
00104 }
00105
00106
00107 void DynamicAABBTreeCollisionManager::update()
00108 {
00109 for(DynamicAABBTable::const_iterator it = table.begin(); it != table.end(); ++it)
00110 {
00111 CollisionObject* obj = it->first;
00112 DynamicAABBNode* node = it->second;
00113 node->bv = obj->getAABB();
00114 }
00115
00116 dtree.refit();
00117 setup_ = false;
00118
00119 setup();
00120 }
00121
00122 void DynamicAABBTreeCollisionManager::update_(CollisionObject* updated_obj)
00123 {
00124 DynamicAABBTable::const_iterator it = table.find(updated_obj);
00125 if(it != table.end())
00126 {
00127 DynamicAABBNode* node = it->second;
00128 if(!node->bv.equal(updated_obj->getAABB()))
00129 dtree.update(node, updated_obj->getAABB());
00130 }
00131 setup_ = false;
00132 }
00133
00134 void DynamicAABBTreeCollisionManager::update(CollisionObject* updated_obj)
00135 {
00136 update_(updated_obj);
00137 setup();
00138 }
00139
00140 void DynamicAABBTreeCollisionManager::update(const std::vector<CollisionObject*>& updated_objs)
00141 {
00142 for(size_t i = 0, size = updated_objs.size(); i < size; ++i)
00143 update_(updated_objs[i]);
00144 setup();
00145 }
00146
00147 bool DynamicAABBTreeCollisionManager::collisionRecurse(DynamicAABBNode* root1, DynamicAABBNode* root2, void* cdata, CollisionCallBack callback) const
00148 {
00149 if(root1->isLeaf() && root2->isLeaf())
00150 {
00151 if(!root1->bv.overlap(root2->bv)) return false;
00152 return callback(static_cast<CollisionObject*>(root1->data), static_cast<CollisionObject*>(root2->data), cdata);
00153 }
00154
00155 if(!root1->bv.overlap(root2->bv)) return false;
00156
00157 if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size())))
00158 {
00159 if(collisionRecurse(root1->children[0], root2, cdata, callback))
00160 return true;
00161 if(collisionRecurse(root1->children[1], root2, cdata, callback))
00162 return true;
00163 }
00164 else
00165 {
00166 if(collisionRecurse(root1, root2->children[0], cdata, callback))
00167 return true;
00168 if(collisionRecurse(root1, root2->children[1], cdata, callback))
00169 return true;
00170 }
00171 return false;
00172 }
00173
00174 bool DynamicAABBTreeCollisionManager::collisionRecurse(DynamicAABBNode* root, CollisionObject* query, void* cdata, CollisionCallBack callback) const
00175 {
00176 if(root->isLeaf())
00177 {
00178 if(!root->bv.overlap(query->getAABB())) return false;
00179 return callback(static_cast<CollisionObject*>(root->data), query, cdata);
00180 }
00181
00182 if(!root->bv.overlap(query->getAABB())) return false;
00183
00184 int select_res = select(query->getAABB(), *(root->children[0]), *(root->children[1]));
00185
00186 if(collisionRecurse(root->children[select_res], query, cdata, callback))
00187 return true;
00188
00189 if(collisionRecurse(root->children[1-select_res], query, cdata, callback))
00190 return true;
00191
00192 return false;
00193 }
00194
00195 bool DynamicAABBTreeCollisionManager::selfCollisionRecurse(DynamicAABBNode* root, void* cdata, CollisionCallBack callback) const
00196 {
00197 if(root->isLeaf()) return false;
00198
00199 if(selfCollisionRecurse(root->children[0], cdata, callback))
00200 return true;
00201
00202 if(selfCollisionRecurse(root->children[1], cdata, callback))
00203 return true;
00204
00205 if(collisionRecurse(root->children[0], root->children[1], cdata, callback))
00206 return true;
00207
00208 return false;
00209 }
00210
00211 bool DynamicAABBTreeCollisionManager::distanceRecurse(DynamicAABBNode* root1, DynamicAABBNode* root2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
00212 {
00213 if(root1->isLeaf() && root2->isLeaf())
00214 {
00215 CollisionObject* root1_obj = static_cast<CollisionObject*>(root1->data);
00216 CollisionObject* root2_obj = static_cast<CollisionObject*>(root2->data);
00217 return callback(root1_obj, root2_obj, cdata, min_dist);
00218 }
00219
00220 if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size())))
00221 {
00222 FCL_REAL d1 = root2->bv.distance(root1->children[0]->bv);
00223 FCL_REAL d2 = root2->bv.distance(root1->children[1]->bv);
00224
00225 if(d2 < d1)
00226 {
00227 if(d2 < min_dist)
00228 {
00229 if(distanceRecurse(root1->children[1], root2, cdata, callback, min_dist))
00230 return true;
00231 }
00232
00233 if(d1 < min_dist)
00234 {
00235 if(distanceRecurse(root1->children[0], root2, cdata, callback, min_dist))
00236 return true;
00237 }
00238 }
00239 else
00240 {
00241 if(d1 < min_dist)
00242 {
00243 if(distanceRecurse(root1->children[0], root2, cdata, callback, min_dist))
00244 return true;
00245 }
00246
00247 if(d2 < min_dist)
00248 {
00249 if(distanceRecurse(root1->children[1], root2, cdata, callback, min_dist))
00250 return true;
00251 }
00252 }
00253 }
00254 else
00255 {
00256 FCL_REAL d1 = root1->bv.distance(root2->children[0]->bv);
00257 FCL_REAL d2 = root1->bv.distance(root2->children[1]->bv);
00258
00259 if(d2 < d1)
00260 {
00261 if(d2 < min_dist)
00262 {
00263 if(distanceRecurse(root1, root2->children[1], cdata, callback, min_dist))
00264 return true;
00265 }
00266
00267 if(d1 < min_dist)
00268 {
00269 if(distanceRecurse(root1, root2->children[0], cdata, callback, min_dist))
00270 return true;
00271 }
00272 }
00273 else
00274 {
00275 if(d1 < min_dist)
00276 {
00277 if(distanceRecurse(root1, root2->children[0], cdata, callback, min_dist))
00278 return true;
00279 }
00280
00281 if(d2 < min_dist)
00282 {
00283 if(distanceRecurse(root1, root2->children[1], cdata, callback, min_dist))
00284 return true;
00285 }
00286 }
00287 }
00288
00289 return false;
00290 }
00291
00292 bool DynamicAABBTreeCollisionManager::distanceRecurse(DynamicAABBNode* root, CollisionObject* query, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
00293 {
00294 if(root->isLeaf())
00295 {
00296 CollisionObject* root_obj = static_cast<CollisionObject*>(root->data);
00297 return callback(root_obj, query, cdata, min_dist);
00298 }
00299
00300 FCL_REAL d1 = query->getAABB().distance(root->children[0]->bv);
00301 FCL_REAL d2 = query->getAABB().distance(root->children[1]->bv);
00302
00303 if(d2 < d1)
00304 {
00305 if(d2 < min_dist)
00306 {
00307 if(distanceRecurse(root->children[1], query, cdata, callback, min_dist))
00308 return true;
00309 }
00310
00311 if(d1 < min_dist)
00312 {
00313 if(distanceRecurse(root->children[0], query, cdata, callback, min_dist))
00314 return true;
00315 }
00316 }
00317 else
00318 {
00319 if(d1 < min_dist)
00320 {
00321 if(distanceRecurse(root->children[0], query, cdata, callback, min_dist))
00322 return true;
00323 }
00324
00325 if(d2 < min_dist)
00326 {
00327 if(distanceRecurse(root->children[1], query, cdata, callback, min_dist))
00328 return true;
00329 }
00330 }
00331
00332 return false;
00333 }
00334
00335 bool DynamicAABBTreeCollisionManager::selfDistanceRecurse(DynamicAABBNode* root, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
00336 {
00337 if(root->isLeaf()) return false;
00338
00339 if(selfDistanceRecurse(root->children[0], cdata, callback, min_dist))
00340 return true;
00341
00342 if(selfDistanceRecurse(root->children[1], cdata, callback, min_dist))
00343 return true;
00344
00345 if(distanceRecurse(root->children[0], root->children[1], cdata, callback, min_dist))
00346 return true;
00347
00348 return false;
00349 }
00350
00351
00352 bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback)
00353 {
00354 if(root1->isLeaf() && !root2->hasChildren())
00355 {
00356 CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
00357
00358 if(!tree2->isNodeFree(root2) && !obj1->isFree())
00359 {
00360 OBB obb1, obb2;
00361 convertBV(root1->bv, Transform3f(), obb1);
00362 convertBV(root2_bv, tf2, obb2);
00363
00364 if(obb1.overlap(obb2))
00365 {
00366 Box* box = new Box();
00367 Transform3f box_tf;
00368 constructBox(root2_bv, tf2, *box, box_tf);
00369
00370 box->cost_density = root2->getOccupancy();
00371 box->threshold_occupied = tree2->getOccupancyThres();
00372
00373 CollisionObject obj2(boost::shared_ptr<CollisionGeometry>(box), box_tf);
00374 return callback(obj1, &obj2, cdata);
00375 }
00376 else return false;
00377 }
00378 else return false;
00379 }
00380
00381 OBB obb1, obb2;
00382 convertBV(root1->bv, Transform3f(), obb1);
00383 convertBV(root2_bv, tf2, obb2);
00384
00385 if(tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false;
00386
00387 if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
00388 {
00389 if(collisionRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback))
00390 return true;
00391 if(collisionRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback))
00392 return true;
00393 }
00394 else
00395 {
00396 for(unsigned int i = 0; i < 8; ++i)
00397 {
00398 if(root2->childExists(i))
00399 {
00400 const OcTree::OcTreeNode* child = root2->getChild(i);
00401 AABB child_bv;
00402 computeChildBV(root2_bv, i, child_bv);
00403
00404 if(collisionRecurse_(root1, tree2, child, child_bv, tf2, cdata, callback))
00405 return true;
00406 }
00407 }
00408 }
00409 return false;
00410 }
00411
00412 bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Vec3f& tf2, void* cdata, CollisionCallBack callback)
00413 {
00414 if(root1->isLeaf() && !root2->hasChildren())
00415 {
00416 CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
00417
00418 if(!tree2->isNodeFree(root2) && !obj1->isFree())
00419 {
00420 const AABB& root2_bv_t = translate(root2_bv, tf2);
00421 if(root1->bv.overlap(root2_bv_t))
00422 {
00423 Box* box = new Box();
00424 Transform3f box_tf;
00425 constructBox(root2_bv, tf2, *box, box_tf);
00426
00427 box->cost_density = root2->getOccupancy();
00428 box->threshold_occupied = tree2->getOccupancyThres();
00429
00430 CollisionObject obj2(boost::shared_ptr<CollisionGeometry>(box), box_tf);
00431 return callback(obj1, &obj2, cdata);
00432 }
00433 else return false;
00434 }
00435 else return false;
00436 }
00437
00438 const AABB& root2_bv_t = translate(root2_bv, tf2);
00439 if(tree2->isNodeFree(root2) || !root1->bv.overlap(root2_bv_t)) return false;
00440
00441 if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
00442 {
00443 if(collisionRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback))
00444 return true;
00445 if(collisionRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback))
00446 return true;
00447 }
00448 else
00449 {
00450 for(unsigned int i = 0; i < 8; ++i)
00451 {
00452 if(root2->childExists(i))
00453 {
00454 const OcTree::OcTreeNode* child = root2->getChild(i);
00455 AABB child_bv;
00456 computeChildBV(root2_bv, i, child_bv);
00457
00458 if(collisionRecurse_(root1, tree2, child, child_bv, tf2, cdata, callback))
00459 return true;
00460 }
00461 }
00462 }
00463 return false;
00464 }
00465
00466
00467
00468 bool DynamicAABBTreeCollisionManager::collisionRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback) const
00469 {
00470 if(tf2.getQuatRotation().isIdentity())
00471 return collisionRecurse_(root1, tree2, root2, root2_bv, tf2.getTranslation(), cdata, callback);
00472 else
00473 return collisionRecurse_(root1, tree2, root2, root2_bv, tf2, cdata, callback);
00474 }
00475
00476
00477 bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Vec3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist)
00478 {
00479 if(root1->isLeaf() && !root2->hasChildren())
00480 {
00481 if(tree2->isNodeOccupied(root2))
00482 {
00483 Box* box = new Box();
00484 Transform3f box_tf;
00485 constructBox(root2_bv, tf2, *box, box_tf);
00486 CollisionObject obj(boost::shared_ptr<CollisionGeometry>(box), box_tf);
00487 return callback(static_cast<CollisionObject*>(root1->data), &obj, cdata, min_dist);
00488 }
00489 else return false;
00490 }
00491
00492 if(!tree2->isNodeOccupied(root2)) return false;
00493
00494 if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
00495 {
00496 const AABB& aabb2 = translate(root2_bv, tf2);
00497 FCL_REAL d1 = aabb2.distance(root1->children[0]->bv);
00498 FCL_REAL d2 = aabb2.distance(root1->children[1]->bv);
00499
00500 if(d2 < d1)
00501 {
00502 if(d2 < min_dist)
00503 {
00504 if(distanceRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
00505 return true;
00506 }
00507
00508 if(d1 < min_dist)
00509 {
00510 if(distanceRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
00511 return true;
00512 }
00513 }
00514 else
00515 {
00516 if(d1 < min_dist)
00517 {
00518 if(distanceRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
00519 return true;
00520 }
00521
00522 if(d2 < min_dist)
00523 {
00524 if(distanceRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
00525 return true;
00526 }
00527 }
00528 }
00529 else
00530 {
00531 for(unsigned int i = 0; i < 8; ++i)
00532 {
00533 if(root2->childExists(i))
00534 {
00535 const OcTree::OcTreeNode* child = root2->getChild(i);
00536 AABB child_bv;
00537 computeChildBV(root2_bv, i, child_bv);
00538 const AABB& aabb2 = translate(child_bv, tf2);
00539
00540 FCL_REAL d = root1->bv.distance(aabb2);
00541
00542 if(d < min_dist)
00543 {
00544 if(distanceRecurse_(root1, tree2, child, child_bv, tf2, cdata, callback, min_dist))
00545 return true;
00546 }
00547 }
00548 }
00549 }
00550
00551 return false;
00552 }
00553
00554
00555 bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist)
00556 {
00557 if(root1->isLeaf() && !root2->hasChildren())
00558 {
00559 if(tree2->isNodeOccupied(root2))
00560 {
00561 Box* box = new Box();
00562 Transform3f box_tf;
00563 constructBox(root2_bv, tf2, *box, box_tf);
00564 CollisionObject obj(boost::shared_ptr<CollisionGeometry>(box), box_tf);
00565 return callback(static_cast<CollisionObject*>(root1->data), &obj, cdata, min_dist);
00566 }
00567 else return false;
00568 }
00569
00570 if(!tree2->isNodeOccupied(root2)) return false;
00571
00572 if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
00573 {
00574 AABB aabb2;
00575 convertBV(root2_bv, tf2, aabb2);
00576
00577 FCL_REAL d1 = aabb2.distance(root1->children[0]->bv);
00578 FCL_REAL d2 = aabb2.distance(root1->children[1]->bv);
00579
00580 if(d2 < d1)
00581 {
00582 if(d2 < min_dist)
00583 {
00584 if(distanceRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
00585 return true;
00586 }
00587
00588 if(d1 < min_dist)
00589 {
00590 if(distanceRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
00591 return true;
00592 }
00593 }
00594 else
00595 {
00596 if(d1 < min_dist)
00597 {
00598 if(distanceRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
00599 return true;
00600 }
00601
00602 if(d2 < min_dist)
00603 {
00604 if(distanceRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
00605 return true;
00606 }
00607 }
00608 }
00609 else
00610 {
00611 for(unsigned int i = 0; i < 8; ++i)
00612 {
00613 if(root2->childExists(i))
00614 {
00615 const OcTree::OcTreeNode* child = root2->getChild(i);
00616 AABB child_bv;
00617 computeChildBV(root2_bv, i, child_bv);
00618
00619 AABB aabb2;
00620 convertBV(child_bv, tf2, aabb2);
00621 FCL_REAL d = root1->bv.distance(aabb2);
00622
00623 if(d < min_dist)
00624 {
00625 if(distanceRecurse_(root1, tree2, child, child_bv, tf2, cdata, callback, min_dist))
00626 return true;
00627 }
00628 }
00629 }
00630 }
00631
00632 return false;
00633 }
00634
00635 bool DynamicAABBTreeCollisionManager::distanceRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
00636 {
00637 if(tf2.getQuatRotation().isIdentity())
00638 return distanceRecurse_(root1, tree2, root2, root2_bv, tf2.getTranslation(), cdata, callback, min_dist);
00639 else
00640 return distanceRecurse_(root1, tree2, root2, root2_bv, tf2, cdata, callback, min_dist);
00641 }
00642
00643
00644 }