collision_space: collision_space: environmentBullet.h Source File

00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, 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 the Willow Garage 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_ENVIRONMENT_MODEL_BULLET_
00038 #define COLLISION_SPACE_ENVIRONMENT_MODEL_BULLET_
00039 
00040 #include "collision_space/environment.h"
00041 
00042 #include "btBulletCollisionCommon.h"
00043 #include <map>
00044 
00045 namespace collision_space
00046 {
00047         
00049     class EnvironmentModelBullet : public EnvironmentModel
00050     {     
00051     public:
00052         
00053         EnvironmentModelBullet(void) : EnvironmentModel(),
00054                                        m_selfCollisionFilterCallback(&m_selfCollisionTest),
00055                                        m_genericCollisionFilterCallback(&m_selfCollisionTest, &m_selfCollision)
00056         {
00057             m_config = new btDefaultCollisionConfiguration();
00058             btCollisionDispatcher* dispatcher = new CollisionDispatcher(&m_selfCollisionTest, &m_selfCollision, m_config);
00059             btBroadphaseInterface *broadphase = new btDbvtBroadphase();
00060             m_world = new btCollisionWorld(dispatcher, broadphase, m_config);
00061         }
00062         
00063         virtual ~EnvironmentModelBullet(void)
00064         { 
00065             freeMemory();
00066         }
00067         
00069         virtual bool getCollisionContacts(const std::vector<AllowedContact> &allowedContacts, std::vector<Contact> &contacts, unsigned int max_count = 1);
00070 
00072         virtual bool isCollision(void);
00073 
00075         virtual bool isSelfCollision(void);
00076         
00078         virtual void clearObjects(void);
00079         
00081         virtual void clearObjects(const std::string &ns);
00082         
00084         virtual void addObject(const std::string &ns, shapes::StaticShape *shape);
00085 
00087         virtual void addObject(const std::string &ns, shapes::Shape* shape, const btTransform &pose);
00088 
00090         virtual void addObjects(const std::string &ns, const std::vector<shapes::Shape*> &shapes, const std::vector<btTransform> &poses);
00091 
00093         virtual void removeCollidingObjects(const shapes::StaticShape *shape);
00094 
00096         virtual void removeCollidingObjects(const shapes::Shape *shape, const btTransform &pose);
00097 
00098       virtual const std::vector<const planning_models::KinematicModel::AttachedBodyModel*> getAttachedBodies(void) const;
00099         
00106       virtual void setRobotModel(const boost::shared_ptr<const planning_models::KinematicModel> &model, 
00107                                  const std::vector<std::string> &links,
00108                                  const std::map<std::string, double>& link_padding_map,
00109                                  double default_padding = 0.0,
00110                                  double scale = 1.0);
00111 
00113         virtual void updateRobotModel(void);
00114 
00116         virtual void updateAttachedBodies(void);
00117 
00119         virtual int setCollisionCheck(const std::string &link, bool state);
00120 
00122   void setCollisionCheckLinks(const std::vector<std::string> &links, bool state);
00123 
00125   void setCollisionCheckOnlyLinks(const std::vector<std::string> &links, bool state);
00126 
00128   void setCollisionCheckAll(bool state);
00129 
00131         virtual EnvironmentModel* clone(void) const;
00132         
00133     protected:
00134 
00135         struct kGeom
00136         {
00137             std::vector<btCollisionObject*>        geom;
00138             bool                                   enabled;
00139           const 
00140 planning_models::KinematicModel::LinkModel *link;
00141             unsigned int                           index;
00142         };
00143 
00144         struct ModelInfo
00145         {
00146             std::vector< kGeom* > linkGeom;
00147             double                scale;
00148             double                padding; 
00149         };
00150         
00151         struct SelfCollisionFilterCallback : public btOverlapFilterCallback
00152         {
00153             SelfCollisionFilterCallback(std::vector< std::vector<bool> > *test) : selfCollisionTest(test)
00154             {
00155             }
00156             
00157             virtual bool needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const
00158             {
00159                 assert(static_cast<btCollisionObject*>(proxy0->m_clientObject) != NULL);
00160                 assert(static_cast<btCollisionObject*>(proxy1->m_clientObject) != NULL);
00161                 
00162                 kGeom *k0 = reinterpret_cast<kGeom*>(reinterpret_cast<btCollisionObject*>(proxy0->m_clientObject)->getUserPointer());
00163                 kGeom *k1 = reinterpret_cast<kGeom*>(reinterpret_cast<btCollisionObject*>(proxy1->m_clientObject)->getUserPointer());
00164                 
00165                 // only check collision between links
00166                 if (!k0 || !k1)
00167                     return false;
00168                 
00169                 // only consider links that are enabled for self-collision checking
00170                 return selfCollisionTest->at(k0->index)[k1->index];
00171             }
00172             
00173             std::vector< std::vector<bool> > *selfCollisionTest;
00174         };
00175 
00176         struct GenericCollisionFilterCallback : public btOverlapFilterCallback
00177         {
00178             GenericCollisionFilterCallback(std::vector< std::vector<bool> > *test, bool *checkSelf) : selfCollisionTest(test),
00179                                                                                                       enableSelfCollision(checkSelf)
00180             {
00181             }
00182             
00183             virtual bool needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const
00184             {
00185                 assert(static_cast<btCollisionObject*>(proxy0->m_clientObject) != NULL);
00186                 assert(static_cast<btCollisionObject*>(proxy1->m_clientObject) != NULL);
00187                 
00188                 kGeom *k0 = reinterpret_cast<kGeom*>(reinterpret_cast<btCollisionObject*>(proxy0->m_clientObject)->getUserPointer());
00189                 kGeom *k1 = reinterpret_cast<kGeom*>(reinterpret_cast<btCollisionObject*>(proxy1->m_clientObject)->getUserPointer());
00190 
00191                 // do not check collision between obstacles
00192                 if (!k0 && !k1)
00193                     return false;
00194 
00195                 // do not check disabled links
00196                 if (k0)
00197                     if (k0->enabled == false)
00198                         return false;
00199                 if (k1)
00200                     if (k1->enabled == false)
00201                         return false;
00202                 
00203                 // do not check collision between links that should not be self-collision checked
00204                 if (k0 && k1)
00205                 {
00206                     if (*enableSelfCollision)
00207                         return selfCollisionTest->at(k0->index)[k1->index];
00208                     else
00209                         return false;
00210                 }
00211                 
00212                 return true;
00213             }
00214             
00215             std::vector< std::vector<bool> > *selfCollisionTest;
00216             bool                             *enableSelfCollision;
00217         };
00218         
00219         class CollisionDispatcher : public btCollisionDispatcher
00220         {
00221         public:
00222             CollisionDispatcher(std::vector< std::vector<bool> > *test, bool *enableSelfCollision,
00223                                 btCollisionConfiguration *config) : btCollisionDispatcher(config),
00224                                                                     m_selfCollisionTest(test),
00225                                                                     m_enableSelfCollision(enableSelfCollision)
00226             {
00227             }
00228             
00229             virtual bool needsCollision(btCollisionObject* b0, btCollisionObject* b1)
00230             {   
00231                 kGeom *k0 = reinterpret_cast<kGeom*>(b0->getUserPointer());
00232                 kGeom *k1 = reinterpret_cast<kGeom*>(b1->getUserPointer());
00233                 
00234                 if (k0 || k1)
00235                 {
00236                     if (k0)
00237                         if (k0->enabled == false)
00238                             return false;
00239                     if (k1)
00240                         if (k1->enabled == false)
00241                             return false;
00242                     if (k0 && k1)
00243                         return m_selfCollisionTest->at(k0->index)[k1->index];
00244                     return true;
00245                 }
00246                 else
00247                     return false;
00248             }
00249             
00250         protected:
00251             
00252             std::vector< std::vector<bool> > *m_selfCollisionTest;
00253             bool                             *m_enableSelfCollision;
00254         };
00255         
00256         btCollisionObject* createCollisionBody(const shapes::Shape *shape, double scale, double padding);
00257         btCollisionObject* createCollisionBody(const shapes::StaticShape *shape);
00258         
00259         void freeMemory(void);
00260         
00261         SelfCollisionFilterCallback         m_selfCollisionFilterCallback;
00262         GenericCollisionFilterCallback      m_genericCollisionFilterCallback;
00263         
00264         ModelInfo                           m_modelGeom;
00265         std::map<std::string, std::vector<btCollisionObject*> > 
00266                                             m_obstacles;
00267         btCollisionWorld                   *m_world;
00268         btDefaultCollisionConfiguration    *m_config;
00269         
00270     };
00271 }
00272 
00273 #endif