From 856fcaa67d6b64fcf44229f000f25ab7e2ffdcf0 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 16 Mar 2017 09:00:11 -0400 Subject: [PATCH 1/3] Fix collision filtering for BulletCollisionDetector --- .../bullet/BulletCollisionDetector.cpp | 77 ++++++++++------ .../bullet/BulletCollisionDispatcher.cpp | 87 +++++++++++++++++++ .../bullet/BulletCollisionDispatcher.hpp | 69 +++++++++++++++ .../collision/bullet/BulletCollisionGroup.cpp | 4 +- dart/collision/bullet/CMakeLists.txt | 4 +- 5 files changed, 213 insertions(+), 28 deletions(-) create mode 100644 dart/collision/bullet/BulletCollisionDispatcher.cpp create mode 100644 dart/collision/bullet/BulletCollisionDispatcher.hpp diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index b48e9fed4bfa8..15e09721bdd38 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -42,6 +42,7 @@ #include "dart/collision/bullet/BulletTypes.hpp" #include "dart/collision/bullet/BulletCollisionObject.hpp" #include "dart/collision/bullet/BulletCollisionGroup.hpp" +#include "dart/collision/bullet/BulletCollisionDispatcher.hpp" #include "dart/dynamics/ShapeFrame.hpp" #include "dart/dynamics/Shape.hpp" #include "dart/dynamics/SphereShape.hpp" @@ -125,7 +126,6 @@ Contact convertContact(const btManifoldPoint& bulletManifoldPoint, const BulletCollisionObject::UserData* userData2); void convertContacts(btCollisionWorld* collWorld, - BulletOverlapFilterCallback* overlapFilterCallback, const CollisionOption& option, CollisionResult& result); @@ -218,6 +218,42 @@ static bool isCollision(btCollisionWorld* world) return false; } +//============================================================================== +void filterOutCollisions( + btCollisionWorld* collWorld, const std::shared_ptr& filter) +{ + assert(collWorld); + + auto dispatcher = static_cast( + collWorld->getDispatcher()); + assert(dispatcher); + + auto numManifolds = dispatcher->getNumManifolds(); + + std::vector manifoldsToRelease; + + for (auto i = 0; i < numManifolds; ++i) + { + auto contactManifold = dispatcher->getManifoldByIndexInternal(i); + const auto bulletCollObj0 = contactManifold->getBody0(); + const auto bulletCollObj1 = contactManifold->getBody1(); + + const auto userPointer0 = static_cast( + bulletCollObj0->getUserPointer()); + const auto userPointer1 = static_cast( + bulletCollObj1->getUserPointer()); + + const auto btCollObj0 = userPointer0->collisionObject; + const auto btCollObj1 = userPointer1->collisionObject; + + if (filter && !filter->needCollision(btCollObj0, btCollObj1)) + manifoldsToRelease.push_back(contactManifold); + } + + for (const auto& manifold : manifoldsToRelease) + dispatcher->clearManifold(manifold); +} + //============================================================================== bool BulletCollisionDetector::collide( CollisionGroup* group, @@ -233,37 +269,26 @@ bool BulletCollisionDetector::collide( if (!checkGroupValidity(this, group)) return false; - // TODO(JS): It seems, when new BulletOverlapFilterCallback is set to - // btCollisionWorld, bullet doesn't update the list of collision pairs that - // was generated before. Also, there is no way to update the list manually. - // Please report us if it's not true. - // - // In order to have filtered pairs in btCollisionWorld, we instead create a - // new btCollisionWorld (by creating new BulletCollisionGroup) and add the - // collision objects to the new btCollisionWorld so that the filter prevents - // btCollisionWorld to generate unnecessary pairs, which is very inefficient - // way. + auto castedGroup = static_cast(group); + auto collisionWorld = castedGroup->getBulletCollisionWorld(); + auto dispatcher = static_cast( + collisionWorld->getDispatcher()); + dispatcher->setFilter(option.collisionFilter); - mGroupForFiltering.reset(new BulletCollisionGroup(shared_from_this())); - auto bulletCollisionWorld = mGroupForFiltering->getBulletCollisionWorld(); - auto bulletPairCache = bulletCollisionWorld->getPairCache(); - auto filterCallback = new BulletOverlapFilterCallback(option, result); - bulletPairCache->setOverlapFilterCallback(filterCallback); + filterOutCollisions(collisionWorld, option.collisionFilter); - mGroupForFiltering->addShapeFramesOf(group); - mGroupForFiltering->updateEngineData(); - - bulletCollisionWorld->performDiscreteCollisionDetection(); + castedGroup->updateEngineData(); + collisionWorld->performDiscreteCollisionDetection(); if (result) { - convertContacts(bulletCollisionWorld, filterCallback, option, *result); + convertContacts(collisionWorld, option, *result); return result->isCollision(); } else { - return isCollision(bulletCollisionWorld); + return isCollision(collisionWorld); } } @@ -304,7 +329,7 @@ bool BulletCollisionDetector::collide( if (result) { - convertContacts(bulletCollisionWorld, filterCallback, option, *result); + convertContacts(bulletCollisionWorld, option, *result); return result->isCollision(); } @@ -587,13 +612,13 @@ Contact convertContact(const btManifoldPoint& bulletManifoldPoint, //============================================================================== void convertContacts( btCollisionWorld* collWorld, - BulletOverlapFilterCallback* overlapFilterCallback, const CollisionOption& option, CollisionResult& result) { assert(collWorld); - auto dispatcher = collWorld->getDispatcher(); + auto dispatcher = static_cast( + collWorld->getDispatcher()); assert(dispatcher); auto numManifolds = dispatcher->getNumManifolds(); @@ -622,7 +647,7 @@ void convertContacts( if (result.getNumContacts() >= option.maxNumContacts) { - overlapFilterCallback->done = true; + dispatcher->setDone(true); return; } } diff --git a/dart/collision/bullet/BulletCollisionDispatcher.cpp b/dart/collision/bullet/BulletCollisionDispatcher.cpp new file mode 100644 index 0000000000000..cef77575b9f08 --- /dev/null +++ b/dart/collision/bullet/BulletCollisionDispatcher.cpp @@ -0,0 +1,87 @@ +/* + * Copyright (c) 2017, Graphics Lab, Georgia Tech Research Corporation + * Copyright (c) 2017, Personal Robotics Lab, Carnegie Mellon University + * All rights reserved. + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/collision/bullet/BulletCollisionDispatcher.hpp" + +#include "dart/collision/bullet/BulletCollisionObject.hpp" + +namespace dart { +namespace collision { + +//============================================================================== +BulletCustomCollisionDispatcher::BulletCustomCollisionDispatcher( + btCollisionConfiguration* collisionConfiguration) + : btCollisionDispatcher(collisionConfiguration), + mDone(false), + mFilter(nullptr) +{ + // Do nothing +} + +//============================================================================== +void BulletCustomCollisionDispatcher::setDone(bool done) +{ + mDone = done; +} + +//============================================================================== +void BulletCustomCollisionDispatcher::setFilter( + const std::shared_ptr& filter) +{ + mFilter = filter; +} + +//============================================================================== +std::shared_ptr +BulletCustomCollisionDispatcher::getFilter() const +{ + return mFilter; +} + +//============================================================================== +bool BulletCustomCollisionDispatcher::needsCollision( + const btCollisionObject* body0, const btCollisionObject* body1) +{ + const auto userData0 = static_cast( + body0->getUserPointer()); + const auto userData1 = static_cast( + body1->getUserPointer()); + + if (mFilter && !mFilter->needCollision(userData0->collisionObject, + userData1->collisionObject)) + { + return false; + } + + return btCollisionDispatcher::needsCollision(body0, body1); +} + +} // namespace collision +} // namespace dart diff --git a/dart/collision/bullet/BulletCollisionDispatcher.hpp b/dart/collision/bullet/BulletCollisionDispatcher.hpp new file mode 100644 index 0000000000000..32615d5735028 --- /dev/null +++ b/dart/collision/bullet/BulletCollisionDispatcher.hpp @@ -0,0 +1,69 @@ +/* + * Copyright (c) 2017, Graphics Lab, Georgia Tech Research Corporation + * Copyright (c) 2017, Personal Robotics Lab, Carnegie Mellon University + * All rights reserved. + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COLLISION_BULLET_BULLETCOLLISIONDISPATCHER_HPP_ +#define DART_COLLISION_BULLET_BULLETCOLLISIONDISPATCHER_HPP_ + +// Must be included before any Bullet headers. +#include "dart/config.hpp" + +#include + +#include "dart/collision/CollisionFilter.hpp" +#include "dart/collision/CollisionObject.hpp" + +namespace dart { +namespace collision { + +class BulletCustomCollisionDispatcher : public btCollisionDispatcher +{ +public: + explicit BulletCustomCollisionDispatcher( + btCollisionConfiguration* collisionConfiguration); + + void setDone(bool done); + + void setFilter(const std::shared_ptr& filter); + + std::shared_ptr getFilter() const; + + bool needsCollision( + const btCollisionObject* body0,const btCollisionObject* body1) override; + +protected: + bool mDone; + + std::shared_ptr mFilter; +}; + +} // namespace collision +} // namespace dart + +#endif // DART_COLLISION_BULLET_BULLETCOLLISIONDISPATCHER_HPP_ diff --git a/dart/collision/bullet/BulletCollisionGroup.cpp b/dart/collision/bullet/BulletCollisionGroup.cpp index 2ebe3d5aac0b8..0ff2844f4e8c8 100644 --- a/dart/collision/bullet/BulletCollisionGroup.cpp +++ b/dart/collision/bullet/BulletCollisionGroup.cpp @@ -33,6 +33,7 @@ #include "dart/collision/CollisionObject.hpp" #include "dart/collision/bullet/BulletCollisionObject.hpp" +#include "dart/collision/bullet/BulletCollisionDispatcher.hpp" namespace dart { namespace collision { @@ -44,13 +45,14 @@ BulletCollisionGroup::BulletCollisionGroup( mBulletProadphaseAlg(new btDbvtBroadphase()), mBulletCollisionConfiguration(new btDefaultCollisionConfiguration()), mBulletDispatcher( - new btCollisionDispatcher(mBulletCollisionConfiguration.get())), + new BulletCustomCollisionDispatcher(mBulletCollisionConfiguration.get())), mBulletCollisionWorld( new btCollisionWorld(mBulletDispatcher.get(), mBulletProadphaseAlg.get(), mBulletCollisionConfiguration.get())) { // Do nothing + mBulletCollisionWorld->getPairCache(); } //============================================================================== diff --git a/dart/collision/bullet/CMakeLists.txt b/dart/collision/bullet/CMakeLists.txt index 61d972dbb91d8..a9a8cc0f29036 100644 --- a/dart/collision/bullet/CMakeLists.txt +++ b/dart/collision/bullet/CMakeLists.txt @@ -1,13 +1,15 @@ # Search all header and source files file(GLOB hdrs "*.hpp") file(GLOB srcs "*.cpp") +file(GLOB detail_hdrs "detail/*.hpp") +file(GLOB detail_srcs "detail/*.cpp") # Set local target name set(target_name ${PROJECT_NAME}-collision-bullet) set(component_name collision-bullet) # Add target -dart_add_library(${target_name} ${hdrs} ${srcs}) +dart_add_library(${target_name} ${hdrs} ${srcs} ${detail_hdrs} ${detail_srcs}) target_include_directories( ${target_name} SYSTEM PUBLIC ${BULLET_INCLUDE_DIRS} From 466dd0ec93b15f2693e08c73a46f1420ec09f24c Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 22 Mar 2017 15:42:17 -0400 Subject: [PATCH 2/3] Move internal API into detail namespace --- .../bullet/BulletCollisionDetector.cpp | 154 ++++++------------ .../collision/bullet/BulletCollisionGroup.cpp | 5 +- dart/collision/bullet/CMakeLists.txt | 2 +- .../BulletCollisionDispatcher.cpp | 23 ++- .../BulletCollisionDispatcher.hpp | 17 +- .../detail/BulletOverlapFilterCallback.cpp | 87 ++++++++++ .../detail/BulletOverlapFilterCallback.hpp | 70 ++++++++ 7 files changed, 236 insertions(+), 122 deletions(-) rename dart/collision/bullet/{ => detail}/BulletCollisionDispatcher.cpp (85%) rename dart/collision/bullet/{ => detail}/BulletCollisionDispatcher.hpp (82%) create mode 100644 dart/collision/bullet/detail/BulletOverlapFilterCallback.cpp create mode 100644 dart/collision/bullet/detail/BulletOverlapFilterCallback.hpp diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index 15e09721bdd38..c2d0f35df4e50 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -42,7 +42,8 @@ #include "dart/collision/bullet/BulletTypes.hpp" #include "dart/collision/bullet/BulletCollisionObject.hpp" #include "dart/collision/bullet/BulletCollisionGroup.hpp" -#include "dart/collision/bullet/BulletCollisionDispatcher.hpp" +#include "dart/collision/bullet/detail/BulletCollisionDispatcher.hpp" +#include "dart/collision/bullet/detail/BulletOverlapFilterCallback.hpp" #include "dart/dynamics/ShapeFrame.hpp" #include "dart/dynamics/Shape.hpp" #include "dart/dynamics/SphereShape.hpp" @@ -61,71 +62,11 @@ namespace collision { namespace { -struct BulletOverlapFilterCallback : public btOverlapFilterCallback -{ - BulletOverlapFilterCallback( - const CollisionOption& option, - CollisionResult* result) - : option(option), - result(result), - foundCollision(false), - done(false) - { - // Do nothing - } - - // return true when pairs need collision - bool needBroadphaseCollision( - btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const override - { - if (done) - return false; - - assert((proxy0 != nullptr && proxy1 != nullptr) && - "Bullet broadphase overlapping pair proxies are nullptr"); - - bool collide = (proxy0->m_collisionFilterGroup & - proxy1->m_collisionFilterMask) != 0; - collide = collide && (proxy1->m_collisionFilterGroup & - proxy0->m_collisionFilterMask); - - const auto& filter = option.collisionFilter; - - if (collide && filter) - { - auto bulletCollObj0 - = static_cast(proxy0->m_clientObject); - auto bulletCollObj1 - = static_cast(proxy1->m_clientObject); - - auto userData0 = static_cast( - bulletCollObj0->getUserPointer()); - auto userData1 = static_cast( - bulletCollObj1->getUserPointer()); - - collide = filter->needCollision(userData0->collisionObject, - userData1->collisionObject); - } - - return collide; - } - - const CollisionOption& option; - const CollisionResult* result; - - /// True if at least one contact is found. This flag is used only when - /// mResult is nullptr; otherwise the actual collision result is in mResult. - bool foundCollision; - - /// Whether the collision iteration can stop - mutable bool done; -}; - Contact convertContact(const btManifoldPoint& bulletManifoldPoint, const BulletCollisionObject::UserData* userData1, const BulletCollisionObject::UserData* userData2); -void convertContacts(btCollisionWorld* collWorld, +void reportContacts(btCollisionWorld* collWorld, const CollisionOption& option, CollisionResult& result); @@ -139,8 +80,6 @@ btCollisionShape* createBulletCollisionShapeFromAssimpMesh(const aiMesh* mesh); } // anonymous namespace - - //============================================================================== std::shared_ptr BulletCollisionDetector::create() { @@ -219,34 +158,41 @@ static bool isCollision(btCollisionWorld* world) } //============================================================================== -void filterOutCollisions( - btCollisionWorld* collWorld, const std::shared_ptr& filter) +void filterOutCollisions(btCollisionWorld* world) { - assert(collWorld); + assert(world); - auto dispatcher = static_cast( - collWorld->getDispatcher()); + auto dispatcher = static_cast( + world->getDispatcher()); assert(dispatcher); - auto numManifolds = dispatcher->getNumManifolds(); + const auto filter = dispatcher->getFilter(); + if (!filter) + return; + + const auto numManifolds = dispatcher->getNumManifolds(); std::vector manifoldsToRelease; for (auto i = 0; i < numManifolds; ++i) { - auto contactManifold = dispatcher->getManifoldByIndexInternal(i); - const auto bulletCollObj0 = contactManifold->getBody0(); - const auto bulletCollObj1 = contactManifold->getBody1(); + const auto contactManifold = dispatcher->getManifoldByIndexInternal(i); - const auto userPointer0 = static_cast( - bulletCollObj0->getUserPointer()); - const auto userPointer1 = static_cast( - bulletCollObj1->getUserPointer()); + const auto body0 = contactManifold->getBody0(); + const auto body1 = contactManifold->getBody1(); - const auto btCollObj0 = userPointer0->collisionObject; - const auto btCollObj1 = userPointer1->collisionObject; + const auto userPointer0 = body0->getUserPointer(); + const auto userPointer1 = body1->getUserPointer(); - if (filter && !filter->needCollision(btCollObj0, btCollObj1)) + const auto userData0 + = static_cast(userPointer0); + const auto userData1 + = static_cast(userPointer1); + + const auto btCollObj0 = userData0->collisionObject; + const auto btCollObj1 = userData1->collisionObject; + + if (filter->needCollision(btCollObj0, btCollObj1)) manifoldsToRelease.push_back(contactManifold); } @@ -266,23 +212,26 @@ bool BulletCollisionDetector::collide( if (0u == option.maxNumContacts) return false; + // Check if 'this' is the collision engine of 'group'. if (!checkGroupValidity(this, group)) return false; auto castedGroup = static_cast(group); auto collisionWorld = castedGroup->getBulletCollisionWorld(); - auto dispatcher = static_cast( + + auto dispatcher = static_cast( collisionWorld->getDispatcher()); dispatcher->setFilter(option.collisionFilter); - filterOutCollisions(collisionWorld, option.collisionFilter); + // Filter out persistent contact pairs already existing in the world + filterOutCollisions(collisionWorld); castedGroup->updateEngineData(); collisionWorld->performDiscreteCollisionDetection(); if (result) { - convertContacts(collisionWorld, option, *result); + reportContacts(collisionWorld, option, *result); return result->isCollision(); } @@ -319,7 +268,7 @@ bool BulletCollisionDetector::collide( mGroupForFiltering.reset(new BulletCollisionGroup(shared_from_this())); auto bulletCollisionWorld = mGroupForFiltering->getBulletCollisionWorld(); auto bulletPairCache = bulletCollisionWorld->getPairCache(); - auto filterCallback = new BulletOverlapFilterCallback(option, result); + auto filterCallback = new detail::BulletOverlapFilterCallback(option.collisionFilter); bulletPairCache->setOverlapFilterCallback(filterCallback); mGroupForFiltering->addShapeFramesOf(group1, group2); @@ -329,7 +278,7 @@ bool BulletCollisionDetector::collide( if (result) { - convertContacts(bulletCollisionWorld, option, *result); + reportContacts(bulletCollisionWorld, option, *result); return result->isCollision(); } @@ -610,41 +559,42 @@ Contact convertContact(const btManifoldPoint& bulletManifoldPoint, } //============================================================================== -void convertContacts( - btCollisionWorld* collWorld, +void reportContacts( + btCollisionWorld* world, const CollisionOption& option, CollisionResult& result) { - assert(collWorld); + assert(world); - auto dispatcher = static_cast( - collWorld->getDispatcher()); + auto dispatcher = static_cast( + world->getDispatcher()); assert(dispatcher); - auto numManifolds = dispatcher->getNumManifolds(); + const auto numManifolds = dispatcher->getNumManifolds(); for (auto i = 0; i < numManifolds; ++i) { - auto contactManifold = dispatcher->getManifoldByIndexInternal(i); - const auto bulletCollObj0 = contactManifold->getBody0(); - const auto bulletCollObj1 = contactManifold->getBody1(); + const auto contactManifold = dispatcher->getManifoldByIndexInternal(i); + + const auto body0 = contactManifold->getBody0(); + const auto body1 = contactManifold->getBody1(); - auto userPointer0 = bulletCollObj0->getUserPointer(); - auto userPointer1 = bulletCollObj1->getUserPointer(); + const auto userPointer0 = body0->getUserPointer(); + const auto userPointer1 = body1->getUserPointer(); - auto userDataA + const auto userData0 = static_cast(userPointer0); - auto userDataB + const auto userData1 = static_cast(userPointer1); - auto numContacts = contactManifold->getNumContacts(); + const auto numContacts = contactManifold->getNumContacts(); for (auto j = 0; j < numContacts; ++j) { - auto& cp = contactManifold->getContactPoint(j); - - result.addContact(convertContact(cp, userDataA, userDataB)); + const auto& cp = contactManifold->getContactPoint(j); + result.addContact(convertContact(cp, userData0, userData1)); + // No need to check further collisions if (result.getNumContacts() >= option.maxNumContacts) { dispatcher->setDone(true); diff --git a/dart/collision/bullet/BulletCollisionGroup.cpp b/dart/collision/bullet/BulletCollisionGroup.cpp index 0ff2844f4e8c8..4ae66d85ab016 100644 --- a/dart/collision/bullet/BulletCollisionGroup.cpp +++ b/dart/collision/bullet/BulletCollisionGroup.cpp @@ -33,7 +33,7 @@ #include "dart/collision/CollisionObject.hpp" #include "dart/collision/bullet/BulletCollisionObject.hpp" -#include "dart/collision/bullet/BulletCollisionDispatcher.hpp" +#include "dart/collision/bullet/detail/BulletCollisionDispatcher.hpp" namespace dart { namespace collision { @@ -45,14 +45,13 @@ BulletCollisionGroup::BulletCollisionGroup( mBulletProadphaseAlg(new btDbvtBroadphase()), mBulletCollisionConfiguration(new btDefaultCollisionConfiguration()), mBulletDispatcher( - new BulletCustomCollisionDispatcher(mBulletCollisionConfiguration.get())), + new detail::BulletCollisionDispatcher(mBulletCollisionConfiguration.get())), mBulletCollisionWorld( new btCollisionWorld(mBulletDispatcher.get(), mBulletProadphaseAlg.get(), mBulletCollisionConfiguration.get())) { // Do nothing - mBulletCollisionWorld->getPairCache(); } //============================================================================== diff --git a/dart/collision/bullet/CMakeLists.txt b/dart/collision/bullet/CMakeLists.txt index a9a8cc0f29036..de12237e687ce 100644 --- a/dart/collision/bullet/CMakeLists.txt +++ b/dart/collision/bullet/CMakeLists.txt @@ -26,7 +26,7 @@ add_component_targets(${PROJECT_NAME} ${component_name} ${target_name}) add_component_dependencies(${PROJECT_NAME} ${component_name} dart) # Coverage test -dart_coveralls_add(${hdrs} ${srcs}) +dart_coveralls_add(${hdrs} ${srcs} ${detail_hdrs} ${detail_srcs}) # Generate header for this namespace dart_get_filename_components(header_names "collision_bullet headers" ${hdrs}) diff --git a/dart/collision/bullet/BulletCollisionDispatcher.cpp b/dart/collision/bullet/detail/BulletCollisionDispatcher.cpp similarity index 85% rename from dart/collision/bullet/BulletCollisionDispatcher.cpp rename to dart/collision/bullet/detail/BulletCollisionDispatcher.cpp index cef77575b9f08..dc3938f7d1663 100644 --- a/dart/collision/bullet/BulletCollisionDispatcher.cpp +++ b/dart/collision/bullet/detail/BulletCollisionDispatcher.cpp @@ -28,17 +28,18 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "dart/collision/bullet/BulletCollisionDispatcher.hpp" +#include "dart/collision/bullet/detail/BulletCollisionDispatcher.hpp" #include "dart/collision/bullet/BulletCollisionObject.hpp" namespace dart { namespace collision { +namespace detail { //============================================================================== -BulletCustomCollisionDispatcher::BulletCustomCollisionDispatcher( - btCollisionConfiguration* collisionConfiguration) - : btCollisionDispatcher(collisionConfiguration), +BulletCollisionDispatcher::BulletCollisionDispatcher( + btCollisionConfiguration* config) + : btCollisionDispatcher(config), mDone(false), mFilter(nullptr) { @@ -46,29 +47,32 @@ BulletCustomCollisionDispatcher::BulletCustomCollisionDispatcher( } //============================================================================== -void BulletCustomCollisionDispatcher::setDone(bool done) +void BulletCollisionDispatcher::setDone(bool done) { mDone = done; } //============================================================================== -void BulletCustomCollisionDispatcher::setFilter( +void BulletCollisionDispatcher::setFilter( const std::shared_ptr& filter) { mFilter = filter; } //============================================================================== -std::shared_ptr -BulletCustomCollisionDispatcher::getFilter() const +auto BulletCollisionDispatcher::getFilter() const +-> std::shared_ptr { return mFilter; } //============================================================================== -bool BulletCustomCollisionDispatcher::needsCollision( +bool BulletCollisionDispatcher::needsCollision( const btCollisionObject* body0, const btCollisionObject* body1) { + if (mDone) + return false; + const auto userData0 = static_cast( body0->getUserPointer()); const auto userData1 = static_cast( @@ -83,5 +87,6 @@ bool BulletCustomCollisionDispatcher::needsCollision( return btCollisionDispatcher::needsCollision(body0, body1); } +} // namespace detail } // namespace collision } // namespace dart diff --git a/dart/collision/bullet/BulletCollisionDispatcher.hpp b/dart/collision/bullet/detail/BulletCollisionDispatcher.hpp similarity index 82% rename from dart/collision/bullet/BulletCollisionDispatcher.hpp rename to dart/collision/bullet/detail/BulletCollisionDispatcher.hpp index 32615d5735028..63655d6f390b6 100644 --- a/dart/collision/bullet/BulletCollisionDispatcher.hpp +++ b/dart/collision/bullet/detail/BulletCollisionDispatcher.hpp @@ -28,8 +28,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COLLISION_BULLET_BULLETCOLLISIONDISPATCHER_HPP_ -#define DART_COLLISION_BULLET_BULLETCOLLISIONDISPATCHER_HPP_ +#ifndef DART_COLLISION_BULLET_DETAIL_BULLETCOLLISIONDISPATCHER_HPP_ +#define DART_COLLISION_BULLET_DETAIL_BULLETCOLLISIONDISPATCHER_HPP_ // Must be included before any Bullet headers. #include "dart/config.hpp" @@ -41,18 +41,20 @@ namespace dart { namespace collision { +namespace detail { -class BulletCustomCollisionDispatcher : public btCollisionDispatcher +class BulletCollisionDispatcher : public btCollisionDispatcher { public: - explicit BulletCustomCollisionDispatcher( - btCollisionConfiguration* collisionConfiguration); + explicit BulletCollisionDispatcher(btCollisionConfiguration* config); void setDone(bool done); + bool getDone() const; + void setFilter(const std::shared_ptr& filter); - std::shared_ptr getFilter() const; + auto getFilter() const -> std::shared_ptr; bool needsCollision( const btCollisionObject* body0,const btCollisionObject* body1) override; @@ -63,7 +65,8 @@ class BulletCustomCollisionDispatcher : public btCollisionDispatcher std::shared_ptr mFilter; }; +} // namespace detail } // namespace collision } // namespace dart -#endif // DART_COLLISION_BULLET_BULLETCOLLISIONDISPATCHER_HPP_ +#endif // DART_COLLISION_BULLET_DETAIL_BULLETCOLLISIONDISPATCHER_HPP_ diff --git a/dart/collision/bullet/detail/BulletOverlapFilterCallback.cpp b/dart/collision/bullet/detail/BulletOverlapFilterCallback.cpp new file mode 100644 index 0000000000000..425deb3259551 --- /dev/null +++ b/dart/collision/bullet/detail/BulletOverlapFilterCallback.cpp @@ -0,0 +1,87 @@ +/* + * Copyright (c) 2017, Graphics Lab, Georgia Tech Research Corporation + * Copyright (c) 2017, Personal Robotics Lab, Carnegie Mellon University + * All rights reserved. + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/collision/bullet/detail/BulletOverlapFilterCallback.hpp" + +#include "dart/collision/CollisionFilter.hpp" +#include "dart/collision/bullet/BulletCollisionObject.hpp" + +namespace dart { +namespace collision { +namespace detail { + +//============================================================================== +BulletOverlapFilterCallback::BulletOverlapFilterCallback( + const std::shared_ptr& filter) + : foundCollision(false), + done(false), + filter(filter) +{ + // Do nothing +} + +//============================================================================== +bool BulletOverlapFilterCallback::needBroadphaseCollision( + btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const +{ + if (done) + return false; + + assert((proxy0 != nullptr && proxy1 != nullptr) && + "Bullet broadphase overlapping pair proxies are nullptr"); + + const bool collide1 + = proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask; + const bool collide2 + = proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask; + + bool collide = collide1 & collide2; + + if (collide && filter) + { + auto object0 = static_cast(proxy0->m_clientObject); + auto object1 = static_cast(proxy1->m_clientObject); + + auto userPtr0 = object0->getUserPointer(); + auto userPtr1 = object1->getUserPointer(); + + auto userData0 = static_cast(userPtr0); + auto userData1 = static_cast(userPtr1); + + return filter->needCollision(userData0->collisionObject, + userData1->collisionObject); + } + + return collide; +} + +} // namespace detail +} // namespace collision +} // namespace dart diff --git a/dart/collision/bullet/detail/BulletOverlapFilterCallback.hpp b/dart/collision/bullet/detail/BulletOverlapFilterCallback.hpp new file mode 100644 index 0000000000000..2e5f95bffc5c2 --- /dev/null +++ b/dart/collision/bullet/detail/BulletOverlapFilterCallback.hpp @@ -0,0 +1,70 @@ +/* + * Copyright (c) 2017, Graphics Lab, Georgia Tech Research Corporation + * Copyright (c) 2017, Personal Robotics Lab, Carnegie Mellon University + * All rights reserved. + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_COLLISION_BULLET_DETAIL_BULLETOVERLAPFILTERCALLBACK_HPP_ +#define DART_COLLISION_BULLET_DETAIL_BULLETOVERLAPFILTERCALLBACK_HPP_ + +// Must be included before any Bullet headers. +#include "dart/config.hpp" + +#include + +#include "dart/collision/CollisionOption.hpp" +#include "dart/collision/CollisionResult.hpp" + +namespace dart { +namespace collision { +namespace detail { + +struct BulletOverlapFilterCallback : public btOverlapFilterCallback +{ + // Constructor + explicit BulletOverlapFilterCallback( + const std::shared_ptr& filter = nullptr); + + /// Returns true when pairs need collision + bool needBroadphaseCollision( + btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const override; + + /// True if at least one contact is found. This flag is used only when + /// mResult is nullptr; otherwise the actual collision result is in mResult. + bool foundCollision; + + /// Whether the collision iteration can stop + mutable bool done; + + std::shared_ptr filter; +}; + +} // namespace detail +} // namespace collision +} // namespace dart + +#endif // DART_COLLISION_BULLET_DETAIL_BULLETOVERLAPFILTERCALLBACK_HPP_ From 2a6aae77fab942709a963aa04ccbed8a705a7937 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 22 Mar 2017 15:50:11 -0400 Subject: [PATCH 3/3] Fix collision filtering logic --- dart/collision/bullet/BulletCollisionDetector.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index c2d0f35df4e50..63ac12f042c5f 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -192,7 +192,7 @@ void filterOutCollisions(btCollisionWorld* world) const auto btCollObj0 = userData0->collisionObject; const auto btCollObj1 = userData1->collisionObject; - if (filter->needCollision(btCollObj0, btCollObj1)) + if (!filter->needCollision(btCollObj0, btCollObj1)) manifoldsToRelease.push_back(contactManifold); }