diff --git a/panda/src/ode/Sources.pp b/panda/src/ode/Sources.pp index 78195b12ef..e46563d4d1 100755 --- a/panda/src/ode/Sources.pp +++ b/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 \ diff --git a/panda/src/ode/config_ode.cxx b/panda/src/ode/config_ode.cxx index f1c8203c0b..573acead45 100755 --- a/panda/src/ode/config_ode.cxx +++ b/panda/src/ode/config_ode.cxx @@ -1,4 +1,4 @@ -// Filename: config_effects.cxx +// Filename: config_ode.cxx // Created by: joswilso (27Dec06) // //////////////////////////////////////////////////////////////////// diff --git a/panda/src/ode/config_ode.h b/panda/src/ode/config_ode.h index 2e897c8ab1..71020e2bdc 100755 --- a/panda/src/ode/config_ode.h +++ b/panda/src/ode/config_ode.h @@ -1,4 +1,4 @@ -// Filename: config_effects.h +// Filename: config_ode.h // Created by: joswilso (27Dec06) // //////////////////////////////////////////////////////////////////// diff --git a/panda/src/ode/odeContact.h b/panda/src/ode/odeContact.h index 4bbce81795..c162380258 100755 --- a/panda/src/ode/odeContact.h +++ b/panda/src/ode/odeContact.h @@ -44,10 +44,11 @@ PUBLISHED: INLINE void set_fdir1(const LVecBase3f &fdir1); public: + void operator = (const OdeContact ©); + bool operator == (const OdeContact &other); const dContact* get_contact_ptr() const; private: - void operator = (const OdeContact ©); dContact _contact; public: diff --git a/panda/src/ode/odeContactCollection.I b/panda/src/ode/odeContactCollection.I new file mode 100644 index 0000000000..8fdcfc78db --- /dev/null +++ b/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(); +} + diff --git a/panda/src/ode/odeContactCollection.cxx b/panda/src/ode/odeContactCollection.cxx new file mode 100644 index 0000000000..eb15d836f3 --- /dev/null +++ b/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 ©) : + _contacts(copy._contacts) { +} + +void OdeContactCollection:: +operator = (const OdeContactCollection ©) { + _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); +} + diff --git a/panda/src/ode/odeContactCollection.h b/panda/src/ode/odeContactCollection.h new file mode 100644 index 0000000000..c47a6f4c77 --- /dev/null +++ b/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 ©); + void operator = (const OdeContactCollection ©); + 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 Contacts; + Contacts _contacts; +}; + +#include "odeContactCollection.I" +#endif + diff --git a/panda/src/ode/odeContactGeom.h b/panda/src/ode/odeContactGeom.h index 91c47c96d1..59adf69867 100755 --- a/panda/src/ode/odeContactGeom.h +++ b/panda/src/ode/odeContactGeom.h @@ -29,9 +29,6 @@ class EXPCL_PANDAODE OdeContactGeom : public TypedReferenceCount { friend class OdeContact; -protected: - OdeContactGeom(const dContactGeom ©); - PUBLISHED: OdeContactGeom(); OdeContactGeom(const OdeContactGeom ©); @@ -52,6 +49,7 @@ PUBLISHED: INLINE void set_g2(const OdeGeom &geom); public: + OdeContactGeom(const dContactGeom ©); const dContactGeom* get_contact_geom_ptr() const; private: diff --git a/panda/src/ode/odeGeom.I b/panda/src/ode/odeGeom.I index 1ab853ac27..75356857d9 100755 --- a/panda/src/ode/odeGeom.I +++ b/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)); diff --git a/panda/src/ode/odeGeom.h b/panda/src/ode/odeGeom.h index 8a752a429d..e8ed36937e 100755 --- a/panda/src/ode/odeGeom.h +++ b/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); diff --git a/panda/src/ode/odeSpace.h b/panda/src/ode/odeSpace.h index 56ee768fcb..5a6d2c2428 100755 --- a/panda/src/ode/odeSpace.h +++ b/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; diff --git a/panda/src/ode/odeUtil.cxx b/panda/src/ode/odeUtil.cxx index cfcda10ab7..9845b339bf 100755 --- a/panda/src/ode/odeUtil.cxx +++ b/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; +} + diff --git a/panda/src/ode/odeUtil.h b/panda/src/ode/odeUtil.h index b1b9d81b00..1bd8b83bed 100755 --- a/panda/src/ode/odeUtil.h +++ b/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 int joint_type); + const OdeBody &body2, + const int joint_type); + static OdeContactCollection collide(const OdeGeom &geom1, const OdeGeom &geom2, + const short int max_contacts = 150); static dReal OC_infinity; diff --git a/panda/src/ode/pode_composite1.cxx b/panda/src/ode/pode_composite1.cxx index e5fae93056..787241bd71 100755 --- a/panda/src/ode/pode_composite1.cxx +++ b/panda/src/ode/pode_composite1.cxx @@ -11,4 +11,5 @@ #include "odeSurfaceParameters.cxx" #include "odeContactGeom.cxx" #include "odeContact.cxx" +#include "odeContactCollection.cxx"