diff --git a/panda/src/ode/odeGeom.cxx b/panda/src/ode/odeGeom.cxx index 523136d3fb..b3901dbd4e 100755 --- a/panda/src/ode/odeGeom.cxx +++ b/panda/src/ode/odeGeom.cxx @@ -19,6 +19,7 @@ #include "config_ode.h" #include "odeGeom.h" +OdeGeom::GeomSurfaceMap OdeGeom::_geom_surface_map; TypeHandle OdeGeom::_type_handle; OdeGeom:: @@ -30,7 +31,34 @@ OdeGeom(dGeomID id) : OdeGeom:: ~OdeGeom() { odegeom_cat.debug() << "~" << get_type() << "(" << _id << ")\n"; + GeomSurfaceMap::iterator iter = _geom_surface_map.find(this->get_id()); + if (iter != _geom_surface_map.end()) { + _geom_surface_map.erase(iter); + } } +int OdeGeom:: +get_default_surface_type() +{ + return OdeGeom::get_default_surface_type(this->get_id()); +} + +int OdeGeom:: +get_default_surface_type(dGeomID id) +{ + GeomSurfaceMap::iterator iter = _geom_surface_map.find(id); + if (iter != _geom_surface_map.end()) { + // odegeom_cat.debug() << "get_default_surface_type the geomId =" << id <<" surfaceType=" << iter->second << "\n"; + return iter->second; + } + // odegeom_cat.debug() << "get_default_surface_type not in map, returning 0" ; + return 0; +} + +void OdeGeom:: +set_default_surface_type( int surfaceType){ + _geom_surface_map[this->get_id()]= surfaceType; +} + void OdeGeom:: destroy() { diff --git a/panda/src/ode/odeGeom.h b/panda/src/ode/odeGeom.h index ca92a71934..9aa35d0a32 100755 --- a/panda/src/ode/odeGeom.h +++ b/panda/src/ode/odeGeom.h @@ -105,6 +105,9 @@ PUBLISHED: INLINE LPoint3f get_offset_position() const; INLINE LMatrix3f get_offset_rotation() const; INLINE LQuaternionf get_offset_quaternion() const; + int get_default_surface_type() ; + void set_default_surface_type( int surfaceType); + static int get_default_surface_type(dGeomID id); OdeSpace get_space() const; @@ -132,6 +135,10 @@ public: protected: dGeomID _id; +private: + typedef pmap GeomSurfaceMap; + static GeomSurfaceMap _geom_surface_map; + public: static TypeHandle get_class_type() { return _type_handle; diff --git a/panda/src/ode/odeSpace.cxx b/panda/src/ode/odeSpace.cxx index 6d1df37db0..1fb31df811 100755 --- a/panda/src/ode/odeSpace.cxx +++ b/panda/src/ode/odeSpace.cxx @@ -77,11 +77,12 @@ clean() { dSpaceClean(_id); } -OdeGeom OdeSpace:: +OdeGeom OdeSpace:: get_geom(int i) { return OdeGeom(dSpaceGetGeom(_id, i)); } + void OdeSpace:: write(ostream &out, unsigned int indent) const { #ifndef NDEBUG //[ @@ -106,7 +107,7 @@ int OdeSpace:: autoCollide() { OdeSpace::contactCount = 0; - dSpaceCollide(_id, 0, &autoCallback); + dSpaceCollide(_id, this, &autoCallback); return OdeSpace::contactCount; } @@ -120,35 +121,64 @@ get_contact_data(int data_index) return OdeSpace::contact_data[data_index]; } +int OdeSpace:: +get_surface_type(OdeSpace * self, dGeomID o1) +// return the surface of the body if this geom has a body, else use the default surface type +{ + int retval = 0; + dBodyID b1 = dGeomGetBody(o1); + if (b1) { + retval = _collide_world->get_surface_body(b1).surfaceType; + } + else { + retval = OdeGeom::get_default_surface_type(o1); + } + // odespace_cat.debug() << "for geom " << o1 << " returning surface type " << retval << "\n"; + return retval; + +} + +int autoCallbackCounter = 0; + 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]; + int surface1 = get_surface_type((OdeSpace*)data, o1); + int surface2 = get_surface_type((OdeSpace*)data, o2); + + sSurfaceParams collide_params; - collide_params = _collide_world->get_surface(_collide_world->get_surface_body(b1).surfaceType,_collide_world->get_surface_body(b2).surfaceType); + // collide_params = _collide_world->get_surface(_collide_world->get_surface_body(b1).surfaceType,_collide_world->get_surface_body(b2).surfaceType); + collide_params = _collide_world->get_surface(surface1, surface2); for (i=0; i < OdeSpace::MAX_CONTACTS; i++) { - contact[i].surface.mode = collide_params.colparams.mode; contact[i].surface.mu = collide_params.colparams.mu; contact[i].surface.mu2 = collide_params.colparams.mu2; contact[i].surface.bounce = collide_params.colparams.bounce; contact[i].surface.bounce_vel = collide_params.colparams.bounce_vel; contact[i].surface.soft_cfm = collide_params.colparams.soft_cfm; - } - if (int numc = dCollide(o1, o2, OdeSpace::MAX_CONTACTS, &contact[0].geom, sizeof(dContact))) + int numc = dCollide(o1, o2, OdeSpace::MAX_CONTACTS, &contact[0].geom, sizeof(dContact)); + if (numc) { + if (odespace_cat.is_debug() && (autoCallbackCounter%30 == 0)) { + odespace_cat.debug() << autoCallbackCounter <<" collision between geoms " << o1 << " and " << o2 << "\n"; + odespace_cat.debug() << "collision between body " << b1 << " and " << b2 << "\n"; + odespace_cat.debug() << "surface1= "<< surface1 << " surface2=" << surface2 << "\n"; + } + autoCallbackCounter += 1; + for(i=0; i < numc; i++) { dJointID c = dJointCreateContact(_collide_world->get_id(), _collide_joint_group, contact + i); diff --git a/panda/src/ode/odeSpace.h b/panda/src/ode/odeSpace.h index 0db0e7c4e4..9820657121 100755 --- a/panda/src/ode/odeSpace.h +++ b/panda/src/ode/odeSpace.h @@ -78,6 +78,7 @@ PUBLISHED: void remove(OdeSpace& space); void clean(); OdeGeom get_geom(int i); // Not INLINE because of forward declaration + static int get_surface_type(OdeSpace * self, dGeomID o1); INLINE OdeSpace get_space() const; diff --git a/panda/src/ode/odeWorld.cxx b/panda/src/ode/odeWorld.cxx index ddcaae9e48..5799c72dc8 100755 --- a/panda/src/ode/odeWorld.cxx +++ b/panda/src/ode/odeWorld.cxx @@ -26,12 +26,14 @@ OdeWorld() : _id(dWorldCreate()) { odeworld_cat.debug() << get_type() << "(" << _id << ")" << "\n"; _num_surfaces = 0; + } OdeWorld:: OdeWorld(const OdeWorld ©) : _id(copy._id) { _num_surfaces = 0; + } OdeWorld:: @@ -51,6 +53,7 @@ destroy() { void OdeWorld:: assign_surface_body(OdeBody& body, int surface) { + // odeworld_cat.debug() << "assign_surface_body body.Id =" << body.get_id() << " surface=" << surface << "\n"; _body_surface_map[body.get_id()].surfaceType = surface; _body_surface_map[body.get_id()].dampen = 0.0f; } @@ -158,11 +161,13 @@ get_surface_body(dBodyID id) BodySurfaceMap::iterator iter = _body_surface_map.find(id); if (iter != _body_surface_map.end()) { + // odeworld_cat.debug() << "In map the bodyId =" << id <<" surfaceType=" << iter->second.surfaceType << "\n"; return iter->second; } sBodyParams defaultParams; defaultParams.surfaceType = 0; defaultParams.dampen = 0.0f; + // odeworld_cat.debug() << "no surface for bodyId=" << id << " returning default 0" << "\n"; return defaultParams; }