diff --git a/panda/src/ode/odeBody.cxx b/panda/src/ode/odeBody.cxx index 9e0f8f45a8..03a8aae5ce 100755 --- a/panda/src/ode/odeBody.cxx +++ b/panda/src/ode/odeBody.cxx @@ -29,6 +29,7 @@ OdeBody(dBodyID id) : OdeBody:: OdeBody(OdeWorld &world) : _id(dBodyCreate(world.get_id())) { + world.add_body_dampening(*this, 0); } OdeBody:: diff --git a/panda/src/ode/odeGeom.cxx b/panda/src/ode/odeGeom.cxx index b3901dbd4e..3236866328 100755 --- a/panda/src/ode/odeGeom.cxx +++ b/panda/src/ode/odeGeom.cxx @@ -19,7 +19,8 @@ #include "config_ode.h" #include "odeGeom.h" -OdeGeom::GeomSurfaceMap OdeGeom::_geom_surface_map; +//OdeGeom::GeomSurfaceMap OdeGeom::_geom_surface_map; +//OdeGeom::GeomCollideIdMap OdeGeom::_geom_collide_id_map; TypeHandle OdeGeom::_type_handle; OdeGeom:: @@ -31,34 +32,56 @@ 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()); + + GeomCollideIdMap::iterator iter2 = _geom_collide_id_map.find(this->get_id()); + if (iter2 != _geom_collide_id_map.end()) { + _geom_collide_id_map.erase(iter2); + } + */ } int OdeGeom:: -get_default_surface_type(dGeomID id) +get_surface_type() { - 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; + return get_space().get_surface_type(this->get_id()); +} + +int OdeGeom:: +get_collide_id() +{ + return get_space().get_collide_id(this->get_id()); } void OdeGeom:: -set_default_surface_type( int surfaceType){ - _geom_surface_map[this->get_id()]= surfaceType; +set_surface_type(int surface_type) +{ + get_space().set_surface_type(surface_type, this->get_id()); } +int OdeGeom:: +set_collide_id(int collide_id) +{ + return get_space().set_collide_id(collide_id, this->get_id()); +} + + +int OdeGeom:: +test_collide_id(int collide_id) +{ + + odegeom_cat.debug() << "test_collide_id start" << "\n"; + int first = get_space().set_collide_id(collide_id, this->get_id()); + odegeom_cat.debug() << "returns" << first << "\n"; + odegeom_cat.debug() << "test_collide_id middle" << "\n"; + int test = get_space().get_collide_id(this->get_id()); + odegeom_cat.debug() << "test_collide_id stop" << "\n"; + return test; +} void OdeGeom:: destroy() { diff --git a/panda/src/ode/odeGeom.h b/panda/src/ode/odeGeom.h index 9aa35d0a32..b4a5d158f2 100755 --- a/panda/src/ode/odeGeom.h +++ b/panda/src/ode/odeGeom.h @@ -105,9 +105,14 @@ 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); + + int get_surface_type() ; + int get_collide_id() ; + int set_collide_id( int collide_id); + void set_surface_type( int surface_type); + + int test_collide_id( int collide_id); + OdeSpace get_space() const; @@ -126,6 +131,8 @@ PUBLISHED: OdeSimpleSpace convert_to_simple_space() const; OdeHashSpace convert_to_hash_space() const; OdeQuadTreeSpace convert_to_quad_tree_space() const; + + public: INLINE dGeomID get_id() const; @@ -135,9 +142,6 @@ public: protected: dGeomID _id; -private: - typedef pmap GeomSurfaceMap; - static GeomSurfaceMap _geom_surface_map; public: static TypeHandle get_class_type() { diff --git a/panda/src/ode/odeHelperStructs.h b/panda/src/ode/odeHelperStructs.h index 32ef7f0194..917a3ee643 100644 --- a/panda/src/ode/odeHelperStructs.h +++ b/panda/src/ode/odeHelperStructs.h @@ -9,7 +9,7 @@ struct sSurfaceParams struct sBodyParams { - int surfaceType; +// int surfaceType; dReal dampen; }; diff --git a/panda/src/ode/odeSpace.cxx b/panda/src/ode/odeSpace.cxx index 1fb31df811..bd92bdecbd 100755 --- a/panda/src/ode/odeSpace.cxx +++ b/panda/src/ode/odeSpace.cxx @@ -24,9 +24,11 @@ TypeHandle OdeSpace::_type_handle; // this data is used in auto_collide const int OdeSpace::MAX_CONTACTS = 16; OdeWorld* OdeSpace::_collide_world; +OdeSpace* OdeSpace::_collide_space; dJointGroupID OdeSpace::_collide_joint_group; int OdeSpace::contactCount = 0; double OdeSpace::contact_data[192]; +int OdeSpace::contact_ids[128]; OdeSpace:: OdeSpace(dSpaceID id) : @@ -94,7 +96,7 @@ write(ostream &out, unsigned int indent) const { void OdeSpace:: set_auto_collide_world(OdeWorld &world) { - _collide_world = &world; + my_world = &world; } void OdeSpace:: @@ -107,6 +109,8 @@ int OdeSpace:: autoCollide() { OdeSpace::contactCount = 0; + _collide_space = this; + _collide_world = my_world; dSpaceCollide(_id, this, &autoCallback); return OdeSpace::contactCount; } @@ -121,42 +125,41 @@ 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 OdeSpace:: +get_contact_id(int data_index, int first) +// 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. { - int retval = 0; - dBodyID b1 = dGeomGetBody(o1); - if (b1) { - retval = _collide_world->get_surface_body(b1).surfaceType; + if (first == 0) + { + return OdeSpace::contact_ids[(data_index * 2) + 0]; } - else { - retval = OdeGeom::get_default_surface_type(o1); + else + { + return OdeSpace::contact_ids[(data_index * 2) + 1]; } - // 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; + static int autoCallbackCounter = 0; dBodyID b1 = dGeomGetBody(o1); dBodyID b2 = dGeomGetBody(o2); dContact contact[OdeSpace::MAX_CONTACTS]; + + int surface1 = _collide_space->get_surface_type(o1); + int surface2 = _collide_space->get_surface_type(o2); - 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(surface1, surface2); for (i=0; i < OdeSpace::MAX_CONTACTS; i++) @@ -184,11 +187,13 @@ autoCallback(void *data, dGeomID o1, dGeomID o2) 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) + if(contactCount < 64) { 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::contact_ids[0 + (OdeSpace::contactCount * 2)] = _collide_space->get_collide_id(o1); + OdeSpace::contact_ids[1 + (OdeSpace::contactCount * 2)] = _collide_space->get_collide_id(o2); OdeSpace::contactCount += 1; } } @@ -217,3 +222,89 @@ convert_to_quad_tree_space() const { return OdeQuadTreeSpace(_id); } + +void OdeSpace:: +set_surface_type( int surfaceType, dGeomID id){ + _geom_surface_map[id]= surfaceType; +} + +void OdeSpace:: +set_surface_type(OdeGeom& geom, int surfaceType){ + dGeomID id = geom.get_id(); + set_surface_type(surfaceType, id); +} + +int OdeSpace:: +get_surface_type(dGeomID id) +{ + GeomSurfaceMap::iterator iter = _geom_surface_map.find(id); + if (iter != _geom_surface_map.end()) { + // odespace_cat.debug() << "get_default_surface_type the geomId =" << id <<" surfaceType=" << iter->second << "\n"; + return iter->second; + } + // odespace_cat.debug() << "get_default_surface_type not in map, returning 0" ; + return 0; +} + +int OdeSpace:: +get_surface_type(OdeGeom& geom){ + dGeomID id = geom.get_id(); + return get_surface_type(id); +} + + +int OdeSpace:: +set_collide_id( int collide_id, dGeomID id){ + _geom_collide_id_map[id]= collide_id; + + //odespace_cat.debug() << "set_collide_id " << id << " " << _geom_collide_id_map[id] <<"\n"; + +/* + GeomCollideIdMap::iterator iter2 = _geom_collide_id_map.begin(); + while (iter2 != _geom_collide_id_map.end()) { + odespace_cat.debug() << "set print get_collide_id the geomId =" << iter2->first <<" collide_id=" << iter2->second << "\n"; + iter2++; + } + + get_collide_id(id); +*/ + return _geom_collide_id_map[id]; +} + +int OdeSpace:: +set_collide_id( OdeGeom& geom, int collide_id) +{ + dGeomID id = geom.get_id(); + return set_collide_id(collide_id, id); +} + +int OdeSpace:: +get_collide_id(OdeGeom& geom) +{ + dGeomID id = geom.get_id(); + return get_collide_id(id); +} + + +int OdeSpace:: +get_collide_id(dGeomID id) +{ + +/* + GeomCollideIdMap::iterator iter2 = _geom_collide_id_map.begin(); + while (iter2 != _geom_collide_id_map.end()) { + odespace_cat.debug() << "get print get_collide_id the geomId =" << iter2->first <<" collide_id=" << iter2->second << "\n"; + iter2++; + } +*/ + + GeomCollideIdMap::iterator iter = _geom_collide_id_map.find(id); + if (iter != _geom_collide_id_map.end()) { + //odespace_cat.debug() << "get_collide_id the geomId =" << id <<" collide_id=" << iter->second << "\n"; + return iter->second; + } + + //odespace_cat.debug() << "get_collide_id not in map, returning 0 id =" << id << "\n" ; + return 0; +} + diff --git a/panda/src/ode/odeSpace.h b/panda/src/ode/odeSpace.h index 9820657121..b3f4dc2c61 100755 --- a/panda/src/ode/odeSpace.h +++ b/panda/src/ode/odeSpace.h @@ -78,7 +78,8 @@ 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); + //static int get_surface_type(OdeSpace * self, dGeomID o1); + INLINE OdeSpace get_space() const; @@ -91,19 +92,33 @@ PUBLISHED: int autoCollide(); static void autoCallback(void*, dGeomID, dGeomID); static double get_contact_data(int data_index); + int get_contact_id(int data_index, int first = 0); + int set_collide_id( int collide_id, dGeomID id); + int set_collide_id(OdeGeom& geom, int collide_id); + void set_surface_type( int surface_type, dGeomID id); + void set_surface_type(OdeGeom& geom, int surface_type); + int get_surface_type(dGeomID o1); + int get_surface_type(OdeGeom& geom); + int get_collide_id(dGeomID o1); + int get_collide_id(OdeGeom& geom); + public: INLINE dSpaceID get_id() const; static OdeWorld* _collide_world; + static OdeSpace* _collide_space; static dJointGroupID _collide_joint_group; static int contactCount; static double contact_data[192]; // 64 times three + static int contact_ids[128]; // 64 times two protected: dSpaceID _id; int _g; // REMOVE ME + OdeWorld* my_world; + public: @@ -122,6 +137,13 @@ public: private: static TypeHandle _type_handle; + + typedef pmap GeomSurfaceMap; + GeomSurfaceMap _geom_surface_map; + + typedef pmap GeomCollideIdMap; + GeomCollideIdMap _geom_collide_id_map; + }; #include "odeSpace.I" diff --git a/panda/src/ode/odeWorld.cxx b/panda/src/ode/odeWorld.cxx index 5799c72dc8..93ed8c2818 100755 --- a/panda/src/ode/odeWorld.cxx +++ b/panda/src/ode/odeWorld.cxx @@ -50,13 +50,22 @@ destroy() { dWorldDestroy(_id); } +/* 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; + _body_dampen_map[body.get_id()].surfaceType = surface; + _body_dampen_map[body.get_id()].dampen = 0.0f; } +*/ + +void OdeWorld:: +add_body_dampening(OdeBody& body, int surface) +{ + _body_dampen_map[body.get_id()].dampen = 0.0f; +} + void OdeWorld:: init_surface_table(PN_uint8 num_surfaces) @@ -155,32 +164,16 @@ set_surface_entry( PN_uint8 pos1, PN_uint8 pos2, -sBodyParams OdeWorld:: -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; -} - void OdeWorld:: set_dampen_on_bodies(dBodyID id1, dBodyID id2,dReal damp) { - if(_body_surface_map[id1].dampen < damp) + if(_body_dampen_map[id1].dampen < damp) { - _body_surface_map[id1].dampen = damp; + _body_dampen_map[id1].dampen = damp; } - if(_body_surface_map[id2].dampen < damp) + if(_body_dampen_map[id2].dampen < damp) { - _body_surface_map[id2].dampen = damp; + _body_dampen_map[id2].dampen = damp; } } @@ -188,11 +181,13 @@ float OdeWorld:: apply_dampening(float dt, OdeBody& body) { dBodyID bodyId = body.get_id(); - dReal damp = _body_surface_map[bodyId].dampen; + dReal damp = _body_dampen_map[bodyId].dampen; float dampening = 1.00 - (damp * dt); body.set_angular_vel(body.get_angular_vel() * dampening); body.set_linear_vel(body.get_linear_vel() * dampening); - _body_surface_map[bodyId].dampen = 0.0; + _body_dampen_map[bodyId].dampen = 0.0; return dampening; } + + diff --git a/panda/src/ode/odeWorld.h b/panda/src/ode/odeWorld.h index b708c8ab2a..6aefc442b9 100755 --- a/panda/src/ode/odeWorld.h +++ b/panda/src/ode/odeWorld.h @@ -89,7 +89,8 @@ PUBLISHED: INLINE int compare_to(const OdeWorld &other) const; void init_surface_table(PN_uint8 num_surfaces); - void assign_surface_body(OdeBody& body, int surface); + //void assign_surface_body(OdeBody& body, int surface); + void add_body_dampening(OdeBody& body, int surface); void set_surface_entry(PN_uint8 pos1, PN_uint8 pos2, dReal mu, dReal bounce, @@ -99,7 +100,7 @@ PUBLISHED: dReal slip, dReal dampen); float apply_dampening(float dt, OdeBody& body); - + public: @@ -114,8 +115,9 @@ private: dWorldID _id; sSurfaceParams *_surface_table; PN_uint8 _num_surfaces; - typedef pmap BodySurfaceMap; - BodySurfaceMap _body_surface_map; + typedef pmap BodyDampenMap; + BodyDampenMap _body_dampen_map; +