From dd849d202aa6163423dfc32bee2a351a66128d65 Mon Sep 17 00:00:00 2001
From: isucan <isucan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Fri, 6 Jan 2012 20:52:54 +0000
Subject: [PATCH] add collision between broad phase managers
git-svn-id: https://kforge.ros.org/fcl/fcl_ros@59 253336fb-580f-4252-a368-f3cef5a2a82b
---
trunk/fcl/include/fcl/broad_phase_collision.h | 3 +++
trunk/fcl/src/broad_phase_collision.cpp | 13 +++++++++++++
2 files changed, 16 insertions(+)
diff --git a/trunk/fcl/include/fcl/broad_phase_collision.h b/trunk/fcl/include/fcl/broad_phase_collision.h
index b99359cb..b301b609 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 d259f0a2..5888f55c 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);
--
GitLab