surface data stored and accessed through the space

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::

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);
   }
+      
+  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() 
+get_surface_type() 
 {
-  return OdeGeom::get_default_surface_type(this->get_id());
+  return get_space().get_surface_type(this->get_id());
 }
 
 int OdeGeom::
-get_default_surface_type(dGeomID id)
+get_collide_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;
+  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() {

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<dGeomID, int> GeomSurfaceMap;
-  static GeomSurfaceMap _geom_surface_map;
 
 public:
   static TypeHandle get_class_type() {

panda/src/ode/odeHelperStructs.h

@@ -9,7 +9,7 @@ struct sSurfaceParams
     
 struct sBodyParams
     {
-        int surfaceType;
+//        int surfaceType;
         dReal dampen;
     };
     

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;
+}
+

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<dGeomID, int> GeomSurfaceMap;
+  GeomSurfaceMap _geom_surface_map;
+
+  typedef pmap<dGeomID, int> GeomCollideIdMap;
+  GeomCollideIdMap _geom_collide_id_map;
+
 };
 
 #include "odeSpace.I"

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;
     
 }
+
+

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<dBodyID, sBodyParams> BodySurfaceMap;
-  BodySurfaceMap _body_surface_map;
+  typedef pmap<dBodyID, sBodyParams> BodyDampenMap;
+  BodyDampenMap _body_dampen_map;
+