diff --git a/panda/src/ode/odeSpace.cxx b/panda/src/ode/odeSpace.cxx index 73b979f6dc..117d1c3849 100755 --- a/panda/src/ode/odeSpace.cxx +++ b/panda/src/ode/odeSpace.cxx @@ -19,7 +19,14 @@ #include "config_ode.h" #include "odeSpace.h" + TypeHandle OdeSpace::_type_handle; +// this data is used in auto_collide +const int OdeSpace::MAX_CONTACTS = 16; +OdeWorld* OdeSpace::_collide_world; +dJointGroupID OdeSpace::_collide_joint_group; +int OdeSpace::contactCount = 0; +double OdeSpace::contact_data[192]; OdeSpace:: OdeSpace(dSpaceID id) : @@ -82,6 +89,81 @@ write(ostream &out, unsigned int indent) const { #endif //] NDEBUG } + +void OdeSpace:: +set_auto_collide_world(OdeWorld &world) +{ + _collide_world = &world; +} + +void OdeSpace:: +set_auto_collide_joint_group(OdeJointGroup &joint_group) +{ + _collide_joint_group = joint_group.get_id(); +} + +int OdeSpace:: +autoCollide() +{ + OdeSpace::contactCount = 0; + dSpaceCollide(_id, 0, &autoCallback); + return OdeSpace::contactCount; +} + +double OdeSpace:: +get_contact_data(int data_index) +// get the contact data it looks like so [x1,y1,z1,x2,y2,z2... x64,y64,z64] +// use the return in from autoCollide to determine how much of the data is +// valid. The data would be more straight forward but the callbacks have to be +// static. +{ + return OdeSpace::contact_data[data_index]; +} + +void OdeSpace:: +autoCallback(void *data, dGeomID o1, dGeomID o2) +// uses data stored on the world to resolve collisions so you don't have to use near_callbacks in python +{ + int i; + + dBodyID b1 = dGeomGetBody(o1); + dBodyID b2 = dGeomGetBody(o2); + + dContact contact[OdeSpace::MAX_CONTACTS]; + + dSurfaceParameters collide_params; + collide_params = _collide_world->get_surface(_collide_world->get_surface_body(b1),_collide_world->get_surface_body(b2)); + + for (i=0; i < OdeSpace::MAX_CONTACTS; i++) + { + + contact[i].surface.mode = collide_params.mode; + contact[i].surface.mu = collide_params.mu; + contact[i].surface.mu2 = collide_params.mu2; + contact[i].surface.bounce = collide_params.bounce; + contact[i].surface.bounce_vel = collide_params.bounce_vel; + contact[i].surface.soft_cfm = collide_params.soft_cfm; + + } + + if (int numc = dCollide(o1, o2, OdeSpace::MAX_CONTACTS, &contact[0].geom, sizeof(dContact))) + { + for(i=0; i < numc; i++) + { + dJointID c = dJointCreateContact(_collide_world->get_id(), _collide_joint_group, contact + i); + dJointAttach(c, b1, b2); + // this creates contact position data for python. It is useful for debugging only 64 points are stored + if(contactCount * 3 < 192) + { + OdeSpace::contact_data[0 + (OdeSpace::contactCount * 3)] = contact[i].geom.pos[0]; + OdeSpace::contact_data[1 + (OdeSpace::contactCount * 3)] = contact[i].geom.pos[1]; + OdeSpace::contact_data[2 + (OdeSpace::contactCount * 3)] = contact[i].geom.pos[2]; + OdeSpace::contactCount += 1; + } + } + } +} + OdeSimpleSpace OdeSpace:: convert_to_simple_space() const { nassertr(_id != 0, OdeSimpleSpace((dSpaceID)0)); diff --git a/panda/src/ode/odeSpace.h b/panda/src/ode/odeSpace.h index 5807a48821..c005752571 100755 --- a/panda/src/ode/odeSpace.h +++ b/panda/src/ode/odeSpace.h @@ -24,6 +24,10 @@ #include "luse.h" #include "bitMask.h" +//included for collision tests +#include "odeWorld.h" +#include "odeJointGroup.h" + #include "ode_includes.h" class OdeGeom; @@ -31,6 +35,8 @@ class OdeTriMeshGeom; class OdeSimpleSpace; class OdeHashSpace; class OdeQuadTreeSpace; + + //////////////////////////////////////////////////////////////////// // Class : OdeSpace @@ -38,6 +44,8 @@ class OdeQuadTreeSpace; //////////////////////////////////////////////////////////////////// class EXPCL_PANDAODE OdeSpace : public TypedObject { friend class OdeGeom; + static const int MAX_CONTACTS; + protected: OdeSpace(dSpaceID id); @@ -61,6 +69,8 @@ PUBLISHED: INLINE void enable(); INLINE void disable(); INLINE int is_enabled(); + INLINE void set_auto_collide_world(OdeWorld&); + INLINE void set_auto_collide_joint_group(OdeJointGroup&); void add(OdeGeom& geom); void add(OdeSpace& space); @@ -76,12 +86,24 @@ PUBLISHED: OdeSimpleSpace convert_to_simple_space() const; OdeHashSpace convert_to_hash_space() const; OdeQuadTreeSpace convert_to_quad_tree_space() const; + + int autoCollide(); + static void autoCallback(void*, dGeomID, dGeomID); + static double get_contact_data(int data_index); public: INLINE dSpaceID get_id() const; + static OdeWorld* _collide_world; + static dJointGroupID _collide_joint_group; + static int contactCount; + + static double contact_data[192]; // 64 times three + protected: dSpaceID _id; + int _g; // REMOVE ME + public: static TypeHandle get_class_type() { diff --git a/panda/src/ode/odeWorld.cxx b/panda/src/ode/odeWorld.cxx index eaa066ed0e..60ba7c78f4 100755 --- a/panda/src/ode/odeWorld.cxx +++ b/panda/src/ode/odeWorld.cxx @@ -25,11 +25,13 @@ OdeWorld:: OdeWorld() : _id(dWorldCreate()) { odeworld_cat.debug() << get_type() << "(" << _id << ")" << "\n"; + _num_surfaces = 0; } OdeWorld:: OdeWorld(const OdeWorld ©) : _id(copy._id) { + _num_surfaces = 0; } OdeWorld:: @@ -39,5 +41,98 @@ OdeWorld:: void OdeWorld:: destroy() { + if(_num_surfaces > 0) + { + delete _surface_table; + } dWorldDestroy(_id); } + +void OdeWorld:: +init_surface_table(uint8 num_surfaces) +{ + _surface_table = new dSurfaceParameters[num_surfaces * num_surfaces]; + _num_surfaces = num_surfaces; +} + +void OdeWorld:: +set_surface(int pos1, int pos2, dSurfaceParameters& entry) +{ + int true_pos = (pos1 * _num_surfaces) + pos2; + _surface_table[true_pos].mode = entry.mode; + _surface_table[true_pos].mu = entry.mu; + _surface_table[true_pos].mu2 = entry.mu2; + _surface_table[true_pos].bounce = entry.bounce; + _surface_table[true_pos].bounce_vel = entry.bounce_vel; + _surface_table[true_pos].soft_cfm = entry.soft_cfm; + _surface_table[true_pos].motion1 = entry.motion1; + _surface_table[true_pos].motion2 = entry.motion2; + _surface_table[true_pos].slip1 = entry.slip1; + _surface_table[true_pos].slip2 = entry.slip2; +} + +dSurfaceParameters& OdeWorld:: +get_surface(uint8 surface1, uint8 surface2) +{ + int true_pos = 0; + if(surface1 >= surface2) + { + true_pos = (surface1 * _num_surfaces) + surface2; + } + else + { + true_pos = (surface2 * _num_surfaces) + surface1; + } + return _surface_table[true_pos]; +} + +void OdeWorld:: +set_surface_entry( uint8 pos1, uint8 pos2, + dReal mu, + dReal bounce, + dReal bounce_vel, + dReal soft_erp, + dReal soft_cfm, + dReal slip) +{ + //todo: add mode + dSurfaceParameters new_params; + new_params.mode = 0; + new_params.mu = mu; + new_params.mu2 = mu; + new_params.bounce = bounce; + new_params.bounce_vel = bounce_vel; + new_params.soft_erp = soft_erp; + new_params.soft_cfm = soft_cfm; + new_params.slip1 = slip; + new_params.slip2 = slip; + new_params.motion1 = 0.0; + new_params.motion2 = 0.0; + //todo: a bit of wasted space here + set_surface(pos1, pos2, new_params); + + if(pos1 >= pos2) + { + set_surface(pos1, pos2, new_params); + } + else + { + set_surface(pos2, pos1, new_params); + } +} + +void OdeWorld:: +assign_surface_body(OdeBody& body, int surface) +{ + _body_surface_map[body.get_id()] = surface; +} + +int OdeWorld:: +get_surface_body(dBodyID id) +{ + BodySurfaceMap::iterator iter = _body_surface_map.find(id); + if (iter != _body_surface_map.end()) { + return iter->second; + } + return 0; +} diff --git a/panda/src/ode/odeWorld.h b/panda/src/ode/odeWorld.h index 8e5472d9ec..398adb3f1f 100755 --- a/panda/src/ode/odeWorld.h +++ b/panda/src/ode/odeWorld.h @@ -78,11 +78,29 @@ PUBLISHED: INLINE void step_fast1(dReal stepsize, int maxiterations); INLINE int compare_to(const OdeWorld &other) const; + INLINE void init_surface_table(uint8 num_surfaces); + INLINE void assign_surface_body(OdeBody& body, int surface); + INLINE void set_surface_entry(uint8 pos1, uint8 pos2, + dReal mu, + dReal bounce, + dReal bounce_vel, + dReal soft_erp, + dReal soft_cfm, + dReal slip); + + public: INLINE dWorldID get_id() const; + INLINE dSurfaceParameters& get_surface(uint8 surface1, uint8 surface2); + INLINE void set_surface(int pos1, int pos2, dSurfaceParameters& entry); + INLINE int get_surface_body(dBodyID id); private: dWorldID _id; + dSurfaceParameters *_surface_table; + uint8 _num_surfaces; + typedef pmap BodySurfaceMap; + BodySurfaceMap _body_surface_map; public: static TypeHandle get_class_type() {