autoCollide support

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));

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() {

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 &copy) :
   _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;
+}

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<dBodyID, int> BodySurfaceMap;
+  BodySurfaceMap _body_surface_map;
 
 public:
   static TypeHandle get_class_type() {