Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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 tf::Transform &pose);
00088
00090 virtual void addObjects(const std::string &ns, const std::vector<shapes::Shape*> &shapes, const std::vector<tf::Transform> &poses);
00091
00093 virtual void removeCollidingObjects(const shapes::StaticShape *shape);
00094
00096 virtual void removeCollidingObjects(const shapes::Shape *shape, const tf::Transform &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
00166 if (!k0 || !k1)
00167 return false;
00168
00169
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
00192 if (!k0 && !k1)
00193 return false;
00194
00195
00196 if (k0)
00197 if (k0->enabled == false)
00198 return false;
00199 if (k1)
00200 if (k1->enabled == false)
00201 return false;
00202
00203
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