37 #include "ompl/extensions/opende/OpenDEStatePropagator.h"
38 #include "ompl/extensions/opende/OpenDEStateSpace.h"
39 #include "ompl/extensions/opende/OpenDEControlSpace.h"
40 #include "ompl/util/Exception.h"
45 if (
OpenDEStateSpace *oss = dynamic_cast<OpenDEStateSpace*>(si->getStateSpace().get()))
46 env_ = oss->getEnvironment();
48 throw Exception(
"OpenDE State Space needed for OpenDEStatePropagator");
61 void nearCallback(
void *data, dGeomID o1, dGeomID o2)
63 dBodyID b1 = dGeomGetBody(o1);
64 dBodyID b2 = dGeomGetBody(o2);
66 if (b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact))
return;
68 CallbackParam *cp =
reinterpret_cast<CallbackParam*
>(data);
70 const unsigned int maxContacts = cp->env->getMaxContacts(o1, o2);
71 if (maxContacts <= 0)
return;
73 dContact *contact =
new dContact[maxContacts];
75 for (
unsigned int i = 0; i < maxContacts; ++i)
76 cp->env->setupContact(o1, o2, contact[i]);
78 if (
int numc = dCollide(o1, o2, maxContacts, &contact[0].geom,
sizeof(dContact)))
80 for (
int i = 0; i < numc; ++i)
82 dJointID c = dJointCreateContact(cp->env->world_, cp->env->contactGroup_, contact + i);
83 dJointAttach(c, b1, b2);
84 bool valid = cp->env->isValidCollision(o1, o2, contact[i]);
87 if (cp->env->verboseContacts_)
89 OMPL_DEBUG(
"%s contact between %s and %s", (valid ?
"Valid" :
"Invalid"),
90 cp->env->getGeomName(o1).c_str(), cp->env->getGeomName(o1).c_str());
111 CallbackParam cp = { env_.get(),
false };
112 for (
unsigned int i = 0 ; i < env_->collisionSpaces_.size() ; ++i)
113 dSpaceCollide(env_->collisionSpaces_[i], &cp, &nearCallback);
116 dWorldQuickStep(env_->world_, (
const dReal)duration);
119 dJointGroupEmpty(env_->contactGroup_);
124 env_->mutex_.unlock();