broadphase_dynamic_AABB_tree.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00038 #include "fcl/broadphase/broadphase_dynamic_AABB_tree.h"
00039 
00040 #if FCL_HAVE_OCTOMAP
00041 #include "fcl/octree.h"
00042 #endif
00043 
00044 namespace fcl
00045 {
00046 
00047 namespace details
00048 {
00049 
00050 namespace dynamic_AABB_tree
00051 {
00052 
00053 #if FCL_HAVE_OCTOMAP
00054 bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback)
00055 {
00056   if(!root2)
00057   {
00058     if(root1->isLeaf())
00059     {
00060       CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
00061 
00062       if(!obj1->isFree())
00063       {
00064         OBB obb1, obb2;
00065         convertBV(root1->bv, Transform3f(), obb1);
00066         convertBV(root2_bv, tf2, obb2);
00067       
00068         if(obb1.overlap(obb2))
00069         {
00070           Box* box = new Box();
00071           Transform3f box_tf;
00072           constructBox(root2_bv, tf2, *box, box_tf);
00073 
00074           box->cost_density = tree2->getDefaultOccupancy();
00075           
00076           CollisionObject obj2(boost::shared_ptr<CollisionGeometry>(box), box_tf);
00077           return callback(obj1, &obj2, cdata);
00078         }
00079       }
00080     }
00081     else
00082     {
00083       if(collisionRecurse_(root1->children[0], tree2, NULL, root2_bv, tf2, cdata, callback))
00084         return true;
00085       if(collisionRecurse_(root1->children[1], tree2, NULL, root2_bv, tf2, cdata, callback))
00086         return true;
00087     }
00088     
00089     return false;
00090   }
00091   else if(root1->isLeaf() && !root2->hasChildren())
00092   {
00093     CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
00094 
00095     if(!tree2->isNodeFree(root2) && !obj1->isFree())
00096     {
00097       OBB obb1, obb2;
00098       convertBV(root1->bv, Transform3f(), obb1);
00099       convertBV(root2_bv, tf2, obb2);
00100       
00101       if(obb1.overlap(obb2))
00102       {
00103         Box* box = new Box();
00104         Transform3f box_tf;
00105         constructBox(root2_bv, tf2, *box, box_tf);
00106 
00107         box->cost_density = root2->getOccupancy();
00108         box->threshold_occupied = tree2->getOccupancyThres();
00109 
00110         CollisionObject obj2(boost::shared_ptr<CollisionGeometry>(box), box_tf);
00111         return callback(obj1, &obj2, cdata);
00112       }
00113       else return false;
00114     }
00115     else return false;
00116   }
00117 
00118   OBB obb1, obb2;
00119   convertBV(root1->bv, Transform3f(), obb1);
00120   convertBV(root2_bv, tf2, obb2);
00121 
00122   if(tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false;
00123 
00124   if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
00125   {
00126     if(collisionRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback))
00127       return true;
00128     if(collisionRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback))
00129       return true;
00130   }
00131   else
00132   {
00133     for(unsigned int i = 0; i < 8; ++i)
00134     {
00135       if(root2->childExists(i))
00136       {
00137         const OcTree::OcTreeNode* child = root2->getChild(i);
00138         AABB child_bv;
00139         computeChildBV(root2_bv, i, child_bv);
00140 
00141         if(collisionRecurse_(root1, tree2, child, child_bv, tf2, cdata, callback))
00142           return true;
00143       }
00144       else
00145       {
00146         AABB child_bv;
00147         computeChildBV(root2_bv, i, child_bv);
00148         if(collisionRecurse_(root1, tree2, NULL, child_bv, tf2, cdata, callback))
00149           return true;
00150       }
00151     }
00152   }
00153   return false;
00154 }
00155 
00156 bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Vec3f& tf2, void* cdata, CollisionCallBack callback)
00157 {
00158   if(!root2)
00159   {
00160     if(root1->isLeaf())
00161     {
00162       CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
00163   
00164       if(!obj1->isFree())
00165       {      
00166         const AABB& root2_bv_t = translate(root2_bv, tf2);
00167         if(root1->bv.overlap(root2_bv_t))
00168         {
00169           Box* box = new Box();
00170           Transform3f box_tf;
00171           constructBox(root2_bv, tf2, *box, box_tf);
00172 
00173           box->cost_density = tree2->getOccupancyThres(); // thresholds are 0, 1, so uncertain
00174 
00175           CollisionObject obj2(boost::shared_ptr<CollisionGeometry>(box), box_tf);
00176           return callback(obj1, &obj2, cdata);
00177         }
00178       }
00179     }
00180     else
00181     {
00182       if(collisionRecurse_(root1->children[0], tree2, NULL, root2_bv, tf2, cdata, callback))
00183         return true;
00184       if(collisionRecurse_(root1->children[1], tree2, NULL, root2_bv, tf2, cdata, callback))
00185         return true;
00186     }
00187 
00188     return false;
00189   }
00190   else if(root1->isLeaf() && !root2->hasChildren())
00191   {
00192     CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
00193   
00194     if(!tree2->isNodeFree(root2) && !obj1->isFree())
00195     {      
00196       const AABB& root2_bv_t = translate(root2_bv, tf2);
00197       if(root1->bv.overlap(root2_bv_t))
00198       {
00199         Box* box = new Box();
00200         Transform3f box_tf;
00201         constructBox(root2_bv, tf2, *box, box_tf);
00202 
00203         box->cost_density = root2->getOccupancy();
00204         box->threshold_occupied = tree2->getOccupancyThres();
00205 
00206         CollisionObject obj2(boost::shared_ptr<CollisionGeometry>(box), box_tf);
00207         return callback(obj1, &obj2, cdata);
00208       }
00209       else return false;
00210     }
00211     else return false;
00212   }
00213 
00214   const AABB& root2_bv_t = translate(root2_bv, tf2);
00215   if(tree2->isNodeFree(root2) || !root1->bv.overlap(root2_bv_t)) return false;
00216 
00217   if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
00218   {
00219     if(collisionRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback))
00220       return true;
00221     if(collisionRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback))
00222       return true;
00223   }
00224   else
00225   {
00226     for(unsigned int i = 0; i < 8; ++i)
00227     {
00228       if(root2->childExists(i))
00229       {
00230         const OcTree::OcTreeNode* child = root2->getChild(i);
00231         AABB child_bv;
00232         computeChildBV(root2_bv, i, child_bv);
00233 
00234         if(collisionRecurse_(root1, tree2, child, child_bv, tf2, cdata, callback))
00235           return true;
00236       }
00237       else
00238       {
00239         AABB child_bv;
00240         computeChildBV(root2_bv, i, child_bv);
00241         if(collisionRecurse_(root1, tree2, NULL, child_bv, tf2, cdata, callback))
00242           return true;
00243       }
00244     }
00245   }
00246   return false;
00247 }
00248 
00249 
00250 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)
00251 {
00252   if(root1->isLeaf() && !root2->hasChildren())
00253   {
00254     if(tree2->isNodeOccupied(root2))
00255     {
00256       Box* box = new Box();
00257       Transform3f box_tf;
00258       constructBox(root2_bv, tf2, *box, box_tf);
00259       CollisionObject obj(boost::shared_ptr<CollisionGeometry>(box), box_tf);
00260       return callback(static_cast<CollisionObject*>(root1->data), &obj, cdata, min_dist);
00261     }
00262     else return false;
00263   }
00264   
00265   if(!tree2->isNodeOccupied(root2)) return false;
00266 
00267   if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
00268   {
00269     AABB aabb2;
00270     convertBV(root2_bv, tf2, aabb2);
00271     
00272     FCL_REAL d1 = aabb2.distance(root1->children[0]->bv);
00273     FCL_REAL d2 = aabb2.distance(root1->children[1]->bv);
00274 
00275     if(d2 < d1)
00276     {
00277       if(d2 < min_dist)
00278       {
00279         if(distanceRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
00280           return true;
00281       }
00282 
00283       if(d1 < min_dist)
00284       {
00285         if(distanceRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
00286           return true;
00287       }
00288     }
00289     else
00290     {
00291       if(d1 < min_dist)
00292       {
00293         if(distanceRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
00294           return true;
00295       }
00296       
00297       if(d2 < min_dist)
00298       {
00299         if(distanceRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
00300           return true;
00301       }
00302     }
00303   }
00304   else
00305   {
00306     for(unsigned int i = 0; i < 8; ++i)
00307     {
00308       if(root2->childExists(i))
00309       {
00310         const OcTree::OcTreeNode* child = root2->getChild(i);
00311         AABB child_bv;
00312         computeChildBV(root2_bv, i, child_bv);
00313 
00314         AABB aabb2;
00315         convertBV(child_bv, tf2, aabb2);
00316         FCL_REAL d = root1->bv.distance(aabb2);
00317 
00318         if(d < min_dist)
00319         {
00320           if(distanceRecurse_(root1, tree2, child, child_bv, tf2, cdata, callback, min_dist))
00321             return true;
00322         }
00323       }
00324     }
00325   }
00326 
00327   return false;
00328 }
00329 
00330 bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback)
00331 {
00332   if(tf2.getQuatRotation().isIdentity())
00333     return collisionRecurse_(root1, tree2, root2, root2_bv, tf2.getTranslation(), cdata, callback);
00334   else // has rotation
00335     return collisionRecurse_(root1, tree2, root2, root2_bv, tf2, cdata, callback);
00336 }
00337 
00338 
00339 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)
00340 {
00341   if(root1->isLeaf() && !root2->hasChildren())
00342   {
00343     if(tree2->isNodeOccupied(root2))
00344     {
00345       Box* box = new Box();
00346       Transform3f box_tf;
00347       constructBox(root2_bv, tf2, *box, box_tf);
00348       CollisionObject obj(boost::shared_ptr<CollisionGeometry>(box), box_tf);
00349       return callback(static_cast<CollisionObject*>(root1->data), &obj, cdata, min_dist);
00350     }
00351     else return false;
00352   }
00353   
00354   if(!tree2->isNodeOccupied(root2)) return false;
00355 
00356   if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
00357   {
00358     const AABB& aabb2 = translate(root2_bv, tf2);
00359     FCL_REAL d1 = aabb2.distance(root1->children[0]->bv);
00360     FCL_REAL d2 = aabb2.distance(root1->children[1]->bv);
00361 
00362     if(d2 < d1)
00363     {
00364       if(d2 < min_dist)
00365       {
00366         if(distanceRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
00367           return true;
00368       }
00369 
00370       if(d1 < min_dist)
00371       {
00372         if(distanceRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
00373           return true;
00374       }
00375     }
00376     else
00377     {
00378       if(d1 < min_dist)
00379       {
00380         if(distanceRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
00381           return true;
00382       }
00383       
00384       if(d2 < min_dist)
00385       {
00386         if(distanceRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
00387           return true;
00388       }
00389     }
00390   }
00391   else
00392   {
00393     for(unsigned int i = 0; i < 8; ++i)
00394     {
00395       if(root2->childExists(i))
00396       {
00397         const OcTree::OcTreeNode* child = root2->getChild(i);
00398         AABB child_bv;
00399         computeChildBV(root2_bv, i, child_bv);
00400         const AABB& aabb2 = translate(child_bv, tf2);
00401    
00402         FCL_REAL d = root1->bv.distance(aabb2);
00403 
00404         if(d < min_dist)
00405         {
00406           if(distanceRecurse_(root1, tree2, child, child_bv, tf2, cdata, callback, min_dist))
00407             return true;
00408         }
00409       }
00410     }
00411   }
00412 
00413   return false;
00414 }
00415 
00416 
00417 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)
00418 {
00419   if(tf2.getQuatRotation().isIdentity())
00420     return distanceRecurse_(root1, tree2, root2, root2_bv, tf2.getTranslation(), cdata, callback, min_dist);
00421   else
00422     return distanceRecurse_(root1, tree2, root2, root2_bv, tf2, cdata, callback, min_dist);
00423 }
00424 
00425 #endif
00426 
00427 bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, DynamicAABBTreeCollisionManager::DynamicAABBNode* root2, void* cdata, CollisionCallBack callback)
00428 {
00429   if(root1->isLeaf() && root2->isLeaf())
00430   {
00431     if(!root1->bv.overlap(root2->bv)) return false;
00432     return callback(static_cast<CollisionObject*>(root1->data), static_cast<CollisionObject*>(root2->data), cdata);
00433   }
00434     
00435   if(!root1->bv.overlap(root2->bv)) return false;
00436     
00437   if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size())))
00438   {
00439     if(collisionRecurse(root1->children[0], root2, cdata, callback))
00440       return true;
00441     if(collisionRecurse(root1->children[1], root2, cdata, callback))
00442       return true;
00443   }
00444   else
00445   {
00446     if(collisionRecurse(root1, root2->children[0], cdata, callback))
00447       return true;
00448     if(collisionRecurse(root1, root2->children[1], cdata, callback))
00449       return true;
00450   }
00451   return false;
00452 }
00453 
00454 bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, CollisionObject* query, void* cdata, CollisionCallBack callback)
00455 {
00456   if(root->isLeaf())
00457   {
00458     if(!root->bv.overlap(query->getAABB())) return false;
00459     return callback(static_cast<CollisionObject*>(root->data), query, cdata);
00460   }
00461     
00462   if(!root->bv.overlap(query->getAABB())) return false;
00463 
00464   int select_res = select(query->getAABB(), *(root->children[0]), *(root->children[1]));
00465     
00466   if(collisionRecurse(root->children[select_res], query, cdata, callback))
00467     return true;
00468     
00469   if(collisionRecurse(root->children[1-select_res], query, cdata, callback))
00470     return true;
00471 
00472   return false;
00473 }
00474 
00475 bool selfCollisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, void* cdata, CollisionCallBack callback)
00476 {
00477   if(root->isLeaf()) return false;
00478 
00479   if(selfCollisionRecurse(root->children[0], cdata, callback))
00480     return true;
00481 
00482   if(selfCollisionRecurse(root->children[1], cdata, callback))
00483     return true;
00484 
00485   if(collisionRecurse(root->children[0], root->children[1], cdata, callback))
00486     return true;
00487 
00488   return false;
00489 }
00490 
00491 bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, DynamicAABBTreeCollisionManager::DynamicAABBNode* root2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist)
00492 {
00493   if(root1->isLeaf() && root2->isLeaf())
00494   {
00495     CollisionObject* root1_obj = static_cast<CollisionObject*>(root1->data);
00496     CollisionObject* root2_obj = static_cast<CollisionObject*>(root2->data);
00497     return callback(root1_obj, root2_obj, cdata, min_dist);
00498   }
00499 
00500   if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size())))
00501   {
00502     FCL_REAL d1 = root2->bv.distance(root1->children[0]->bv);
00503     FCL_REAL d2 = root2->bv.distance(root1->children[1]->bv);
00504       
00505     if(d2 < d1)
00506     {
00507       if(d2 < min_dist)
00508       {
00509         if(distanceRecurse(root1->children[1], root2, cdata, callback, min_dist))
00510           return true;
00511       }
00512         
00513       if(d1 < min_dist)
00514       {
00515         if(distanceRecurse(root1->children[0], root2, cdata, callback, min_dist))
00516           return true;
00517       }
00518     }
00519     else
00520     {
00521       if(d1 < min_dist)
00522       {
00523         if(distanceRecurse(root1->children[0], root2, cdata, callback, min_dist))
00524           return true;
00525       }
00526 
00527       if(d2 < min_dist)
00528       {
00529         if(distanceRecurse(root1->children[1], root2, cdata, callback, min_dist))
00530           return true;
00531       }
00532     }
00533   }
00534   else
00535   {
00536     FCL_REAL d1 = root1->bv.distance(root2->children[0]->bv);
00537     FCL_REAL d2 = root1->bv.distance(root2->children[1]->bv);
00538       
00539     if(d2 < d1)
00540     {
00541       if(d2 < min_dist)
00542       {
00543         if(distanceRecurse(root1, root2->children[1], cdata, callback, min_dist))
00544           return true;
00545       }
00546         
00547       if(d1 < min_dist)
00548       {
00549         if(distanceRecurse(root1, root2->children[0], cdata, callback, min_dist))
00550           return true;
00551       }
00552     }
00553     else
00554     {
00555       if(d1 < min_dist)
00556       {
00557         if(distanceRecurse(root1, root2->children[0], cdata, callback, min_dist))
00558           return true;
00559       }
00560 
00561       if(d2 < min_dist)
00562       {
00563         if(distanceRecurse(root1, root2->children[1], cdata, callback, min_dist))
00564           return true;
00565       }
00566     }
00567   }
00568 
00569   return false;
00570 }
00571 
00572 bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, CollisionObject* query, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist)
00573 { 
00574   if(root->isLeaf())
00575   {
00576     CollisionObject* root_obj = static_cast<CollisionObject*>(root->data);
00577     return callback(root_obj, query, cdata, min_dist); 
00578   }
00579 
00580   FCL_REAL d1 = query->getAABB().distance(root->children[0]->bv);
00581   FCL_REAL d2 = query->getAABB().distance(root->children[1]->bv);
00582     
00583   if(d2 < d1)
00584   {
00585     if(d2 < min_dist)
00586     {
00587       if(distanceRecurse(root->children[1], query, cdata, callback, min_dist))
00588         return true;
00589     }
00590 
00591     if(d1 < min_dist)
00592     {
00593       if(distanceRecurse(root->children[0], query, cdata, callback, min_dist))
00594         return true;
00595     }
00596   }
00597   else
00598   {
00599     if(d1 < min_dist)
00600     {
00601       if(distanceRecurse(root->children[0], query, cdata, callback, min_dist))
00602         return true;
00603     }
00604 
00605     if(d2 < min_dist)
00606     {
00607       if(distanceRecurse(root->children[1], query, cdata, callback, min_dist))
00608         return true;
00609     }
00610   }
00611 
00612   return false;
00613 }
00614 
00615 bool selfDistanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist)
00616 {
00617   if(root->isLeaf()) return false;
00618 
00619   if(selfDistanceRecurse(root->children[0], cdata, callback, min_dist))
00620     return true;
00621 
00622   if(selfDistanceRecurse(root->children[1], cdata, callback, min_dist))
00623     return true;
00624 
00625   if(distanceRecurse(root->children[0], root->children[1], cdata, callback, min_dist))
00626     return true;
00627 
00628   return false;
00629 }
00630 
00631 } // dynamic_AABB_tree
00632 
00633 } // details
00634 
00635 void DynamicAABBTreeCollisionManager::registerObjects(const std::vector<CollisionObject*>& other_objs)
00636 {
00637   if(size() > 0)
00638   {
00639     BroadPhaseCollisionManager::registerObjects(other_objs);
00640   }
00641   else
00642   {
00643     std::vector<DynamicAABBNode*> leaves(other_objs.size());
00644     table.rehash(other_objs.size());
00645     for(size_t i = 0, size = other_objs.size(); i < size; ++i)
00646     {
00647       DynamicAABBNode* node = new DynamicAABBNode; // node will be managed by the dtree
00648       node->bv = other_objs[i]->getAABB();
00649       node->parent = NULL;
00650       node->children[1] = NULL;
00651       node->data = other_objs[i];
00652       table[other_objs[i]] = node;
00653       leaves[i] = node;
00654     }
00655    
00656     dtree.init(leaves, tree_init_level);
00657    
00658     setup_ = true;
00659   }
00660 }
00661 
00662 void DynamicAABBTreeCollisionManager::registerObject(CollisionObject* obj)
00663 {
00664   DynamicAABBNode* node = dtree.insert(obj->getAABB(), obj);
00665   table[obj] = node;
00666 }
00667 
00668 void DynamicAABBTreeCollisionManager::unregisterObject(CollisionObject* obj)
00669 {
00670   DynamicAABBNode* node = table[obj];
00671   table.erase(obj);
00672   dtree.remove(node);
00673 }
00674 
00675 void DynamicAABBTreeCollisionManager::setup()
00676 {
00677   if(!setup_)
00678   {
00679     int num = dtree.size();
00680     if(num == 0) 
00681     {
00682       setup_ = true; 
00683       return;
00684     }
00685     
00686     int height = dtree.getMaxHeight();
00687 
00688     
00689     if(height - std::log((FCL_REAL)num) / std::log(2.0) < max_tree_nonbalanced_level)
00690       dtree.balanceIncremental(tree_incremental_balance_pass);
00691     else
00692       dtree.balanceTopdown();
00693 
00694     setup_ = true;
00695   }
00696 }
00697 
00698 
00699 void DynamicAABBTreeCollisionManager::update()
00700 { 
00701   for(DynamicAABBTable::const_iterator it = table.begin(); it != table.end(); ++it)
00702   {
00703     CollisionObject* obj = it->first;
00704     DynamicAABBNode* node = it->second;
00705     node->bv = obj->getAABB();
00706   }
00707 
00708   dtree.refit();
00709   setup_ = false;
00710 
00711   setup();
00712 }
00713 
00714 void DynamicAABBTreeCollisionManager::update_(CollisionObject* updated_obj)
00715 {
00716   DynamicAABBTable::const_iterator it = table.find(updated_obj);
00717   if(it != table.end())
00718   {
00719     DynamicAABBNode* node = it->second;
00720     if(!node->bv.equal(updated_obj->getAABB()))
00721       dtree.update(node, updated_obj->getAABB());
00722   }
00723   setup_ = false;
00724 }
00725 
00726 void DynamicAABBTreeCollisionManager::update(CollisionObject* updated_obj)
00727 {
00728   update_(updated_obj);
00729   setup();
00730 }
00731 
00732 void DynamicAABBTreeCollisionManager::update(const std::vector<CollisionObject*>& updated_objs)
00733 {
00734   for(size_t i = 0, size = updated_objs.size(); i < size; ++i)
00735     update_(updated_objs[i]);
00736   setup();
00737 }
00738 
00739 void DynamicAABBTreeCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
00740 {
00741   if(size() == 0) return;
00742   switch(obj->getCollisionGeometry()->getNodeType())
00743   {
00744 #if FCL_HAVE_OCTOMAP
00745   case GEOM_OCTREE:
00746     {
00747       if(!octree_as_geometry_collide)
00748       {
00749         const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
00750         details::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback); 
00751       }
00752       else
00753         details::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), obj, cdata, callback);
00754     }
00755     break;
00756 #endif
00757   default:
00758     details::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), obj, cdata, callback);
00759   }
00760 }
00761 
00762 void DynamicAABBTreeCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
00763 {
00764   if(size() == 0) return;
00765   FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
00766   switch(obj->getCollisionGeometry()->getNodeType())
00767   {
00768 #if FCL_HAVE_OCTOMAP
00769   case GEOM_OCTREE:
00770     {
00771       if(!octree_as_geometry_distance)
00772       {
00773         const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
00774         details::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback, min_dist);
00775       }
00776       else
00777         details::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), obj, cdata, callback, min_dist);          
00778     }
00779     break;
00780 #endif
00781   default:
00782     details::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), obj, cdata, callback, min_dist);
00783   }
00784 }
00785 
00786 
00787 void DynamicAABBTreeCollisionManager::collide(void* cdata, CollisionCallBack callback) const
00788 {
00789   if(size() == 0) return;
00790   details::dynamic_AABB_tree::selfCollisionRecurse(dtree.getRoot(), cdata, callback);
00791 }
00792 
00793 void DynamicAABBTreeCollisionManager::distance(void* cdata, DistanceCallBack callback) const
00794 {
00795   if(size() == 0) return;
00796   FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
00797   details::dynamic_AABB_tree::selfDistanceRecurse(dtree.getRoot(), cdata, callback, min_dist);
00798 }
00799 
00800 void DynamicAABBTreeCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
00801 {
00802   DynamicAABBTreeCollisionManager* other_manager = static_cast<DynamicAABBTreeCollisionManager*>(other_manager_);
00803   if((size() == 0) || (other_manager->size() == 0)) return;
00804   details::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), other_manager->dtree.getRoot(), cdata, callback);
00805 }
00806 
00807 void DynamicAABBTreeCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
00808 {
00809   DynamicAABBTreeCollisionManager* other_manager = static_cast<DynamicAABBTreeCollisionManager*>(other_manager_);
00810   if((size() == 0) || (other_manager->size() == 0)) return;
00811   FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
00812   details::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), other_manager->dtree.getRoot(), cdata, callback, min_dist);
00813 }
00814 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


fcl
Author(s): Jia Pan
autogenerated on Tue Jan 15 2013 16:05:30