All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends

broadphase_SSaP.cpp

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 
00037 #include "fcl/broadphase/broadphase_SSaP.h"
00038 #include <algorithm>
00039 #include <limits>
00040 
00041 namespace fcl
00042 {
00043 
00045 struct SortByXLow
00046 {
00047   bool operator()(const CollisionObject* a, const CollisionObject* b) const
00048   {
00049     if(a->getAABB().min_[0] < b->getAABB().min_[0])
00050       return true;
00051     return false;
00052   }
00053 };
00054 
00056 struct SortByYLow
00057 {
00058   bool operator()(const CollisionObject* a, const CollisionObject* b) const
00059   {
00060     if(a->getAABB().min_[1] < b->getAABB().min_[1])
00061       return true;
00062     return false;
00063   }
00064 };
00065 
00067 struct SortByZLow
00068 {
00069   bool operator()(const CollisionObject* a, const CollisionObject* b) const
00070   {
00071     if(a->getAABB().min_[2] < b->getAABB().min_[2])
00072       return true;
00073     return false;
00074   }
00075 };
00076 
00078 class DummyCollisionObject : public CollisionObject
00079 {
00080 public:
00081   DummyCollisionObject(const AABB& aabb_) : CollisionObject()
00082   {
00083     aabb = aabb_;
00084   }
00085 
00086   void computeLocalAABB() {}
00087 };
00088 
00089 
00090 void SSaPCollisionManager::unregisterObject(CollisionObject* obj)
00091 {
00092   setup();
00093 
00094   DummyCollisionObject dummyHigh(AABB(obj->getAABB().max_));
00095 
00096   std::vector<CollisionObject*>::iterator pos_start1 = objs_x.begin();
00097   std::vector<CollisionObject*>::iterator pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow());
00098 
00099   while(pos_start1 < pos_end1)
00100   {
00101     if(*pos_start1 == obj)
00102     {
00103       objs_x.erase(pos_start1);
00104       break;
00105     }
00106     ++pos_start1;
00107   }
00108 
00109   std::vector<CollisionObject*>::iterator pos_start2 = objs_y.begin();
00110   std::vector<CollisionObject*>::iterator pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow());
00111 
00112   while(pos_start2 < pos_end2)
00113   {
00114     if(*pos_start2 == obj)
00115     {
00116       objs_y.erase(pos_start2);
00117       break;
00118     }
00119     ++pos_start2;
00120   }
00121 
00122   std::vector<CollisionObject*>::iterator pos_start3 = objs_z.begin();
00123   std::vector<CollisionObject*>::iterator pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow());
00124 
00125   while(pos_start3 < pos_end3)
00126   {
00127     if(*pos_start3 == obj)
00128     {
00129       objs_z.erase(pos_start3);
00130       break;
00131     }
00132     ++pos_start3;
00133   }
00134 }
00135 
00136 
00137 void SSaPCollisionManager::registerObject(CollisionObject* obj)
00138 {
00139   objs_x.push_back(obj);
00140   objs_y.push_back(obj);
00141   objs_z.push_back(obj);
00142   setup_ = false;
00143 }
00144 
00145 void SSaPCollisionManager::setup()
00146 {
00147   if(!setup_)
00148   {
00149     std::sort(objs_x.begin(), objs_x.end(), SortByXLow());
00150     std::sort(objs_y.begin(), objs_y.end(), SortByYLow());
00151     std::sort(objs_z.begin(), objs_z.end(), SortByZLow());
00152     setup_ = true;
00153   }
00154 }
00155 
00156 void SSaPCollisionManager::update()
00157 {
00158   setup_ = false;
00159   setup();
00160 }
00161 
00162 void SSaPCollisionManager::clear()
00163 {
00164   objs_x.clear();
00165   objs_y.clear();
00166   objs_z.clear();
00167   setup_ = false;
00168 }
00169 
00170 void SSaPCollisionManager::getObjects(std::vector<CollisionObject*>& objs) const
00171 {
00172   objs.resize(objs_x.size());
00173   std::copy(objs_x.begin(), objs_x.end(), objs.begin());
00174 }
00175 
00176 bool SSaPCollisionManager::checkColl(std::vector<CollisionObject*>::const_iterator pos_start, std::vector<CollisionObject*>::const_iterator pos_end,
00177                                      CollisionObject* obj, void* cdata, CollisionCallBack callback) const
00178 {
00179   while(pos_start < pos_end)
00180   {
00181     if(*pos_start != obj) // no collision between the same object
00182     {
00183       if((*pos_start)->getAABB().overlap(obj->getAABB()))
00184       {
00185         if(callback(*pos_start, obj, cdata))
00186           return true;
00187       }
00188     }
00189     pos_start++;
00190   }
00191   return false;
00192 }
00193 
00194 bool SSaPCollisionManager::checkDis(std::vector<CollisionObject*>::const_iterator pos_start, std::vector<CollisionObject*>::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
00195 {
00196   while(pos_start < pos_end)
00197   {
00198     if(*pos_start != obj) // no distance between the same object
00199     {
00200       if((*pos_start)->getAABB().distance(obj->getAABB()) < min_dist)
00201       {
00202         if(callback(*pos_start, obj, cdata, min_dist)) 
00203           return true;
00204       }
00205     }
00206     pos_start++;
00207   }
00208   
00209   return false;
00210 }
00211 
00212 
00213 
00214 void SSaPCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
00215 {
00216   if(size() == 0) return;
00217 
00218   collide_(obj, cdata, callback);
00219 }
00220 
00221 bool SSaPCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
00222 {
00223   static const unsigned int CUTOFF = 100;
00224 
00225   DummyCollisionObject dummyHigh(AABB(obj->getAABB().max_));
00226   bool coll_res = false;
00227 
00228   std::vector<CollisionObject*>::const_iterator pos_start1 = objs_x.begin();
00229   std::vector<CollisionObject*>::const_iterator pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow());
00230   unsigned int d1 = pos_end1 - pos_start1;
00231 
00232   if(d1 > CUTOFF)
00233   {
00234     std::vector<CollisionObject*>::const_iterator pos_start2 = objs_y.begin();
00235     std::vector<CollisionObject*>::const_iterator pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow());
00236     unsigned int d2 = pos_end2 - pos_start2;
00237 
00238     if(d2 > CUTOFF)
00239     {
00240       std::vector<CollisionObject*>::const_iterator pos_start3 = objs_z.begin();
00241       std::vector<CollisionObject*>::const_iterator pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow());
00242       unsigned int d3 = pos_end3 - pos_start3;
00243 
00244       if(d3 > CUTOFF)
00245       {
00246         if(d3 <= d2 && d3 <= d1)
00247           coll_res = checkColl(pos_start3, pos_end3, obj, cdata, callback);
00248         else
00249         {
00250           if(d2 <= d3 && d2 <= d1)
00251             coll_res = checkColl(pos_start2, pos_end2, obj, cdata, callback);
00252           else
00253             coll_res = checkColl(pos_start1, pos_end1, obj, cdata, callback);
00254         }
00255       }
00256       else
00257         coll_res = checkColl(pos_start3, pos_end3, obj, cdata, callback);
00258     }
00259     else
00260       coll_res = checkColl(pos_start2, pos_end2, obj, cdata, callback);
00261   }
00262   else
00263     coll_res = checkColl(pos_start1, pos_end1, obj, cdata, callback);
00264 
00265   return coll_res;
00266 }
00267 
00268 
00269 void SSaPCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
00270 {
00271   if(size() == 0) return;
00272 
00273   FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
00274   distance_(obj, cdata, callback, min_dist); 
00275 }
00276 
00277 bool SSaPCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
00278 {
00279   static const unsigned int CUTOFF = 100;
00280   Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
00281   Vec3f dummy_vector = obj->getAABB().max_;
00282   if(min_dist < std::numeric_limits<FCL_REAL>::max())
00283     dummy_vector += Vec3f(min_dist, min_dist, min_dist);
00284 
00285   std::vector<CollisionObject*>::const_iterator pos_start1 = objs_x.begin();
00286   std::vector<CollisionObject*>::const_iterator pos_start2 = objs_y.begin();
00287   std::vector<CollisionObject*>::const_iterator pos_start3 = objs_z.begin();
00288   std::vector<CollisionObject*>::const_iterator pos_end1 = objs_x.end();
00289   std::vector<CollisionObject*>::const_iterator pos_end2 = objs_y.end();
00290   std::vector<CollisionObject*>::const_iterator pos_end3 = objs_z.end();
00291 
00292   int status = 1;
00293   FCL_REAL old_min_distance;
00294 
00295   while(1)
00296   {
00297     old_min_distance = min_dist;
00298     DummyCollisionObject dummyHigh((AABB(dummy_vector)));
00299 
00300     pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow());
00301     unsigned int d1 = pos_end1 - pos_start1;
00302 
00303     bool dist_res = false;
00304     
00305     if(d1 > CUTOFF)
00306     {
00307       pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow());
00308       unsigned int d2 = pos_end2 - pos_start2;
00309 
00310       if(d2 > CUTOFF)
00311       {
00312         pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow());
00313         unsigned int d3 = pos_end3 - pos_start3;
00314 
00315         if(d3 > CUTOFF)
00316         {
00317           if(d3 <= d2 && d3 <= d1)
00318             dist_res = checkDis(pos_start3, pos_end3, obj, cdata, callback, min_dist);
00319           else
00320           {
00321             if(d2 <= d3 && d2 <= d1)
00322               dist_res = checkDis(pos_start2, pos_end2, obj, cdata, callback, min_dist);
00323             else
00324               dist_res = checkDis(pos_start1, pos_end1, obj, cdata, callback, min_dist);
00325           }
00326         }
00327         else
00328           dist_res = checkDis(pos_start3, pos_end3, obj, cdata, callback, min_dist);
00329       }
00330       else
00331         dist_res = checkDis(pos_start2, pos_end2, obj, cdata, callback, min_dist);
00332     }
00333     else
00334     {
00335       dist_res = checkDis(pos_start1, pos_end1, obj, cdata, callback, min_dist);
00336     }
00337 
00338     if(dist_res) return true;
00339 
00340     if(status == 1)
00341     {
00342       if(old_min_distance < std::numeric_limits<FCL_REAL>::max())
00343         break;
00344       else
00345       {
00346         // from infinity to a finite one, only need one additional loop 
00347         // to check the possible missed ones to the right of the objs array
00348         if(min_dist < old_min_distance) 
00349         {
00350           dummy_vector = obj->getAABB().max_ + Vec3f(min_dist, min_dist, min_dist);
00351           status = 0;
00352         }
00353         else // need more loop
00354         {
00355           if(dummy_vector.equal(obj->getAABB().max_))
00356             dummy_vector = dummy_vector + delta;
00357           else
00358             dummy_vector = dummy_vector * 2 - obj->getAABB().max_;
00359         }
00360       }
00361       
00362       // yes, following is wrong, will result in too large distance.
00363       // if(pos_end1 != objs_x.end()) pos_start1 = pos_end1;
00364       // if(pos_end2 != objs_y.end()) pos_start2 = pos_end2;
00365       // if(pos_end3 != objs_z.end()) pos_start3 = pos_end3;
00366     }
00367     else if(status == 0)
00368       break;
00369   }
00370 
00371   return false;
00372 }
00373 
00374 void SSaPCollisionManager::collide(void* cdata, CollisionCallBack callback) const
00375 {
00376   if(size() == 0) return;
00377 
00378   std::vector<CollisionObject*>::const_iterator pos, run_pos, pos_end;
00379   size_t axis = selectOptimalAxis(objs_x, objs_y, objs_z, 
00380                                   pos, pos_end);
00381   size_t axis2 = (axis + 1 > 2) ? 0 : (axis + 1);
00382   size_t axis3 = (axis2 + 1 > 2) ? 0 : (axis2 + 1);
00383 
00384   run_pos = pos;
00385 
00386   while((run_pos < pos_end) && (pos < pos_end))
00387   {
00388     CollisionObject* obj = *(pos++);
00389 
00390     while(1)
00391     {
00392       if((*run_pos)->getAABB().min_[axis] < obj->getAABB().min_[axis])
00393       {
00394         run_pos++;
00395         if(run_pos == pos_end) break;
00396         continue;
00397       }
00398       else
00399       {
00400         run_pos++;
00401         break;
00402       }
00403     }
00404 
00405     if(run_pos < pos_end)
00406     {
00407       std::vector<CollisionObject*>::const_iterator run_pos2 = run_pos;
00408 
00409       while((*run_pos2)->getAABB().min_[axis] <= obj->getAABB().max_[axis])
00410       {
00411         CollisionObject* obj2 = *run_pos2;
00412         run_pos2++;
00413 
00414         if((obj->getAABB().max_[axis2] >= obj2->getAABB().min_[axis2]) && (obj2->getAABB().max_[axis2] >= obj->getAABB().min_[axis2]))
00415         {
00416           if((obj->getAABB().max_[axis3] >= obj2->getAABB().min_[axis3]) && (obj2->getAABB().max_[axis3] >= obj->getAABB().min_[axis3]))
00417           {
00418             if(callback(obj, obj2, cdata))
00419               return;
00420           }
00421         }
00422 
00423         if(run_pos2 == pos_end) break;
00424       }
00425     }
00426   }
00427 }
00428 
00429 
00430 void SSaPCollisionManager::distance(void* cdata, DistanceCallBack callback) const
00431 {
00432   if(size() == 0) return;
00433 
00434   std::vector<CollisionObject*>::const_iterator it, it_end;
00435   size_t axis = selectOptimalAxis(objs_x, objs_y, objs_z, it, it_end);
00436 
00437   FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
00438   for(; it != it_end; ++it)
00439   {
00440     if(distance_(*it, cdata, callback, min_dist))
00441       return;
00442   }
00443 }
00444 
00445 void SSaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
00446 {
00447   SSaPCollisionManager* other_manager = static_cast<SSaPCollisionManager*>(other_manager_);
00448   
00449   if((size() == 0) || (other_manager->size() == 0)) return;
00450 
00451   if(this == other_manager)
00452   {
00453     collide(cdata, callback);
00454     return;
00455   }
00456 
00457 
00458   std::vector<CollisionObject*>::const_iterator it, end;
00459   if(this->size() < other_manager->size())
00460   {
00461     for(it = objs_x.begin(), end = objs_x.end(); it != end; ++it)
00462       if(other_manager->collide_(*it, cdata, callback)) return;
00463   }
00464   else
00465   {
00466     for(it = other_manager->objs_x.begin(), end != other_manager->objs_x.end(); it != end; ++it)
00467       if(collide_(*it, cdata, callback)) return;
00468   }  
00469 }
00470 
00471 void SSaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
00472 {
00473   SSaPCollisionManager* other_manager = static_cast<SSaPCollisionManager*>(other_manager_);
00474 
00475   if((size() == 0) || (other_manager->size() == 0)) return;
00476 
00477   if(this == other_manager)
00478   {
00479     distance(cdata, callback);
00480     return;
00481   }
00482   
00483   FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
00484   std::vector<CollisionObject*>::const_iterator it, end;
00485   if(this->size() < other_manager->size())
00486   {
00487     for(it = objs_x.begin(), end = objs_x.end(); it != end; ++it)
00488       if(other_manager->distance_(*it, cdata, callback, min_dist)) return;
00489   }
00490   else
00491   {
00492     for(it = other_manager->objs_x.begin(), end = other_manager->objs_x.end(); it != end; ++it)
00493       if(distance_(*it, cdata, callback, min_dist)) return;
00494   }
00495 }
00496 
00497 bool SSaPCollisionManager::empty() const
00498 {
00499   return objs_x.empty();
00500 }
00501 
00502 
00503 
00504 }