All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends

broadphase_SaP.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_SaP.h"
00038 #include <algorithm>
00039 #include <limits>
00040 #include <boost/bind.hpp>
00041 
00042 namespace fcl
00043 {
00044 
00045 void SaPCollisionManager::unregisterObject(CollisionObject* obj)
00046 {
00047   std::list<SaPAABB*>::iterator it = AABB_arr.begin();
00048   for(std::list<SaPAABB*>::iterator end = AABB_arr.end(); it != end; ++it)
00049   {
00050     if((*it)->obj == obj)
00051       break;
00052   }
00053 
00054   AABB_arr.erase(it);
00055   obj_aabb_map.erase(obj);
00056 
00057   if(it == AABB_arr.end())
00058     return;
00059 
00060   SaPAABB* curr = *it;
00061   *it = NULL;
00062 
00063   for(int coord = 0; coord < 3; ++coord)
00064   {
00065     //first delete the lo endpoint of the interval.
00066     if(curr->lo->prev[coord] == NULL)
00067       elist[coord] = curr->lo->next[coord];
00068     else
00069       curr->lo->prev[coord]->next[coord] = curr->lo->next[coord];
00070 
00071     curr->lo->next[coord]->prev[coord] = curr->lo->prev[coord];
00072 
00073     //then, delete the "hi" endpoint.
00074     if(curr->hi->prev[coord] == NULL)
00075       elist[coord] = curr->hi->next[coord];
00076     else
00077       curr->hi->prev[coord]->next[coord] = curr->hi->next[coord];
00078 
00079     if(curr->hi->next[coord] != NULL)
00080       curr->hi->next[coord]->prev[coord] = curr->hi->prev[coord];
00081   }
00082 
00083   delete curr->lo;
00084   delete curr->hi;
00085   delete curr;
00086 
00087   overlap_pairs.remove_if(isUnregistered(obj));
00088 }
00089 
00090 void SaPCollisionManager::registerObjects(const std::vector<CollisionObject*>& other_objs)
00091 {
00092   if(size() > 0)
00093     BroadPhaseCollisionManager::registerObjects(other_objs);
00094   else
00095   {
00096     std::vector<EndPoint*> endpoints(2 * other_objs.size());
00097     
00098     for(size_t i = 0; i < other_objs.size(); ++i)
00099     {
00100       SaPAABB* sapaabb = new SaPAABB();
00101       sapaabb->obj = other_objs[i];
00102       sapaabb->lo = new EndPoint();
00103       sapaabb->hi = new EndPoint();
00104       sapaabb->cached = other_objs[i]->getAABB();
00105       endpoints[2 * i] = sapaabb->lo;
00106       endpoints[2 * i + 1] = sapaabb->hi;
00107       sapaabb->lo->minmax = 0;
00108       sapaabb->hi->minmax = 1;
00109       sapaabb->lo->aabb = sapaabb;
00110       sapaabb->hi->aabb = sapaabb;
00111       AABB_arr.push_back(sapaabb);
00112       obj_aabb_map[other_objs[i]] = sapaabb;
00113     }
00114 
00115 
00116     FCL_REAL scale[3];
00117     for(size_t coord = 0; coord < 3; ++coord)
00118     { 
00119       std::sort(endpoints.begin(), endpoints.end(), 
00120                 boost::bind(std::less<FCL_REAL>(),
00121                             boost::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const >(&EndPoint::getVal), _1, coord),
00122                             boost::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const >(&EndPoint::getVal), _2, coord)));
00123 
00124       endpoints[0]->prev[coord] = NULL;
00125       endpoints[0]->next[coord] = endpoints[1];
00126       for(int i = 1; i < endpoints.size() - 1; ++i)
00127       {
00128         endpoints[i]->prev[coord] = endpoints[i-1];
00129         endpoints[i]->next[coord] = endpoints[i+1];
00130       }
00131       endpoints[endpoints.size() - 1]->prev[coord] = endpoints[endpoints.size() - 2];
00132       endpoints[endpoints.size() - 1]->next[coord] = NULL;
00133 
00134       elist[coord] = endpoints[0];
00135 
00136       scale[coord] = endpoints.back()->aabb->cached.max_[coord] - endpoints[0]->aabb->cached.min_[coord];
00137     }
00138     
00139     int axis = 0;
00140     if(scale[axis] < scale[1]) axis = 1;
00141     if(scale[axis] < scale[2]) axis = 2;
00142 
00143     EndPoint* pos = elist[axis];
00144     
00145     while(pos != NULL)
00146     {
00147       EndPoint* pos_next = NULL;
00148       SaPAABB* aabb = pos->aabb;
00149       EndPoint* pos_it = pos->next[axis];
00150       
00151       while(pos_it != NULL)
00152       {
00153         if(pos_it->aabb == aabb)
00154         {
00155           if(pos_next == NULL) pos_next = pos_it; 
00156           break;
00157         }
00158       
00159         if(pos_it->minmax == 0)
00160         {
00161           if(pos_next == NULL) pos_next = pos_it;
00162           if(pos_it->aabb->cached.overlap(aabb->cached))
00163             overlap_pairs.push_back(SaPPair(pos_it->aabb->obj, aabb->obj));
00164         }
00165         pos_it = pos_it->next[axis];
00166       }
00167 
00168       pos = pos_next;
00169     }
00170   }
00171 
00172   updateVelist();
00173 }
00174 
00175 void SaPCollisionManager::registerObject(CollisionObject* obj)
00176 {
00177   SaPAABB* curr = new SaPAABB;
00178   curr->cached = obj->getAABB();
00179   curr->obj = obj;
00180   curr->lo = new EndPoint;
00181   curr->lo->minmax = 0;
00182   curr->lo->aabb = curr;
00183 
00184   curr->hi = new EndPoint;
00185   curr->hi->minmax = 1;
00186   curr->hi->aabb = curr;
00187 
00188   for(int coord = 0; coord < 3; ++coord)
00189   {
00190     EndPoint* current = elist[coord];
00191 
00192     // first insert the lo end point
00193     if(current == NULL) // empty list
00194     {
00195       elist[coord] = curr->lo;
00196       curr->lo->prev[coord] = curr->lo->next[coord] = NULL;
00197     }
00198     else // otherwise, find the correct location in the list and insert
00199     {
00200       EndPoint* curr_lo = curr->lo;
00201       FCL_REAL curr_lo_val = curr_lo->getVal()[coord];
00202       while((current->getVal()[coord] < curr_lo_val) && (current->next[coord] != NULL))
00203         current = current->next[coord];
00204 
00205       if(current->getVal()[coord] >= curr_lo_val)
00206       {
00207         curr_lo->prev[coord] = current->prev[coord];
00208         curr_lo->next[coord] = current;
00209         if(current->prev[coord] == NULL)
00210           elist[coord] = curr_lo;
00211         else
00212           current->prev[coord]->next[coord] = curr_lo;
00213 
00214         current->prev[coord] = curr_lo;
00215       }
00216       else
00217       {
00218         curr_lo->prev[coord] = current;
00219         curr_lo->next[coord] = NULL;
00220         current->next[coord] = curr_lo;
00221       }
00222     }
00223 
00224     // now insert hi end point
00225     current = curr->lo;
00226 
00227     EndPoint* curr_hi = curr->hi;
00228     FCL_REAL curr_hi_val = curr_hi->getVal()[coord];
00229     
00230     if(coord == 0)
00231     {
00232       while((current->getVal()[coord] < curr_hi_val) && (current->next[coord] != NULL))
00233       {
00234         if(current != curr->lo)
00235           if(current->aabb->cached.overlap(curr->cached))
00236             overlap_pairs.push_back(SaPPair(current->aabb->obj, obj));
00237 
00238         current = current->next[coord];
00239       }
00240     }
00241     else
00242     {
00243       while((current->getVal()[coord] < curr_hi_val) && (current->next[coord] != NULL))
00244         current = current->next[coord];
00245     }
00246 
00247     if(current->getVal()[coord] >= curr_hi_val)
00248     {
00249       curr_hi->prev[coord] = current->prev[coord];
00250       curr_hi->next[coord] = current;
00251       if(current->prev[coord] == NULL)
00252         elist[coord] = curr_hi;
00253       else
00254         current->prev[coord]->next[coord] = curr_hi;
00255 
00256       current->prev[coord] = curr_hi;
00257     }
00258     else
00259     {
00260       curr_hi->prev[coord] = current;
00261       curr_hi->next[coord] = NULL;
00262       current->next[coord] = curr_hi;
00263     }
00264   }
00265 
00266   AABB_arr.push_back(curr);
00267 
00268   obj_aabb_map[obj] = curr;
00269 
00270   updateVelist();
00271 }
00272 
00273 void SaPCollisionManager::setup()
00274 {
00275   FCL_REAL scale[3];
00276   scale[0] = (velist[0].back())->getVal(0) - velist[0][0]->getVal(0);
00277   scale[1] = (velist[1].back())->getVal(1) - velist[1][0]->getVal(1);;
00278   scale[2] = (velist[2].back())->getVal(2) - velist[2][0]->getVal(2);
00279   size_t axis = 0;
00280   if(scale[axis] < scale[1]) axis = 1;
00281   if(scale[axis] < scale[2]) axis = 2;
00282   optimal_axis = axis;
00283 }
00284 
00285 void SaPCollisionManager::update_(SaPAABB* updated_aabb)
00286 {
00287   if(updated_aabb->cached.equal(updated_aabb->obj->getAABB()))
00288     return;
00289 
00290   SaPAABB* current = updated_aabb;
00291 
00292   Vec3f new_min = current->obj->getAABB().min_;
00293   Vec3f new_max = current->obj->getAABB().max_;
00294 
00295   SaPAABB dummy;
00296   dummy.cached = current->obj->getAABB();
00297 
00298   for(int coord = 0; coord < 3; ++coord)
00299   {
00300     int direction; // -1 reverse, 0 nochange, 1 forward
00301     EndPoint* temp;
00302 
00303     if(current->lo->getVal(coord) > new_min[coord])
00304       direction = -1;
00305     else if(current->lo->getVal(coord) < new_min[coord])
00306       direction = 1;
00307     else direction = 0;
00308 
00309     if(direction == -1)
00310     {
00311       //first update the "lo" endpoint of the interval
00312       if(current->lo->prev[coord] != NULL)
00313       {
00314         temp = current->lo;
00315         while((temp != NULL) && (temp->getVal(coord) > new_min[coord]))
00316         {
00317           if(temp->minmax == 1)
00318             if(temp->aabb->cached.overlap(dummy.cached))
00319               addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
00320           temp = temp->prev[coord];
00321         }
00322 
00323         if(temp == NULL)
00324         {
00325           current->lo->prev[coord]->next[coord] = current->lo->next[coord];
00326           current->lo->next[coord]->prev[coord] = current->lo->prev[coord];
00327           current->lo->prev[coord] = NULL;
00328           current->lo->next[coord] = elist[coord];
00329           elist[coord]->prev[coord] = current->lo;
00330           elist[coord] = current->lo;
00331         }
00332         else
00333         {
00334           current->lo->prev[coord]->next[coord] = current->lo->next[coord];
00335           current->lo->next[coord]->prev[coord] = current->lo->prev[coord];
00336           current->lo->prev[coord] = temp;
00337           current->lo->next[coord] = temp->next[coord];
00338           temp->next[coord]->prev[coord] = current->lo;
00339           temp->next[coord] = current->lo;
00340         }
00341       }
00342 
00343       current->lo->getVal(coord) = new_min[coord];
00344 
00345       // update hi end point
00346       temp = current->hi;
00347       while(temp->getVal(coord) > new_max[coord])
00348       {
00349         if((temp->minmax == 0) && (temp->aabb->cached.overlap(current->cached)))
00350           removeFromOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
00351         temp = temp->prev[coord];
00352       }
00353 
00354       current->hi->prev[coord]->next[coord] = current->hi->next[coord];
00355       if(current->hi->next[coord] != NULL)
00356         current->hi->next[coord]->prev[coord] = current->hi->prev[coord];
00357       current->hi->prev[coord] = temp;
00358       current->hi->next[coord] = temp->next[coord];
00359       if(temp->next[coord] != NULL)
00360         temp->next[coord]->prev[coord] = current->hi;
00361       temp->next[coord] = current->hi;
00362 
00363       current->hi->getVal(coord) = new_max[coord];
00364     }
00365     else if(direction == 1)
00366     {
00367       //here, we first update the "hi" endpoint.
00368       if(current->hi->next[coord] != NULL)
00369       {
00370         temp = current->hi;
00371         while((temp->next[coord] != NULL) && (temp->getVal(coord) < new_max[coord]))
00372         {
00373           if(temp->minmax == 0)
00374             if(temp->aabb->cached.overlap(dummy.cached))
00375               addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
00376           temp = temp->next[coord];
00377         }
00378 
00379         if(temp->getVal(coord) < new_max[coord])
00380         {
00381           current->hi->prev[coord]->next[coord] = current->hi->next[coord];
00382           current->hi->next[coord]->prev[coord] = current->hi->prev[coord];
00383           current->hi->prev[coord] = temp;
00384           current->hi->next[coord] = NULL;
00385           temp->next[coord] = current->hi;
00386         }
00387         else
00388         {
00389           current->hi->prev[coord]->next[coord] = current->hi->next[coord];
00390           current->hi->next[coord]->prev[coord] = current->hi->prev[coord];
00391           current->hi->prev[coord] = temp->prev[coord];
00392           current->hi->next[coord] = temp;
00393           temp->prev[coord]->next[coord] = current->hi;
00394           temp->prev[coord] = current->hi;
00395         }
00396       }
00397 
00398       current->hi->getVal(coord) = new_max[coord];
00399 
00400       //then, update the "lo" endpoint of the interval.
00401       temp = current->lo;
00402 
00403       while(temp->getVal(coord) < new_min[coord])
00404       {
00405         if((temp->minmax == 1) && (temp->aabb->cached.overlap(current->cached)))
00406           removeFromOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
00407         temp = temp->next[coord];
00408       }
00409 
00410       if(current->lo->prev[coord] != NULL)
00411         current->lo->prev[coord]->next[coord] = current->lo->next[coord];
00412       else
00413         elist[coord] = current->lo->next[coord];
00414       current->lo->next[coord]->prev[coord] = current->lo->prev[coord];
00415       current->lo->prev[coord] = temp->prev[coord];
00416       current->lo->next[coord] = temp;
00417       if(temp->prev[coord] != NULL)
00418         temp->prev[coord]->next[coord] = current->lo;
00419       else
00420         elist[coord] = current->lo;
00421       temp->prev[coord] = current->lo;
00422       current->lo->getVal(coord) = new_min[coord];
00423     }
00424   }
00425 }
00426 
00427 void SaPCollisionManager::update(CollisionObject* updated_obj)
00428 {
00429   update_(obj_aabb_map[updated_obj]);
00430 
00431   updateVelist();
00432 
00433   setup();
00434 }
00435 
00436 void SaPCollisionManager::update(const std::vector<CollisionObject*>& updated_objs)
00437 {
00438   for(size_t i = 0; i < updated_objs.size(); ++i)
00439     update_(obj_aabb_map[updated_objs[i]]);
00440 
00441   updateVelist();
00442 
00443   setup();
00444 }
00445 
00446 void SaPCollisionManager::update()
00447 {
00448   for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(), end = AABB_arr.end(); it != end; ++it)
00449   {
00450     update_(*it);
00451   }
00452 
00453   updateVelist();
00454 
00455   setup();
00456 }
00457 
00458 
00459 void SaPCollisionManager::clear()
00460 {
00461   for(std::list<SaPAABB*>::iterator it = AABB_arr.begin(), end = AABB_arr.end();
00462       it != end;
00463       ++it)
00464   {
00465     delete (*it)->hi;
00466     delete (*it)->lo;
00467     delete *it;
00468     *it = NULL;
00469   }
00470 
00471   AABB_arr.clear();
00472   overlap_pairs.clear();
00473 
00474   elist[0] = NULL;
00475   elist[1] = NULL;
00476   elist[2] = NULL;
00477 
00478   velist[0].clear();
00479   velist[1].clear();
00480   velist[2].clear();
00481 
00482   obj_aabb_map.clear();
00483 }
00484 
00485 void SaPCollisionManager::getObjects(std::vector<CollisionObject*>& objs) const
00486 {
00487   objs.resize(AABB_arr.size());
00488   int i = 0;
00489   for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(), end = AABB_arr.end(); it != end; ++it, ++i)
00490   {
00491     objs[i] = (*it)->obj;
00492   }
00493 }
00494 
00495 bool SaPCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
00496 {
00497   size_t axis = optimal_axis;
00498   const AABB& obj_aabb = obj->getAABB();
00499 
00500   FCL_REAL min_val = obj_aabb.min_[axis];
00501   FCL_REAL max_val = obj_aabb.max_[axis];
00502 
00503   EndPoint dummy;
00504   SaPAABB dummy_aabb;
00505   dummy_aabb.cached = obj_aabb;
00506   dummy.minmax = 1;
00507   dummy.aabb = &dummy_aabb;
00508   
00509   // compute stop_pos by binary search, this is cheaper than check it in while iteration linearly
00510   std::vector<EndPoint*>::const_iterator res_it = std::upper_bound(velist[axis].begin(), velist[axis].end(), &dummy,
00511                                                                    boost::bind(std::less<FCL_REAL>(),
00512                                                                                boost::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(&EndPoint::getVal), _1, axis),
00513                                                                                boost::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(&EndPoint::getVal), _2, axis)));
00514   
00515   EndPoint* end_pos = NULL;
00516   if(res_it != velist[axis].end())
00517     end_pos = *res_it;
00518   
00519   EndPoint* pos = elist[axis];
00520 
00521   while(pos != end_pos)
00522   {
00523     if(pos->aabb->obj != obj)
00524     {
00525       if((pos->minmax == 0) && (pos->aabb->hi->getVal(axis) >= min_val))
00526       {
00527         if(pos->aabb->cached.overlap(obj->getAABB()))
00528           if(callback(obj, pos->aabb->obj, cdata))
00529             return true;
00530       }
00531     }
00532     pos = pos->next[axis];
00533   } 
00534 
00535   return false;
00536 }
00537 
00538 void SaPCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
00539 {
00540   if(size() == 0) return;
00541   
00542   collide_(obj, cdata, callback);
00543 }
00544 
00545 bool SaPCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
00546 {
00547   Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
00548   AABB aabb = obj->getAABB();
00549 
00550   if(min_dist < std::numeric_limits<FCL_REAL>::max())
00551   {
00552     Vec3f min_dist_delta(min_dist, min_dist, min_dist);
00553     aabb.expand(min_dist_delta);
00554   }
00555 
00556   size_t axis = optimal_axis;
00557 
00558   int status = 1;
00559   FCL_REAL old_min_distance;
00560 
00561   EndPoint* start_pos = elist[axis];
00562 
00563   while(1)
00564   {
00565     old_min_distance = min_dist;
00566     FCL_REAL min_val = aabb.min_[axis];
00567     FCL_REAL max_val = aabb.max_[axis];
00568 
00569     EndPoint dummy; 
00570     SaPAABB dummy_aabb;
00571     dummy_aabb.cached = aabb;
00572     dummy.minmax = 1;
00573     dummy.aabb = &dummy_aabb;
00574     
00575  
00576     std::vector<EndPoint*>::const_iterator res_it = std::upper_bound(velist[axis].begin(), velist[axis].end(), &dummy,
00577                                                                      boost::bind(std::less<FCL_REAL>(),
00578                                                                                  boost::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(&EndPoint::getVal), _1, axis),
00579                                                                                  boost::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(&EndPoint::getVal), _2, axis)));
00580 
00581     EndPoint* end_pos = NULL;
00582     if(res_it != velist[axis].end())
00583       end_pos = *res_it;
00584 
00585     EndPoint* pos = start_pos;
00586 
00587     while(pos != end_pos)
00588     {
00589       // can change to pos->aabb->hi->getVal(axis) >= min_val - min_dist, and then update start_pos to end_pos.
00590       // but this seems slower.
00591       if((pos->minmax == 0) && (pos->aabb->hi->getVal(axis) >= min_val)) 
00592       {
00593         CollisionObject* curr_obj = pos->aabb->obj;
00594         if(curr_obj != obj)
00595         {
00596           if(!enable_tested_set_)
00597           {
00598             if(pos->aabb->cached.distance(obj->getAABB()) < min_dist)
00599             {
00600               if(callback(curr_obj, obj, cdata, min_dist))
00601                 return true;
00602             }
00603           }
00604           else
00605           {
00606             if(!inTestedSet(curr_obj, obj))
00607             {
00608               if(pos->aabb->cached.distance(obj->getAABB()) < min_dist)
00609               {
00610                 if(callback(curr_obj, obj, cdata, min_dist))
00611                   return true;
00612               }
00613               
00614               insertTestedSet(curr_obj, obj);
00615             }
00616           }
00617         }
00618       }
00619 
00620       pos = pos->next[axis];
00621     }
00622 
00623     if(status == 1)
00624     {
00625       if(old_min_distance < std::numeric_limits<FCL_REAL>::max())
00626         break;
00627       else
00628       {
00629         if(min_dist < old_min_distance)
00630         {
00631           Vec3f min_dist_delta(min_dist, min_dist, min_dist);
00632           aabb = AABB(obj->getAABB(), min_dist_delta);
00633           status = 0;
00634         }
00635         else
00636         {
00637           if(aabb.equal(obj->getAABB()))
00638             aabb.expand(delta);
00639           else
00640             aabb.expand(obj->getAABB(), 2.0);
00641         }
00642       }
00643     }
00644     else if(status == 0)
00645       break;
00646   }
00647 
00648   return false;
00649 }
00650 
00651 void SaPCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
00652 {
00653   if(size() == 0) return;
00654 
00655   FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
00656   
00657   distance_(obj, cdata, callback, min_dist);
00658 }
00659 
00660 void SaPCollisionManager::collide(void* cdata, CollisionCallBack callback) const
00661 {
00662   if(size() == 0) return;
00663 
00664   for(std::list<SaPPair>::const_iterator it = overlap_pairs.begin(), end = overlap_pairs.end(); it != end; ++it)
00665   {
00666     CollisionObject* obj1 = it->obj1;
00667     CollisionObject* obj2 = it->obj2;
00668 
00669     if(callback(obj1, obj2, cdata))
00670       return;
00671   }
00672 }
00673 
00674 void SaPCollisionManager::distance(void* cdata, DistanceCallBack callback) const
00675 {
00676   if(size() == 0) return;
00677 
00678   enable_tested_set_ = true;
00679   tested_set.clear();
00680   
00681   FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
00682 
00683   for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(), end = AABB_arr.end(); it != end; ++it)
00684   {
00685     if(distance_((*it)->obj, cdata, callback, min_dist))
00686       break;
00687   }
00688 
00689   enable_tested_set_ = false;
00690   tested_set.clear();
00691 }
00692 
00693 void SaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
00694 {
00695   SaPCollisionManager* other_manager = static_cast<SaPCollisionManager*>(other_manager_);
00696 
00697   if((size() == 0) || (other_manager->size() == 0)) return;
00698 
00699   if(this == other_manager)
00700   {
00701     collide(cdata, callback);
00702     return;
00703   }
00704 
00705   if(this->size() < other_manager->size())
00706   {
00707     for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(); it != AABB_arr.end(); ++it)
00708     {
00709       if(other_manager->collide_((*it)->obj, cdata, callback))
00710         return;
00711     }
00712   }
00713   else
00714   {
00715     for(std::list<SaPAABB*>::const_iterator it = other_manager->AABB_arr.begin(), end = other_manager->AABB_arr.end(); it != end; ++it)
00716     {
00717       if(collide_((*it)->obj, cdata, callback))
00718         return;
00719     }
00720   }
00721 }
00722 
00723 void SaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
00724 {
00725   SaPCollisionManager* other_manager = static_cast<SaPCollisionManager*>(other_manager_);
00726 
00727   if((size() == 0) || (other_manager->size() == 0)) return;
00728 
00729   if(this == other_manager)
00730   {
00731     distance(cdata, callback);
00732     return;
00733   }
00734 
00735   FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
00736 
00737   if(this->size() < other_manager->size())
00738   {
00739     for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(), end = AABB_arr.end(); it != end; ++it)
00740     {
00741       if(other_manager->distance_((*it)->obj, cdata, callback, min_dist))
00742         return;
00743     }
00744   }
00745   else
00746   {
00747     for(std::list<SaPAABB*>::const_iterator it = other_manager->AABB_arr.begin(), end = other_manager->AABB_arr.end(); it != end; ++it)
00748     {
00749       if(distance_((*it)->obj, cdata, callback, min_dist))
00750         return;
00751     }
00752   }
00753 }
00754 
00755 bool SaPCollisionManager::empty() const
00756 {
00757   return AABB_arr.size();
00758 }
00759 
00760 
00761 
00762 }