Generic ODE improvements, and added OdeUtil.collide()

panda/src/ode/Sources.pp

@@ -29,6 +29,7 @@
     odeContact.I odeContact.h \
     odeAMotorJoint.I odeAMotorJoint.h \
     odeBallJoint.I odeBallJoint.h \
+    odeContactCollection.I odeContactCollection.h \
     odeContactJoint.I odeContactJoint.h \
     odeFixedJoint.I odeFixedJoint.h \
     odeHingeJoint.I odeHingeJoint.h \
@@ -60,6 +61,7 @@
     odeSpace.cxx \
     odeGeom.cxx \
     odeSurfaceParameters.cxx \
+    odeContactCollection.cxx \
     odeContactGeom.cxx odeContact.cxx \
     odeAMotorJoint.cxx odeBallJoint.cxx \
     odeContactJoint.cxx odeFixedJoint.cxx \
@@ -87,6 +89,7 @@
     odeGeom.I odeGeom.h \
     odeSurfaceParameters.I odeSurfaceParameters.h \
     odeContactGeom.I odeContactGeom.h \
+    odeContactCollection.I odeContactCollection.h \
     odeContact.I odeContact.h \
     odeAMotorJoint.I odeAMotorJoint.h \
     odeBallJoint.I odeBallJoint.h \

panda/src/ode/config_ode.cxx

@@ -1,4 +1,4 @@
-// Filename: config_effects.cxx
+// Filename: config_ode.cxx
 // Created by:  joswilso (27Dec06)
 //
 ////////////////////////////////////////////////////////////////////

panda/src/ode/config_ode.h

@@ -1,4 +1,4 @@
-// Filename: config_effects.h
+// Filename: config_ode.h
 // Created by:  joswilso (27Dec06)
 //
 ////////////////////////////////////////////////////////////////////

panda/src/ode/odeContact.h

@@ -44,10 +44,11 @@ PUBLISHED:
   INLINE void set_fdir1(const LVecBase3f &fdir1);
 
 public:
+  void operator = (const OdeContact &copy);
+  bool operator == (const OdeContact &other);
   const dContact* get_contact_ptr() const;
 
 private:
-  void operator = (const OdeContact &copy);
   dContact _contact;
 
 public:

panda/src/ode/odeContactCollection.I

@@ -0,0 +1,88 @@
+// Filename: odeContactCollection.I
+// Created by:  pro-rsoft (17Dec08)
+//
+////////////////////////////////////////////////////////////////////
+//
+// PANDA 3D SOFTWARE
+// Copyright (c) Carnegie Mellon University.  All rights reserved.
+//
+// All use of this software is subject to the terms of the revised BSD
+// license.  You should have received a copy of this license along
+// with this source code in a file named "LICENSE."
+//
+////////////////////////////////////////////////////////////////////
+
+////////////////////////////////////////////////////////////////////
+//     Function: OdeContactCollection::Destructor
+//       Access: Published
+//  Description:
+////////////////////////////////////////////////////////////////////
+INLINE OdeContactCollection::
+~OdeContactCollection() {
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: OdeContactCollection::operator +=
+//       Access: Published
+//  Description: Appends the other list onto the end of this one.
+////////////////////////////////////////////////////////////////////
+INLINE void OdeContactCollection::
+operator += (const OdeContactCollection &other) {
+  add_contacts_from(other);
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: OdeContactCollection::operator +
+//       Access: Published
+//  Description: Returns a OdeContactCollection representing the
+//               concatenation of the two lists.
+////////////////////////////////////////////////////////////////////
+INLINE OdeContactCollection OdeContactCollection::
+operator + (const OdeContactCollection &other) const {
+  OdeContactCollection a(*this);
+  a += other;
+  return a;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: OdeContactCollection::clear
+//       Access: Published
+//  Description: Clears the contact collection.
+////////////////////////////////////////////////////////////////////
+INLINE void OdeContactCollection::
+clear() {
+  _contacts.clear();
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: OdeContactCollection::is_empty
+//       Access: Published
+//  Description: Returns true if the collection is empty.
+////////////////////////////////////////////////////////////////////
+INLINE bool OdeContactCollection::
+is_empty() const {
+  return _contacts.empty();
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: OdeContactCollection::get_num_contacts
+//       Access: Published
+//  Description: Returns the number of Contacts in the collection.
+//               This is the same thing as size().
+////////////////////////////////////////////////////////////////////
+INLINE int OdeContactCollection::
+get_num_contacts() const {
+  return _contacts.size();
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: OdeContactCollection::size
+//       Access: Published
+//  Description: Returns the number of Contacts in the collection.
+//               This is the same thing as get_num_contacts().
+////////////////////////////////////////////////////////////////////
+INLINE int OdeContactCollection::
+size() const {
+  return _contacts.size();
+}
+

panda/src/ode/odeContactCollection.cxx

@@ -0,0 +1,116 @@
+// Filename: odeContactCollection.cxx
+// Created by:  pro-rsoft (17Dec08)
+//
+////////////////////////////////////////////////////////////////////
+//
+// PANDA 3D SOFTWARE
+// Copyright (c) Carnegie Mellon University.  All rights reserved.
+//
+// All use of this software is subject to the terms of the revised BSD
+// license.  You should have received a copy of this license along
+// with this source code in a file named "LICENSE."
+//
+////////////////////////////////////////////////////////////////////
+
+#include "odeContactCollection.h"
+
+OdeContactCollection::
+OdeContactCollection() {
+}
+
+OdeContactCollection::
+OdeContactCollection(const OdeContactCollection &copy) :
+  _contacts(copy._contacts) {
+}
+
+void OdeContactCollection::
+operator = (const OdeContactCollection &copy) {
+  _contacts = copy._contacts;
+}
+
+void OdeContactCollection::
+add_contact(PT(OdeContact) contact) {
+  _contacts.push_back(contact);
+}
+
+bool OdeContactCollection::
+remove_contact(PT(OdeContact) contact) {
+  int contact_index = -1;
+  for (int i = 0; contact_index == -1 && i < (int)_contacts.size(); i++) {
+    if (_contacts[i] == contact) {
+      contact_index = i;
+    }
+  }
+
+  if (contact_index == -1) {
+    // The indicated contact was not a member of the collection.
+    return false;
+  }
+
+  _contacts.erase(_contacts.begin() + contact_index);
+  return true;
+}
+
+void OdeContactCollection::
+add_contacts_from(const OdeContactCollection &other) {
+  int other_num_contacts = other.get_num_contacts();
+  for (int i = 0; i < other_num_contacts; i++) {
+    add_contact(other.get_contact(i));
+  }
+}
+
+void OdeContactCollection::
+remove_contacts_from(const OdeContactCollection &other) {
+  Contacts new_contacts;
+  int num_contacts = get_num_contacts();
+  for (int i = 0; i < num_contacts; i++) {
+    PT(OdeContact) contact = get_contact(i);
+    if (!other.has_contact(contact)) {
+      new_contacts.push_back(contact);
+    }
+  }
+  _contacts = new_contacts;
+}
+
+void OdeContactCollection::
+remove_duplicate_contacts() {
+  Contacts new_contacts;
+
+  int num_contacts = get_num_contacts();
+  for (int i = 0; i < num_contacts; i++) {
+    PT(OdeContact) contact = get_contact(i);
+    bool duplicated = false;
+
+    for (int j = 0; j < i && !duplicated; j++) {
+      duplicated = (contact == get_contact(j));
+    }
+
+    if (!duplicated) {
+      new_contacts.push_back(contact);
+    }
+  }
+
+  _contacts = new_contacts;
+}
+
+bool OdeContactCollection::
+has_contact(PT(OdeContact) contact) const {
+  for (int i = 0; i < get_num_contacts(); i++) {
+    if (contact == get_contact(i)) {
+      return true;
+    }
+  }
+  return false;
+}
+
+PT(OdeContact) OdeContactCollection::
+get_contact(int index) const {
+  nassertr(index >= 0 && index < (int)_contacts.size(), NULL);
+  return _contacts[index];
+}
+
+PT(OdeContact) OdeContactCollection::
+operator [] (int index) const {
+  return get_contact(index);
+}
+

panda/src/ode/odeContactCollection.h

@@ -0,0 +1,57 @@
+// Filename: odeContactCollection.h
+// Created by:  pro-rsoft (17Dec08)
+//
+////////////////////////////////////////////////////////////////////
+//
+// PANDA 3D SOFTWARE
+// Copyright (c) Carnegie Mellon University.  All rights reserved.
+//
+// All use of this software is subject to the terms of the revised BSD
+// license.  You should have received a copy of this license along
+// with this source code in a file named "LICENSE."
+//
+////////////////////////////////////////////////////////////////////
+
+#ifndef ODECONTACTCOLLECTION_H
+#define ODECONTACTCOLLECTION_H
+
+#include "odeContact.h"
+
+////////////////////////////////////////////////////////////////////
+//       Class : OdeContactCollection
+// Description : This is a set of zero or more OdeContacts. It's
+//               handy for returning from functions that need to
+//               return multiple OdeContacts.
+////////////////////////////////////////////////////////////////////
+class EXPCL_PANDAODE OdeContactCollection {
+PUBLISHED:
+  OdeContactCollection();
+  OdeContactCollection(const OdeContactCollection &copy);
+  void operator = (const OdeContactCollection &copy);
+  INLINE ~OdeContactCollection();
+
+  void add_contact(PT(OdeContact) contact);
+  bool remove_contact(PT(OdeContact) contact);
+  void add_contacts_from(const OdeContactCollection &other);
+  void remove_contacts_from(const OdeContactCollection &other);
+  void remove_duplicate_contacts();
+  bool has_contact(PT(OdeContact) contact) const;
+  INLINE void clear();
+
+  INLINE bool is_empty() const;
+  INLINE int get_num_contacts() const;
+  PT(OdeContact) get_contact(int index) const;
+  MAKE_SEQ(get_contacts, get_num_contacts, get_contact);
+  PT(OdeContact) operator [] (int index) const;
+  INLINE int size() const;
+  INLINE void operator += (const OdeContactCollection &other);
+  INLINE OdeContactCollection operator + (const OdeContactCollection &other) const;
+
+private:  
+  typedef pvector<PT(OdeContact)> Contacts;
+  Contacts _contacts;
+};
+
+#include "odeContactCollection.I"
+#endif
+

panda/src/ode/odeContactGeom.h

@@ -29,9 +29,6 @@
 class EXPCL_PANDAODE OdeContactGeom : public TypedReferenceCount {
   friend class OdeContact;
 
-protected:
-  OdeContactGeom(const dContactGeom &copy);
-
 PUBLISHED:
   OdeContactGeom();
   OdeContactGeom(const OdeContactGeom &copy);
@@ -52,6 +49,7 @@ PUBLISHED:
   INLINE void set_g2(const OdeGeom &geom);
   
 public:
+  OdeContactGeom(const dContactGeom &copy);
   const dContactGeom* get_contact_geom_ptr() const;
 
 private:

panda/src/ode/odeGeom.I

@@ -17,6 +17,11 @@ get_id() const {
   return _id;
 }
 
+INLINE bool OdeGeom::
+has_body() const {
+  return (dGeomGetBody(_id) != NULL);
+}
+
 INLINE OdeBody OdeGeom::
 get_body() const {
   return OdeBody(dGeomGetBody(_id));

panda/src/ode/odeGeom.h

@@ -70,6 +70,7 @@ PUBLISHED:
 
   //INLINE void set_data(void* data);
   INLINE void set_body(OdeBody &body);
+  INLINE bool has_body() const;
   INLINE OdeBody get_body() const;
   INLINE void set_position(dReal x, dReal y, dReal z);
   INLINE void set_position(const LVecBase3f &pos);

panda/src/ode/odeSpace.h

@@ -36,8 +36,6 @@ class OdeTriMeshGeom;
 class OdeSimpleSpace;
 class OdeHashSpace;
 class OdeQuadTreeSpace;
-    
-
 
 ////////////////////////////////////////////////////////////////////
 //       Class : OdeSpace
@@ -46,7 +44,6 @@ class OdeQuadTreeSpace;
 class EXPCL_PANDAODE OdeSpace : public TypedObject {
   friend class OdeGeom;
   static const int MAX_CONTACTS;
-  
 
 protected:
   OdeSpace(dSpaceID id);
@@ -81,7 +78,6 @@ PUBLISHED:
   OdeGeom get_geom(int i); // Not INLINE because of forward declaration
   //static int get_surface_type(OdeSpace * self, dGeomID o1);
   
-
   INLINE OdeSpace get_space() const;
 
   virtual void write(ostream &out = cout, unsigned int indent=0) const;
@@ -91,10 +87,8 @@ PUBLISHED:
   OdeQuadTreeSpace convert_to_quad_tree_space() const;
   
   int auto_collide();
-  static void auto_callback(void*, dGeomID, dGeomID);
 #ifdef HAVE_PYTHON
   int collide(PyObject* arg, PyObject* near_callback);
-  static void near_callback(void*, dGeomID, dGeomID);
 #endif
   static double get_contact_data(int data_index);
   int get_contact_id(int data_index, int first = 0);
@@ -107,8 +101,12 @@ PUBLISHED:
   int get_collide_id(dGeomID o1);
   int get_collide_id(OdeGeom& geom);
 
-
+public:
-public: 
+  static void auto_callback(void*, dGeomID, dGeomID);
+#ifdef HAVE_PYTHON
+  static void near_callback(void*, dGeomID, dGeomID);
+#endif
+  
   INLINE dSpaceID get_id() const;
   static OdeWorld* _collide_world;
   static OdeSpace* _collide_space;
@@ -120,15 +118,12 @@ public:
 
   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:
   static TypeHandle get_class_type() {
     return _type_handle;

panda/src/ode/odeUtil.cxx

@@ -29,7 +29,8 @@ get_connecting_joint(const OdeBody &body1, const OdeBody &body2) {
 ////////////////////////////////////////////////////////////////////
 //     Function: OdeUtil::get_connecting_joint_list
 //       Access: Public, Static
-//  Description: Returns a list of joints connecting the bodies.
+//  Description: Returns a collection of joints connecting the
+//               specified bodies.
 ////////////////////////////////////////////////////////////////////
 OdeJointCollection OdeUtil::
 get_connecting_joint_list(const OdeBody &body1, const OdeBody &body2) {
@@ -79,3 +80,25 @@ are_connected_excluding(const OdeBody &body1,
         joint_type);
 }
 
+////////////////////////////////////////////////////////////////////
+//     Function: OdeUtil::collide
+//       Access: Public, Static
+//  Description: Given two geometry objects that potentially touch
+//               (geom1 and geom2), generate contact information
+//               for them. Returns a collection of OdeContacts.
+////////////////////////////////////////////////////////////////////
+OdeContactCollection OdeUtil::
+collide(const OdeGeom &geom1, const OdeGeom &geom2, const short int max_contacts) {
+  dContactGeom *contact_list = (dContactGeom *)PANDA_MALLOC_ARRAY(max_contacts * sizeof(dContactGeom));
+  int num_contacts = dCollide(geom1.get_id(), geom2.get_id(), max_contacts, contact_list, sizeof(contact_list));
+  OdeContactCollection contacts;
+  for (int i = 0; i < num_contacts; i++) {
+    PT(OdeContact) contact = new OdeContact();
+    contact->set_geom(OdeContactGeom(contact_list[i]));
+    contacts.add_contact(contact);
+  }
+  
+  PANDA_FREE_ARRAY(contact_list);
+  return contacts;
+}
+

panda/src/ode/odeUtil.h

@@ -21,6 +21,7 @@
 
 #include "ode_includes.h"
 #include "odeJointCollection.h"
+#include "odeContactCollection.h"
 
 class OdeBody;
 class OdeJoint;
@@ -32,14 +33,16 @@ class OdeJoint;
 class EXPCL_PANDAODE OdeUtil {
 PUBLISHED:
   static OdeJoint get_connecting_joint(const OdeBody &body1,
-				       const OdeBody &body2);
+                                       const OdeBody &body2);
   static OdeJointCollection get_connecting_joint_list(const OdeBody &body1,
-						      const OdeBody &body2);
+                                                      const OdeBody &body2);
   static int are_connected(const OdeBody &body1,
                            const OdeBody &body2);
   static int are_connected_excluding(const OdeBody &body1,
-	                             const OdeBody &body2,
+                                     const OdeBody &body2,
-	                             const int joint_type);
+                                     const int joint_type);
+  static OdeContactCollection collide(const OdeGeom &geom1, const OdeGeom &geom2,
+                                      const short int max_contacts = 150);
 
   static dReal OC_infinity;  
 

panda/src/ode/pode_composite1.cxx

@@ -11,4 +11,5 @@
 #include "odeSurfaceParameters.cxx"
 #include "odeContactGeom.cxx"
 #include "odeContact.cxx"
+#include "odeContactCollection.cxx"