diff --git a/trunk/fcl/include/fcl/broad_phase_collision.h b/trunk/fcl/include/fcl/broad_phase_collision.h index b99359cbc78ca0cd99dc0c5f9c18881456bba6b8..b301b60977d393ba3a9d0e5cd2bbf33ec76e84dc 100644 --- a/trunk/fcl/include/fcl/broad_phase_collision.h +++ b/trunk/fcl/include/fcl/broad_phase_collision.h @@ -84,6 +84,9 @@ public: /** \brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) */ virtual void collide(void* cdata, CollisionCallBack callback) const = 0; + /** \brief perform collision test for all pairs of objects belonging to the \e other manager */ + virtual void collide(const BroadPhaseCollisionManager *other, void* cdata, CollisionCallBack callback) const; + /** \brief whether the manager is empty */ virtual bool empty() const = 0; diff --git a/trunk/fcl/src/broad_phase_collision.cpp b/trunk/fcl/src/broad_phase_collision.cpp index d259f0a2cfd8db99fd4832efa3586714ae8da961..5888f55c6ca5ec524700e09a72eadacf7de3643d 100644 --- a/trunk/fcl/src/broad_phase_collision.cpp +++ b/trunk/fcl/src/broad_phase_collision.cpp @@ -69,6 +69,19 @@ bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cd return cdata->done; } +void BroadPhaseCollisionManager::collide(const BroadPhaseCollisionManager *other, void* cdata, CollisionCallBack callback) const +{ + if (other->size() < size()) + other->collide(this, cdata, callback); + else + { + std::vector<CollisionObject*> objs; + getObjects(objs); + for (std::size_t i = 0 ; i < objs.size() ; ++i) + other->collide(objs[i], cdata, callback); + } +} + void NaiveCollisionManager::unregisterObject(CollisionObject* obj) { objs.remove(obj);