broadphase_SaP.h
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 
00037 #ifndef FCL_BROAD_PHASE_SAP_H
00038 #define FCL_BROAD_PHASE_SAP_H
00039 
00040 #include "fcl/broadphase/broadphase.h"
00041 
00042 #include <map>
00043 #include <list>
00044 
00045 namespace fcl
00046 {
00047 
00049 class SaPCollisionManager : public BroadPhaseCollisionManager
00050 {
00051 public:
00052 
00053   SaPCollisionManager()
00054   {
00055     elist[0] = NULL;
00056     elist[1] = NULL;
00057     elist[2] = NULL;
00058 
00059     optimal_axis = 0;
00060   }
00061 
00062   ~SaPCollisionManager()
00063   {
00064     clear();
00065   }
00066 
00068   void registerObjects(const std::vector<CollisionObject*>& other_objs);
00069 
00071   void registerObject(CollisionObject* obj);
00072 
00074   void unregisterObject(CollisionObject* obj);
00075 
00077   void setup();
00078 
00080   void update();
00081 
00083   void update(CollisionObject* updated_obj);
00084 
00086   void update(const std::vector<CollisionObject*>& updated_objs);
00087 
00089   void clear();
00090 
00092   void getObjects(std::vector<CollisionObject*>& objs) const;
00093 
00095   void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
00096 
00098   void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const;
00099 
00101   void collide(void* cdata, CollisionCallBack callback) const;
00102 
00104   void distance(void* cdata, DistanceCallBack callback) const;
00105 
00107   void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const;
00108 
00110   void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const;
00111 
00113   bool empty() const;
00114   
00116   inline size_t size() const { return AABB_arr.size(); }
00117 
00118 protected:
00119 
00120   struct EndPoint;
00121 
00123   struct SaPAABB
00124   {
00126     CollisionObject* obj;
00127 
00129     EndPoint* lo;
00130 
00132     EndPoint* hi;
00133 
00135     AABB cached;
00136   };
00137 
00139   struct EndPoint
00140   {
00142     char minmax;
00143 
00145     SaPAABB* aabb;
00146 
00148     EndPoint* prev[3];
00150     EndPoint* next[3];
00151 
00153     inline const Vec3f& getVal() const
00154     {
00155       if(minmax) return aabb->cached.max_;
00156       else return aabb->cached.min_;
00157     }
00158 
00160     inline Vec3f& getVal()
00161     {
00162       if(minmax) return aabb->cached.max_;
00163       else return aabb->cached.min_;
00164     }
00165 
00166     inline Vec3f::U getVal(size_t i) const
00167     {
00168       if(minmax) return aabb->cached.max_[i];
00169       else return aabb->cached.min_[i];
00170     }
00171 
00172     inline Vec3f::U& getVal(size_t i)
00173     {
00174       if(minmax) return aabb->cached.max_[i];
00175       else return aabb->cached.min_[i];
00176     }
00177 
00178   };
00179 
00181   struct SaPPair
00182   {
00183     SaPPair(CollisionObject* a, CollisionObject* b)
00184     {
00185       if(a < b)
00186       {
00187         obj1 = a;
00188         obj2 = b;
00189       }
00190       else
00191       {
00192         obj1 = b;
00193         obj2 = a;
00194       }
00195     }
00196 
00197     CollisionObject* obj1;
00198     CollisionObject* obj2;
00199 
00200     bool operator == (const SaPPair& other) const
00201     {
00202       return ((obj1 == other.obj1) && (obj2 == other.obj2));
00203     }
00204   };
00205 
00207   class isUnregistered
00208   {
00209     CollisionObject* obj;
00210 
00211   public:
00212     isUnregistered(CollisionObject* obj_) : obj(obj_)
00213     {}
00214 
00215     bool operator() (const SaPPair& pair) const
00216     {
00217       return (pair.obj1 == obj) || (pair.obj2 == obj);
00218     }
00219   };
00220 
00222   class isNotValidPair
00223   {
00224     CollisionObject* obj1;
00225     CollisionObject* obj2;
00226 
00227   public:
00228     isNotValidPair(CollisionObject* obj1_, CollisionObject* obj2_) : obj1(obj1_),
00229                                                                      obj2(obj2_)
00230     {}
00231 
00232     bool operator() (const SaPPair& pair)
00233     {
00234       return (pair.obj1 == obj1) && (pair.obj2 == obj2);
00235     }
00236   };
00237 
00238   void update_(SaPAABB* updated_aabb);
00239 
00240   void updateVelist() 
00241   {
00242     for(int coord = 0; coord < 3; ++coord)
00243     {
00244       velist[coord].resize(size() * 2);
00245       EndPoint* current = elist[coord];
00246       size_t id = 0;
00247       while(current)
00248       {
00249         velist[coord][id] = current;
00250         current = current->next[coord];
00251         id++;
00252       }
00253     }    
00254   }
00255 
00257   EndPoint* elist[3];
00258   
00260   std::vector<EndPoint*> velist[3];
00261 
00263   std::list<SaPAABB*> AABB_arr;
00264 
00266   std::list<SaPPair> overlap_pairs;
00267 
00268   size_t optimal_axis;
00269 
00270   std::map<CollisionObject*, SaPAABB*> obj_aabb_map;
00271 
00272   bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
00273 
00274   bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
00275 
00276   void addToOverlapPairs(const SaPPair& p)
00277   {
00278     bool repeated = false;
00279     for(std::list<SaPPair>::iterator it = overlap_pairs.begin(), end = overlap_pairs.end();
00280         it != end;
00281         ++it)
00282     {
00283       if(*it == p)
00284       {
00285         repeated = true;
00286         break;
00287       }
00288     }
00289 
00290     if(!repeated)
00291       overlap_pairs.push_back(p);
00292   }
00293 
00294   void removeFromOverlapPairs(const SaPPair& p)
00295   {
00296     for(std::list<SaPPair>::iterator it = overlap_pairs.begin(), end = overlap_pairs.end();
00297         it != end;
00298         ++it)
00299     {
00300       if(*it == p)
00301       {
00302         overlap_pairs.erase(it);
00303         break;
00304       }
00305     }
00306 
00307     // or overlap_pairs.remove_if(isNotValidPair(p));
00308   }
00309 };
00310 
00311 
00312 
00313 }
00314 
00315 
00316 #endif
 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