Added collide2 and space_to_geom

panda/src/ode/odeGeom.h

@@ -37,6 +37,7 @@ class OdeSimpleSpace;
 class OdeHashSpace;
 class OdeQuadTreeSpace;
 
+class OdeUtil;
 class OdeCollisionEntry;
 
 ////////////////////////////////////////////////////////////////////
@@ -46,6 +47,7 @@ class OdeCollisionEntry;
 class EXPCL_PANDAODE OdeGeom : public TypedObject {
   friend class OdeContactGeom;
   friend class OdeSpace;
+  friend class OdeUtil;
   friend class OdeCollisionEntry;
 
 protected:

panda/src/ode/odeUtil.cxx

@@ -14,7 +14,16 @@
 
 #include "odeUtil.h"
 
+#ifdef HAVE_PYTHON
+  #include "py_panda.h"
+  #include "typedReferenceCount.h"
+  #ifndef CPPPARSER
+    extern EXPCL_PANDAODE Dtool_PyTypedObject Dtool_OdeGeom;
+  #endif
+#endif
+
 dReal OdeUtil::OC_infinity = dInfinity;
+PyObject* OdeUtil::_python_callback = NULL;
 
 ////////////////////////////////////////////////////////////////////
 //     Function: OdeUtil::get_connecting_joint
@@ -102,3 +111,45 @@ collide(const OdeGeom &geom1, const OdeGeom &geom2, const short int max_contacts
   return contacts;
 }
 
+#ifdef HAVE_PYTHON
+////////////////////////////////////////////////////////////////////
+//     Function: OdeUtil::collide2
+//       Access: Public, Static
+//  Description: Calls the callback for all potentially intersecting
+//               pairs that contain one geom from geom1 and one geom
+//               from geom2.
+////////////////////////////////////////////////////////////////////
+int OdeUtil::
+collide2(const OdeGeom &geom1, const OdeGeom &geom2, PyObject* arg, PyObject* callback) {
+  nassertr(callback != NULL, -1);
+  if (!PyCallable_Check(callback)) {
+    PyErr_Format(PyExc_TypeError, "'%s' object is not callable", callback->ob_type->tp_name);
+    return -1;
+  } else {
+    _python_callback = (PyObject*) callback;
+    Py_XINCREF(_python_callback);
+    dSpaceCollide2(geom1.get_id(), geom2.get_id(), (void*) arg, &near_callback);
+    Py_XDECREF(_python_callback);
+    return 0;
+  }
+}
+
+void OdeUtil::
+near_callback(void *data, dGeomID o1, dGeomID o2) {
+  ode_cat.spam() << "near_callback called, data: " << data << ", dGeomID1: " << o1 << ", dGeomID2: " << o2 << "\n";
+  OdeGeom g1 (o1);
+  OdeGeom g2 (o2);
+  PyObject* p1 = DTool_CreatePyInstanceTyped(&g1, Dtool_OdeGeom, true, false, g1.get_type_index());
+  PyObject* p2 = DTool_CreatePyInstanceTyped(&g2, Dtool_OdeGeom, true, false, g2.get_type_index());
+  PyObject* result = PyEval_CallFunction(_python_callback, "OOO", (PyObject*) data, p1, p2);
+  if (!result) {
+    ode_cat.error() << "An error occurred while calling python function!\n";
+    PyErr_Print();
+  }
+}
+#endif
+
+OdeGeom OdeUtil::
+space_to_geom(const OdeSpace &space) {
+  return OdeGeom((dGeomID)space.get_id());
+}

panda/src/ode/odeUtil.h

@@ -23,8 +23,13 @@
 #include "odeJointCollection.h"
 #include "odeContactCollection.h"
 
+#ifdef HAVE_PYTHON
+  #include "py_panda.h"
+#endif
+
 class OdeBody;
 class OdeJoint;
+class OdeGeom;
 
 ////////////////////////////////////////////////////////////////////
 //       Class : OdeUtil
@@ -43,8 +48,11 @@ PUBLISHED:
                                      const int joint_type);
   static OdeContactCollection collide(const OdeGeom &geom1, const OdeGeom &geom2,
                                       const short int max_contacts = 150);
+  static int collide2(const OdeGeom &geom1, const OdeGeom &geom2,
+                                       PyObject* arg, PyObject* callback);
+  static OdeGeom space_to_geom(const OdeSpace &space);
 
-  static dReal OC_infinity;  
+  static dReal OC_infinity;
 
   // RAU we can't access OC_infinity as constants are not exposed in python
   static dReal get_infinity() {return OC_infinity;};
@@ -52,6 +60,12 @@ PUBLISHED:
   static int rand_get_seed() {return dRandGetSeed();};
 
   static void rand_set_seed(int s) {dRandSetSeed(s);};
+  
+private:
+#ifdef HAVE_PYTHON
+  static void near_callback(void*, dGeomID, dGeomID);
+  static PyObject* _python_callback;
+#endif
 };
 
 #endif