collision_space_ccd: collision_space_ccd: environmentBVH.h Source 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 COLLISION_SPACE_CCD_ENVIRONMENT_MODEL_BVH_H
00038 #define COLLISION_SPACE_CCD_ENVIRONMENT_MODEL_BVH_H
00039 
00040 #include "collision_space_ccd/environment.h"
00041 #include "collision_checking/collision_geom.h"
00042 #include "collision_space_ccd/interval_tree.h"
00043 #include <map>
00044 #include <vector>
00045 #include <set>
00046 
00047 using namespace collision_checking;
00048 
00049 namespace collision_space_ccd
00050 {
00051         
00053 template<typename BV>
00054 class EnvironmentModelBVH : public EnvironmentModel
00055 {     
00056 public:
00057                 
00058   EnvironmentModelBVH(void);
00059 
00060   virtual ~EnvironmentModelBVH(void);
00061 
00062 
00064   virtual bool getCollisionContacts(const std::vector<AllowedContact>& allowedContacts, std::vector<Contact>& contacts, unsigned int max_count = 1) const;
00065 
00067   virtual bool getAllCollisionContacts(const std::vector<AllowedContact>& allowedContacts, std::vector<Contact>& contacts, unsigned int num_per_contact = 1) const;
00068 
00070   virtual bool isCollision(void) const;
00071 
00073   virtual bool isSelfCollision(void) const;
00074 
00076   virtual bool isEnvironmentCollision(void) const;
00077         
00079   virtual void clearObjects(void);
00080         
00082   virtual void clearObjects(const std::string& ns);
00083 
00085   virtual bool hasObject(const std::string& ns) const;
00086                 
00088   virtual void addObject(const std::string& ns, shapes::StaticShape* shape);
00089 
00091   virtual void addObject(const std::string& ns, shapes::Shape* shape, const btTransform& pose);
00092 
00094   virtual void addObjects(const std::string& ns, const std::vector<shapes::Shape*>& shapes, const std::vector<btTransform>& poses);
00095 
00096   virtual void getAttachedBodyPoses(std::map<std::string, std::vector<btTransform> >& pose_map) const;
00097 
00104   virtual void setRobotModel(const planning_models::KinematicModel* model, 
00105                              const AllowedCollisionMatrix& allowed_collision_matrix,
00106                              const std::map<std::string, double>& link_padding_map,
00107                              double default_padding = 0.0,
00108                              double scale = 1.0); 
00109 
00111   virtual void updateRobotModel(const planning_models::KinematicState* state);
00112 
00114   virtual void updateAttachedBodies(void);
00115 
00117   virtual void updateAttachedBodies(const std::map<std::string, double>& link_padding_map);
00118 
00120   virtual void setAlteredLinkPadding(const std::map<std::string, double>& link_padding_map);
00121 
00123   virtual void revertAlteredLinkPadding();
00124 
00126   virtual EnvironmentModel* clone(void) const;
00127 
00128 protected:
00129 
00131   struct AttGeom
00132   {
00133     ~AttGeom()
00134     {
00135       for(unsigned int i = 0; i < geom.size(); i++)
00136       {
00137         delete geom[i];
00138       }
00139       for(unsigned int i = 0; i < padded_geom.size(); i++)
00140       {
00141         delete padded_geom[i];
00142       }
00143     }
00144 
00145     std::vector<CollisionGeom*> geom; // managed by self_geom_manager
00146     std::vector<CollisionGeom*> padded_geom; // for collision with environment geometry
00147     const planning_models::KinematicModel::AttachedBodyModel* att;
00148     unsigned int index;
00149   };
00150 
00152   struct LinkGeom
00153   {
00154     ~LinkGeom()
00155     {
00156       for(unsigned int i = 0; i < geom.size(); i++)
00157       {
00158         delete geom[i];
00159       }
00160       for(unsigned int i = 0; i < padded_geom.size(); i++)
00161       {
00162         delete padded_geom[i];
00163       }
00164       deleteAttachedBodies();
00165     }
00166     
00167     void deleteAttachedBodies()
00168     {
00169       for(unsigned int i = 0; i < att_bodies.size(); i++)
00170       {
00171         delete att_bodies[i];
00172       }
00173       att_bodies.clear();
00174     }
00175 
00176     std::vector<CollisionGeom*> geom; // managed by self_geom_manager
00177     std::vector<CollisionGeom*> padded_geom; // for collision with environment geometry
00178     std::vector<AttGeom*> att_bodies;
00179     const planning_models::KinematicModel::LinkModel* link;
00180     unsigned int index;
00181   };
00182 
00183   struct CollisionData
00184   {
00185     CollisionData(void)
00186     {
00187       done = false;
00188       collides = false;
00189       max_contacts = 0;
00190       contacts = NULL;
00191       allowed_collision_matrix = NULL;
00192       allowed = NULL;
00193       exhaustive = false;
00194     }
00195 
00196     //these are parameters
00197     unsigned int max_contacts;
00198     const AllowedCollisionMatrix* allowed_collision_matrix;
00199     const std::map<CollisionGeom*, std::pair<std::string, BodyType> >* geom_lookup_map;
00200     const std::vector<AllowedContact>* allowed;
00201     bool exhaustive;
00202 
00203     //these are for return info
00204     bool done;
00205     bool collides;
00206     std::vector<EnvironmentModelBVH::Contact>* contacts;
00207 
00208     //for the last collision found
00209     std::string body_name_1;
00210     BodyType body_type_1;
00211 
00212     std::string body_name_2;
00213     BodyType body_type_2;
00214   };
00215 
00216   class SAPManager
00217   {
00218   public:
00219 
00220     SAPManager()
00221     {
00222       setup_ = false;
00223     }
00224 
00225     void unregisterGeom(CollisionGeom* geom);
00226 
00227     void registerGeom(CollisionGeom* geom);
00228 
00229     void setup();
00230 
00231     void update();
00232 
00233     void clear();
00234 
00235     void getGeoms(std::vector<CollisionGeom*>& geoms) const;
00236 
00237     void collide(CollisionGeom* geom, CollisionData* cdata) const;
00238 
00239     void checkColl(std::vector<CollisionGeom*>::const_iterator pos_start, std::vector<CollisionGeom*>::const_iterator pos_end,
00240                    CollisionGeom* geom, CollisionData* cdata) const;
00241 
00242     void collide(CollisionData* cdata) const;
00243 
00244     bool empty() const;
00245 
00246   private:
00247     struct SortByXLow
00248     {
00249       bool operator()(const CollisionGeom* a, const CollisionGeom* b) const
00250       {
00251         if(a->aabb.min_[0] < b->aabb.min_[0])
00252           return true;
00253         return false;
00254       }
00255     };
00256 
00257     struct SortByYLow
00258      {
00259        bool operator()(const CollisionGeom* a, const CollisionGeom* b) const
00260        {
00261          if(a->aabb.min_[1] < b->aabb.min_[1])
00262            return true;
00263          return false;
00264        }
00265      };
00266 
00267      struct SortByZLow
00268      {
00269        bool operator()(const CollisionGeom* a, const CollisionGeom* b) const
00270        {
00271          if(a->aabb.min_[2] < b->aabb.min_[2])
00272            return true;
00273          return false;
00274        }
00275      };
00276 
00277      struct SortByXTest
00278      {
00279        bool operator()(const CollisionGeom* a, const CollisionGeom* b) const
00280        {
00281          if(a->aabb.max_[0] < b->aabb.min_[0])
00282            return true;
00283          return false;
00284        }
00285      };
00286 
00287      struct SortByYTest
00288      {
00289        bool operator()(const CollisionGeom* a, const CollisionGeom* b) const
00290        {
00291          if(a->aabb.max_[1] < b->aabb.min_[1])
00292            return true;
00293          return false;
00294        }
00295      };
00296 
00297      struct SortByZTest
00298      {
00299        bool operator()(const CollisionGeom* a, const CollisionGeom* b) const
00300        {
00301          if(a->aabb.max_[2] < b->aabb.min_[2])
00302            return true;
00303          return false;
00304        }
00305      };
00306 
00307      std::vector<CollisionGeom*> geoms_x;
00308      std::vector<CollisionGeom*> geoms_y;
00309      std::vector<CollisionGeom*> geoms_z;
00310 
00311      bool setup_;
00312   };
00313         
00314   struct ModelInfo
00315   {
00316     std::vector<LinkGeom*> link_geom;
00317   };
00318         
00319   struct CollisionNamespace
00320   {
00321     CollisionNamespace(const std::string& nm) : name(nm) {}
00322 
00323     virtual ~CollisionNamespace(void)
00324     {
00325       clear();
00326     }
00327             
00328     void clear(void)
00329     {
00330       for(unsigned int i = 0; i < geoms.size(); ++i)
00331         delete geoms[i];
00332       geoms.clear();
00333     }
00334             
00335     std::string name;
00336     std::vector<CollisionGeom*> geoms;
00337     mutable SAPManager env_geom_manager; // manage all the geometries in the namespace
00338   };
00339         
00340         
00342   void testCollision(CollisionData* data) const;
00343 
00345   void testSelfCollision(CollisionData* data) const;
00346 
00348   void testEnvironmentCollision(CollisionData* data) const;
00349 
00351   void testObjectCollision(CollisionNamespace* cn, CollisionData* data) const;
00352 
00353   static void testGeomCollision(CollisionData* data, CollisionGeom* o1, CollisionGeom* o2);
00354 
00355   CollisionGeom* copyGeom(CollisionGeom* geom) const;
00356 
00357   void createBVHRobotModel();
00358 
00359   CollisionGeom* createBVHGeom(const shapes::Shape* shape, double scale, double padding);
00360 
00361   CollisionGeom* createBVHGeom(const shapes::StaticShape* shape);
00362 
00363   void updateGeom(CollisionGeom* geom, const btTransform& pose) const;
00364 
00365   void addAttachedBody(LinkGeom* lg, const planning_models::KinematicModel::AttachedBodyModel* attm,
00366                        double padd);
00367 
00368   std::map<std::string, bool> attached_bodies_in_collision_matrix_;
00369 
00370   void setAttachedBodiesLinkPadding();
00371 
00372   void revertAttachedBodiesLinkPadding();
00373 
00374   void freeMemory(void);        
00375         
00376   ModelInfo model_geom_;
00377   std::map<std::string, CollisionNamespace*> coll_namespaces_;
00378 
00379   std::map<CollisionGeom*, std::pair<std::string, BodyType> > geom_lookup_map_;
00380 
00381   bool previous_set_robot_model_;
00382 
00383   mutable SAPManager self_geom_manager;
00384 
00385 
00386 };
00387 }
00388 
00389 #endif