added collide callback to OdeWorld

git-svn-id: svn://anubis/misc/OdeWorld@144 bd8a9e45-a331-0410-811e-c64571078777

OdeWorld.cc

@@ -1,6 +1,37 @@
 
 #include "OdeWorld.h"
 
+#define WORLD_STEP 0.001
+
+/* used by ODE to perform collision detection */
+void OdeWorld_collide_callback(void * data, dGeomID o1, dGeomID o2)
+{
+    const int maxNumContacts = 4;
+    OdeWorld * ow = (OdeWorld *) data;
+    static dContact contact[maxNumContacts];
+    dBodyID b1 = dGeomGetBody(o1);
+    dBodyID b2 = dGeomGetBody(o2);
+    if ((b1 == b2) || (b1 && b2 && dAreConnected(b1, b2)))
+        return;
+    int num = dCollide(o1, o2, maxNumContacts,
+            &contact[0].geom, sizeof(contact[0]));
+    for (int i = 0; i < num; i++)
+    {
+        contact[i].surface.mode =
+            dContactSlip1 | dContactSlip2 | dContactBounce |
+            dContactSoftERP | dContactSoftCFM | dContactApprox1;
+        contact[i].surface.mu = 0.5;
+        contact[i].surface.slip1 = 0.0;
+        contact[i].surface.slip2 = 0.0;
+        contact[i].surface.soft_erp = 0.8;
+        contact[i].surface.soft_cfm = 0.01;
+        contact[i].surface.bounce = 0.0;
+        dJointID joint = dJointCreateContact(ow->m_world,
+                ow->m_contactJointGroup, contact + i);
+        dJointAttach(joint, b1, b2);
+    }
+}
+
 OdeWorld::OdeWorld()
 {
     m_world = dWorldCreate();
@@ -15,3 +46,11 @@ OdeWorld::~OdeWorld()
     dSpaceDestroy(m_space);
     dWorldDestroy(m_world);
 }
+
+/* invokes ODE to do physics on our world */
+void OdeWorld::worldStep()
+{
+    dSpaceCollide(m_space, this, OdeWorld_collide_callback);
+    dWorldQuickStep(m_world, WORLD_STEP);
+    dJointGroupEmpty(m_contactJointGroup);
+}

OdeWorld.h

@@ -13,6 +13,10 @@ class OdeWorld
         {
             dWorldSetGravity(m_world, x, y, z);
         }
+        void worldStep();
+
+        friend void OdeWorld_collide_callback(void * data,
+                dGeomID o1, dGeomID o2);
 
     protected:
         dWorldID        m_world;