From ef60ea4de8ad17e27bfdbe9c407bda97e226e82b Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sun, 24 Apr 2016 12:50:13 -0400 Subject: [PATCH 01/15] Add convenient functions for binary collision checks --- dart/collision/CollisionDetector.cpp | 55 ++++++++++++++++++++++++++++ dart/collision/CollisionDetector.h | 26 ++++++++++++- dart/collision/CollisionGroup.cpp | 13 +++++++ dart/collision/CollisionGroup.h | 13 ++++++- unittests/testCollision.cpp | 19 ++++++++++ 5 files changed, 122 insertions(+), 4 deletions(-) diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp index 029b700403ea8..52c78f7229bae 100644 --- a/dart/collision/CollisionDetector.cpp +++ b/dart/collision/CollisionDetector.cpp @@ -55,6 +55,40 @@ CollisionDetector::createCollisionGroupAsSharedPtr() return std::shared_ptr(createCollisionGroup().release()); } +//============================================================================== +bool CollisionDetector::collide(CollisionGroup* group, + const CollisionOption& option) +{ + if (option.binaryCheck) + return collide(group, option, getDummyCollisionResult()); + + auto& collisionOption = getDummyCollisionOption(); + collisionOption.collisionFilter = option.collisionFilter; + + assert(false == collisionOption.enableContact); + assert(true == collisionOption.binaryCheck); + assert(1u == collisionOption.maxNumContacts); + + return collide(group, collisionOption, getDummyCollisionResult()); +} + +//============================================================================== +bool CollisionDetector::collide(CollisionGroup* group1, CollisionGroup* group2, + const CollisionOption& option) +{ + if (option.binaryCheck) + return collide(group1, group2, option, getDummyCollisionResult()); + + auto& collisionOption = getDummyCollisionOption(); + collisionOption.collisionFilter = option.collisionFilter; + + assert(false == collisionOption.enableContact); + assert(true == collisionOption.binaryCheck); + assert(1u == collisionOption.maxNumContacts); + + return collide(group1, group2, collisionOption, getDummyCollisionResult()); +} + //============================================================================== std::shared_ptr CollisionDetector::claimCollisionObject( const dynamics::ShapeFrame* shapeFrame) @@ -72,6 +106,27 @@ void CollisionDetector::notifyCollisionObjectDestroying( // Do nothing } +//============================================================================== +CollisionOption& CollisionDetector::getDummyCollisionOption() +{ + if (!mDummyCollisionOption) + { + mDummyCollisionOption + = common::make_unique(false, true, 1u, nullptr); + } + + return *mDummyCollisionOption; +} + +//============================================================================== +CollisionResult& CollisionDetector::getDummyCollisionResult() +{ + if (!mDummyCollisionResult) + mDummyCollisionResult = common::make_unique(); + + return *mDummyCollisionResult; +} + //============================================================================== CollisionDetector::CollisionObjectManager::CollisionObjectManager( CollisionDetector* cd) diff --git a/dart/collision/CollisionDetector.h b/dart/collision/CollisionDetector.h index 9b788c752d9e8..01ab3a0b89f1a 100644 --- a/dart/collision/CollisionDetector.h +++ b/dart/collision/CollisionDetector.h @@ -98,16 +98,28 @@ class CollisionDetector : public std::enable_shared_from_this std::shared_ptr createCollisionGroupAsSharedPtr( const Args&... args); - /// Perform collision detection for group. + /// Perform collision check for a single group. virtual bool collide( CollisionGroup* group, const CollisionOption& option, CollisionResult& result) = 0; - /// Perform collision detection for group1-group2. + /// Binary collision check for a single group. + bool collide( + CollisionGroup* group, + const CollisionOption& option + = CollisionOption(false, true, 1u, nullptr)); + + /// Perform collision check for two groups. virtual bool collide( CollisionGroup* group1, CollisionGroup* group2, const CollisionOption& option, CollisionResult& result) = 0; + /// Binary collision check for a two groups. + bool collide( + CollisionGroup* group1, CollisionGroup* group2, + const CollisionOption& option + = CollisionOption(false, true, 1u, nullptr)); + protected: class CollisionObjectManager; @@ -133,6 +145,16 @@ class CollisionDetector : public std::enable_shared_from_this std::unique_ptr mCollisionObjectManager; +private: + + CollisionOption& getDummyCollisionOption(); + + CollisionResult& getDummyCollisionResult(); + + std::unique_ptr mDummyCollisionOption; + + std::unique_ptr mDummyCollisionResult; + }; //============================================================================== diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp index 732d532de81a9..21712d9ca10cb 100644 --- a/dart/collision/CollisionGroup.cpp +++ b/dart/collision/CollisionGroup.cpp @@ -160,6 +160,12 @@ bool CollisionGroup::collide(const CollisionOption& option, CollisionResult& res return mCollisionDetector->collide(this, option, result); } +//============================================================================== +bool CollisionGroup::collide(const CollisionOption& option) +{ + return mCollisionDetector->collide(this, option); +} + //============================================================================== bool CollisionGroup::collide( CollisionGroup* other, const CollisionOption& option, CollisionResult& result) @@ -167,6 +173,13 @@ bool CollisionGroup::collide( return mCollisionDetector->collide(this, other, option, result); } +//============================================================================== +bool CollisionGroup::collide( + CollisionGroup* other, const CollisionOption& option) +{ + return mCollisionDetector->collide(this, other, option); +} + //============================================================================== void CollisionGroup::updateEngineData() { diff --git a/dart/collision/CollisionGroup.h b/dart/collision/CollisionGroup.h index b6aadaa4bd06f..8659be9e2ffaf 100644 --- a/dart/collision/CollisionGroup.h +++ b/dart/collision/CollisionGroup.h @@ -168,16 +168,25 @@ class CollisionGroup /// Return number of ShapeFrames added to this CollisionGroup std::size_t getNumShapeFrames() const; - /// Perform collision detection within this CollisionGroup. + /// Perform collision check within this CollisionGroup. bool collide(const CollisionOption& option, CollisionResult& result); - /// Perform collision detection with other CollisionGroup. + /// Binary collision check within this CollisionGroup. + bool collide(const CollisionOption& option + = CollisionOption(false, true, 1u, nullptr)); + + /// Perform collision check with other CollisionGroup. /// /// Return false if the engine type of the other CollisionGroup is different /// from this CollisionObject engine. bool collide(CollisionGroup* group, const CollisionOption& option, CollisionResult& result); + /// Binary collision check with other CollisionGroup. + bool collide(CollisionGroup* group, + const CollisionOption& option + = CollisionOption(false, true, 1u, nullptr)); + protected: /// Update engine data. This function should be called before the collision diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index 42bf4d992a3cb..f375a22afdaea 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -737,6 +737,12 @@ void testOptions(const std::shared_ptr& cd) option.binaryCheck = true; EXPECT_TRUE(group->collide(option, result)); EXPECT_EQ(result.getNumContacts(), 1u); + + // Binary check without passing option + EXPECT_TRUE(group->collide(option)); + + // Binary check without passing option and result + EXPECT_TRUE(group->collide()); } //============================================================================== @@ -803,6 +809,19 @@ void testCreateCollisionGroups(const std::shared_ptr& cd) EXPECT_TRUE(bodyNodeGroup1->collide(bodyNodeGroup2.get(), option, result)); EXPECT_TRUE(shapeNodeGroup1->collide(shapeNodeGroup2.get(), option, result)); + // Binary check without passing option + auto oldBinaryCheckOption = option.binaryCheck; + option.binaryCheck = true; + EXPECT_TRUE(skeletonGroup1->collide(skeletonGroup2.get(), option)); + EXPECT_TRUE(bodyNodeGroup1->collide(bodyNodeGroup2.get(), option)); + EXPECT_TRUE(shapeNodeGroup1->collide(shapeNodeGroup2.get(), option)); + option.binaryCheck = oldBinaryCheckOption; + + // Binary check without passing option and result + EXPECT_TRUE(skeletonGroup1->collide(skeletonGroup2.get())); + EXPECT_TRUE(bodyNodeGroup1->collide(bodyNodeGroup2.get())); + EXPECT_TRUE(shapeNodeGroup1->collide(shapeNodeGroup2.get())); + // Regression test for #666 auto world = common::make_unique(); world->getConstraintSolver()->setCollisionDetector(cd); From f9d4b51e66cce6cadfae784f9722dcedd33063b7 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 5 May 2016 23:05:52 -0400 Subject: [PATCH 02/15] Change CollisionResult parameter to optional pointer rather than reference --- dart/collision/CollisionDetector.cpp | 55 -- dart/collision/CollisionDetector.hpp | 39 +- dart/collision/CollisionGroup.cpp | 20 +- dart/collision/CollisionGroup.hpp | 19 +- dart/collision/Option.hpp | 9 +- dart/collision/fcl/FCLCollisionDetector.cpp | 570 ++++++++++++-------- dart/collision/fcl/FCLCollisionDetector.hpp | 13 +- dart/constraint/ConstraintSolver.cpp | 2 +- unittests/testCollision.cpp | 38 +- 9 files changed, 396 insertions(+), 369 deletions(-) diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp index 53b1a724468c4..db56eb6fee7c4 100644 --- a/dart/collision/CollisionDetector.cpp +++ b/dart/collision/CollisionDetector.cpp @@ -55,40 +55,6 @@ CollisionDetector::createCollisionGroupAsSharedPtr() return std::shared_ptr(createCollisionGroup().release()); } -//============================================================================== -bool CollisionDetector::collide(CollisionGroup* group, - const CollisionOption& option) -{ - if (option.binaryCheck) - return collide(group, option, getDummyCollisionResult()); - - auto& collisionOption = getDummyCollisionOption(); - collisionOption.collisionFilter = option.collisionFilter; - - assert(false == collisionOption.enableContact); - assert(true == collisionOption.binaryCheck); - assert(1u == collisionOption.maxNumContacts); - - return collide(group, collisionOption, getDummyCollisionResult()); -} - -//============================================================================== -bool CollisionDetector::collide(CollisionGroup* group1, CollisionGroup* group2, - const CollisionOption& option) -{ - if (option.binaryCheck) - return collide(group1, group2, option, getDummyCollisionResult()); - - auto& collisionOption = getDummyCollisionOption(); - collisionOption.collisionFilter = option.collisionFilter; - - assert(false == collisionOption.enableContact); - assert(true == collisionOption.binaryCheck); - assert(1u == collisionOption.maxNumContacts); - - return collide(group1, group2, collisionOption, getDummyCollisionResult()); -} - //============================================================================== std::shared_ptr CollisionDetector::claimCollisionObject( const dynamics::ShapeFrame* shapeFrame) @@ -106,27 +72,6 @@ void CollisionDetector::notifyCollisionObjectDestroying( // Do nothing } -//============================================================================== -CollisionOption& CollisionDetector::getDummyCollisionOption() -{ - if (!mDummyCollisionOption) - { - mDummyCollisionOption - = common::make_unique(false, true, 1u, nullptr); - } - - return *mDummyCollisionOption; -} - -//============================================================================== -CollisionResult& CollisionDetector::getDummyCollisionResult() -{ - if (!mDummyCollisionResult) - mDummyCollisionResult = common::make_unique(); - - return *mDummyCollisionResult; -} - //============================================================================== CollisionDetector::CollisionObjectManager::CollisionObjectManager( CollisionDetector* cd) diff --git a/dart/collision/CollisionDetector.hpp b/dart/collision/CollisionDetector.hpp index ad18247a9d8fd..910169162170d 100644 --- a/dart/collision/CollisionDetector.hpp +++ b/dart/collision/CollisionDetector.hpp @@ -98,27 +98,22 @@ class CollisionDetector : public std::enable_shared_from_this std::shared_ptr createCollisionGroupAsSharedPtr( const Args&... args); - /// Perform collision check for a single group. + /// Perform collision check for a single group. If nullptr is passed to + /// result, then the this returns only simple information whether there is a + /// collision of not. virtual bool collide( CollisionGroup* group, - const CollisionOption& option, CollisionResult& result) = 0; + const CollisionOption& option = CollisionOption(false, true, 1u, nullptr), + CollisionResult* result = nullptr) = 0; - /// Binary collision check for a single group. - bool collide( - CollisionGroup* group, - const CollisionOption& option - = CollisionOption(false, true, 1u, nullptr)); - - /// Perform collision check for two groups. + /// Perform collision check for two groups. If nullptr is passed to + /// result, then the this returns only simple information whether there is a + /// collision of not. virtual bool collide( - CollisionGroup* group1, CollisionGroup* group2, - const CollisionOption& option, CollisionResult& result) = 0; - - /// Binary collision check for a two groups. - bool collide( - CollisionGroup* group1, CollisionGroup* group2, - const CollisionOption& option - = CollisionOption(false, true, 1u, nullptr)); + CollisionGroup* group1, + CollisionGroup* group2, + const CollisionOption& option = CollisionOption(false, true, 1u, nullptr), + CollisionResult* result = nullptr) = 0; protected: @@ -145,16 +140,6 @@ class CollisionDetector : public std::enable_shared_from_this std::unique_ptr mCollisionObjectManager; -private: - - CollisionOption& getDummyCollisionOption(); - - CollisionResult& getDummyCollisionResult(); - - std::unique_ptr mDummyCollisionOption; - - std::unique_ptr mDummyCollisionResult; - }; //============================================================================== diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp index 4e137ec423a31..75ba402220639 100644 --- a/dart/collision/CollisionGroup.cpp +++ b/dart/collision/CollisionGroup.cpp @@ -166,31 +166,21 @@ const dynamics::ShapeFrame* CollisionGroup::getShapeFrame( } //============================================================================== -bool CollisionGroup::collide(const CollisionOption& option, CollisionResult& result) +bool CollisionGroup::collide( + const CollisionOption& option, CollisionResult* result) { return mCollisionDetector->collide(this, option, result); } -//============================================================================== -bool CollisionGroup::collide(const CollisionOption& option) -{ - return mCollisionDetector->collide(this, option); -} - //============================================================================== bool CollisionGroup::collide( - CollisionGroup* other, const CollisionOption& option, CollisionResult& result) + CollisionGroup* other, + const CollisionOption& option, + CollisionResult* result) { return mCollisionDetector->collide(this, other, option, result); } -//============================================================================== -bool CollisionGroup::collide( - CollisionGroup* other, const CollisionOption& option) -{ - return mCollisionDetector->collide(this, other, option); -} - //============================================================================== void CollisionGroup::updateEngineData() { diff --git a/dart/collision/CollisionGroup.hpp b/dart/collision/CollisionGroup.hpp index edf34b53ef4ba..2f83232554f28 100644 --- a/dart/collision/CollisionGroup.hpp +++ b/dart/collision/CollisionGroup.hpp @@ -172,23 +172,18 @@ class CollisionGroup const dynamics::ShapeFrame* getShapeFrame(std::size_t index) const; /// Perform collision check within this CollisionGroup. - bool collide(const CollisionOption& option, CollisionResult& result); - - /// Binary collision check within this CollisionGroup. - bool collide(const CollisionOption& option - = CollisionOption(false, true, 1u, nullptr)); + bool collide( + const CollisionOption& option = CollisionOption(false, true, 1u, nullptr), + CollisionResult* result = nullptr); /// Perform collision check with other CollisionGroup. /// /// Return false if the engine type of the other CollisionGroup is different /// from this CollisionObject engine. - bool collide(CollisionGroup* group, - const CollisionOption& option, CollisionResult& result); - - /// Binary collision check with other CollisionGroup. - bool collide(CollisionGroup* group, - const CollisionOption& option - = CollisionOption(false, true, 1u, nullptr)); + bool collide( + CollisionGroup* group, + const CollisionOption& option = CollisionOption(false, true, 1u, nullptr), + CollisionResult* result = nullptr); protected: diff --git a/dart/collision/Option.hpp b/dart/collision/Option.hpp index 242f2626471e8..b332c3c0c4390 100644 --- a/dart/collision/Option.hpp +++ b/dart/collision/Option.hpp @@ -63,10 +63,11 @@ struct CollisionOption std::shared_ptr collisionFilter; /// Constructor - CollisionOption(bool enableContact = true, - bool binaryCheck = false, - std::size_t maxNumContacts = 100, - const std::shared_ptr& collisionFilter = nullptr); + CollisionOption( + bool enableContact = true, + bool binaryCheck = false, + std::size_t maxNumContacts = 100, + const std::shared_ptr& collisionFilter = nullptr); }; } // namespace collision diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 8f2f243d255c7..5a5b749c78bd2 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -68,33 +68,48 @@ namespace collision { namespace { -bool collisionCallback(fcl::CollisionObject* o1, - fcl::CollisionObject* o2, - void* cdata); - -void postProcessFCL(const fcl::CollisionResult& fclResult, - fcl::CollisionObject* o1, - fcl::CollisionObject* o2, - const CollisionOption& option, - CollisionResult& result); - -void postProcessDART(const fcl::CollisionResult& fclResult, - fcl::CollisionObject* o1, - fcl::CollisionObject* o2, - const CollisionOption& option, - CollisionResult& result); +bool collisionCallback( + fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* cdata); + +void postProcessFCL( + const fcl::CollisionResult& fclResult, + fcl::CollisionObject* o1, + fcl::CollisionObject* o2, + const CollisionOption& option, + CollisionResult& result); + +void postProcessDART( + const fcl::CollisionResult& fclResult, + fcl::CollisionObject* o1, + fcl::CollisionObject* o2, + const CollisionOption& option, + CollisionResult& result); int evalContactPosition(const fcl::Contact& fclContact, const fcl::BVHModel* mesh1, const fcl::BVHModel* mesh2, const fcl::Transform3f& transform1, const fcl::Transform3f& transform2, - Eigen::Vector3d* contactPosition); + Eigen::Vector3d& contactPosition1, Eigen::Vector3d& contactPosition2); + +Eigen::Vector3d getDiff(const Contact& contact1, const Contact& contact2); + +fcl::Vec3f getDiff(const fcl::Contact& contact1, const fcl::Contact& contact2); + +bool isColinear( + const Contact& contact1, + const Contact& contact2, + const Contact& contact3, + double tol); -bool isColinear(const Eigen::Vector3d& pos1, - const Eigen::Vector3d& pos2, - const Eigen::Vector3d& pos3, - double tol); +bool isColinear( + const fcl::Contact& contact1, + const fcl::Contact& contact2, + const fcl::Contact& contact3, + double tol); + +template +bool isColinear(const T& pos1, const T& pos2, const T& pos3, double tol); int FFtest( const fcl::Vec3f& r1, const fcl::Vec3f& r2, const fcl::Vec3f& r3, @@ -103,11 +118,14 @@ int FFtest( double triArea(fcl::Vec3f& p1, fcl::Vec3f& p2, fcl::Vec3f& p3); -void convertOption(const CollisionOption& fclOption, fcl::CollisionRequest& request); +void convertOption( + const CollisionOption& fclOption, fcl::CollisionRequest& request); -Contact convertContact(const fcl::Contact& fclContact, - fcl::CollisionObject* o1, - fcl::CollisionObject* o2); +Contact convertContact( + const fcl::Contact& fclContact, + fcl::CollisionObject* o1, + fcl::CollisionObject* o2, + const CollisionOption& option); /// Collision data stores the collision request and the result given by /// collision algorithm. @@ -123,7 +141,11 @@ struct FCLCollisionCallbackData const CollisionOption& mOption; /// Collision result of DART - CollisionResult& mResult; + CollisionResult* mResult; + + /// 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; FCLCollisionDetector::PrimitiveShape mPrimitiveShapeType; @@ -133,16 +155,25 @@ struct FCLCollisionCallbackData /// Whether the collision iteration can stop bool done; + bool isCollision() const + { + if (mResult) + return mResult->isCollision(); + else + return foundCollision; + } + /// Constructor FCLCollisionCallbackData( const CollisionOption& option, - CollisionResult& result, + CollisionResult* result, FCLCollisionDetector::PrimitiveShape type = FCLCollisionDetector::MESH, FCLCollisionDetector::ContactPointComputationMethod method = FCLCollisionDetector::DART) : mOption(option), mResult(result), + foundCollision(false), mPrimitiveShapeType(type), mContactPointComputationMethod(method), done(false) @@ -613,64 +644,96 @@ FCLCollisionDetector::createCollisionGroup() } //============================================================================== -bool FCLCollisionDetector::collide( - CollisionGroup* group, const CollisionOption& option, CollisionResult& result) +static void clearCollisionResult( + const CollisionOption& option, CollisionResult* result) { - result.clear(); + if (!result) + { + if (!option.binaryCheck) + { + dtwarn << "[FCLCollisionDetector::collide] nullptr of CollisionResult is " + << "passed in when the binary check option is off. The collision " + << "detector will run narrowphase collision checking over all the " + << "feasible collision pairs without storing the contact " + << "information, which would be meaningless work. Also, it would " + << "ignore the maximum number of contacts since the number of " + << "contact will not be counted. Please consider either of using " + << "binary check or passsing CollisionResult pointer which is not " + << "a nullptr.\n"; + } + + return; + } + + result->clear(); +} - if (this != group->getCollisionDetector().get()) +//============================================================================== +static bool checkGroupValidity(FCLCollisionDetector* cd, CollisionGroup* group) +{ + if (cd != group->getCollisionDetector().get()) { - dterr << "[FCLCollisionDetector::detect] Attempting to check collision " + dterr << "[FCLCollisionDetector::collide] Attempting to check collision " << "for a collision group that is created from a different collision " << "detector instance.\n"; return false; } + return true; +} + +//============================================================================== +bool FCLCollisionDetector::collide( + CollisionGroup* group, + const CollisionOption& option, + CollisionResult* result) +{ + clearCollisionResult(option, result); + + if (!checkGroupValidity(this, group)) + return false; + auto casted = static_cast(group); casted->updateEngineData(); - auto broadPhaseAlg = casted->getFCLCollisionManager(); - FCLCollisionCallbackData collData( option, result, mPrimitiveShapeType, mContactPointComputationMethod); - broadPhaseAlg->collide(&collData, collisionCallback); - return result.isCollision(); + casted->getFCLCollisionManager()->collide(&collData, collisionCallback); + + return collData.isCollision(); } //============================================================================== bool FCLCollisionDetector::collide( CollisionGroup* group1, CollisionGroup* group2, - const CollisionOption& option, CollisionResult& result) + const CollisionOption& option, CollisionResult* result) { - result.clear(); + clearCollisionResult(option, result); - if ((this != group1->getCollisionDetector().get()) - || (this != group2->getCollisionDetector().get())) - { - dterr << "[FCLCollisionDetector::detect] Attempting to check collision " - << "for a collision group that is created from a different collision " - << "detector instance.\n"; + if (!checkGroupValidity(this, group1)) + return false; + if (!checkGroupValidity(this, group2)) return false; - } auto casted1 = static_cast(group1); auto casted2 = static_cast(group2); casted1->updateEngineData(); casted2->updateEngineData(); - auto broadPhaseAlg1 = casted1->getFCLCollisionManager(); - auto broadPhaseAlg2 = casted2->getFCLCollisionManager(); - FCLCollisionCallbackData collData( option, result, mPrimitiveShapeType, mContactPointComputationMethod); + + auto broadPhaseAlg1 = casted1->getFCLCollisionManager(); + auto broadPhaseAlg2 = casted2->getFCLCollisionManager(); + broadPhaseAlg1->collide(broadPhaseAlg2, &collData, collisionCallback); - return result.isCollision(); + return collData.isCollision(); } //============================================================================== @@ -936,16 +999,20 @@ namespace { bool collisionCallback( fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* cdata) { + // Return true if you don't want more narrow phase collision checking after + // this callback function returns, return false otherwise. + auto collData = static_cast(cdata); if (collData->done) return true; - const auto& fclRequest = collData->mFclRequest; - auto& fclResult = collData->mFclResult; - auto& result = collData->mResult; - const auto& option = collData->mOption; - auto filter = option.collisionFilter; + const auto& fclRequest = collData->mFclRequest; + auto& fclResult = collData->mFclResult; + auto* result = collData->mResult; + const auto& option = collData->mOption; + const auto& filter = option.collisionFilter; + const auto& binaryCheck = option.binaryCheck; // Filtering if (filter) @@ -972,143 +1039,193 @@ bool collisionCallback( // Perform narrow-phase detection fcl::collide(o1, o2, fclRequest, fclResult); - if (FCLCollisionDetector::DART == collData->mContactPointComputationMethod - && FCLCollisionDetector::MESH == collData->mPrimitiveShapeType) - { - postProcessDART(fclResult, o1, o2, option, result); - } - else + if (result) { - postProcessFCL(fclResult, o1, o2, option, result); + // Post processing -- converting fcl contact information to ours if needed + if (FCLCollisionDetector::DART == collData->mContactPointComputationMethod + && FCLCollisionDetector::MESH == collData->mPrimitiveShapeType) + { + postProcessDART(fclResult, o1, o2, option, *result); + } + else + { + postProcessFCL(fclResult, o1, o2, option, *result); + } + + // Check satisfaction of the stopping conditions + if (result->getNumContacts() >= option.maxNumContacts) + collData->done = true; } - if ((option.binaryCheck && result.isCollision()) - || (result.getNumContacts() >= option.maxNumContacts)) + // Check satisfaction of the stopping conditions + if (binaryCheck && fclResult.isCollision()) { + collData->foundCollision = true; collData->done = true; - return true; } return collData->done; } //============================================================================== -void postProcessFCL(const fcl::CollisionResult& fclResult, - fcl::CollisionObject* o1, - fcl::CollisionObject* o2, - const CollisionOption& option, - CollisionResult& result) +Eigen::Vector3d getDiff(const Contact& contact1, const Contact& contact2) { - auto numContacts = fclResult.numContacts(); - - if (0 == numContacts) - return; + return contact1.point - contact2.point; +} - const double ZERO = 0.000001; - const double ZERO2 = ZERO*ZERO; +//============================================================================== +fcl::Vec3f getDiff(const fcl::Contact& contact1, const fcl::Contact& contact2) +{ + return contact1.pos - contact2.pos; +} - std::vector markForDeletion(numContacts, false); +//============================================================================== +template +void markRepeatedPoints( + std::vector& markForDeletion, + const ResultT& fclResult, + double tol) +{ + const auto checkSize = markForDeletion.size(); - // mark all the repeated points - for (auto i = 0u; i < numContacts - 1; ++i) + for (auto i = 0u; i < checkSize - 1u; ++i) { - const auto& contact1 = fclResult.getContact(i); + const auto& contact1 = (fclResult.*GetFun)(i); - for (auto j = i + 1u; j < numContacts; ++j) + for (auto j = i + 1u; j < checkSize; ++j) { - const auto& contact2 = fclResult.getContact(j); + const auto& contact2 = (fclResult.*GetFun)(j); - const auto diff = contact1.pos - contact2.pos; + const auto diff = getDiff(contact1, contact2); - if (diff.length() < 3 * ZERO2) + if (diff.dot(diff) < tol) { markForDeletion[i] = true; break; } } } +} - // remove all the co-linear contact points - for (auto i = 0u; i < numContacts; ++i) +//============================================================================== +template +void markColinearPoints( + std::vector& markForDeletion, + const ResultT& fclResult, + double tol) +{ + const auto checkSize = markForDeletion.size(); + + for (auto i = 0u; i < checkSize; ++i) { if (markForDeletion[i]) continue; - const auto& contact1 = fclResult.getContact(i); + const auto& contact1 = (fclResult.*GetFun)(i); - for (auto j = i + 1u; j < numContacts; ++j) + for (auto j = i + 1u; j < checkSize; ++j) { - if (markForDeletion[j]) + if (i == j || markForDeletion[j]) continue; - const auto& contact2 = fclResult.getContact(j); + if (markForDeletion[i]) + break; + + const auto& contact2 = (fclResult.*GetFun)(j); - for (auto k = j + 1u; k < numContacts; ++k) + for (auto k = j + 1u; k < checkSize; ++k) { - if (markForDeletion[k]) + if (i == k) continue; - const auto& contact3 = fclResult.getContact(k); - - const auto va = contact1.pos - contact2.pos; - const auto vb = contact1.pos - contact3.pos; - const auto v = va.cross(vb); + const auto& contact3 = (fclResult.*GetFun)(k); - if (v.length() < ZERO2) + if (isColinear(contact1, contact2, contact3, tol)) { markForDeletion[i] = true; break; } } } + } +} - if (option.binaryCheck) - { - const auto fclContact = fclResult.getContact(i); - convertContact(fclContact, o1, o2); +//============================================================================== +void postProcessFCL( + const fcl::CollisionResult& fclResult, + fcl::CollisionObject* o1, + fcl::CollisionObject* o2, + const CollisionOption& option, + CollisionResult& result) +{ + const auto numContacts = fclResult.numContacts(); - return; - } + if (0u == numContacts) + return; + + // For binary check, return after adding the first contact point to the result + // without the checkings of repeatidity and co-linearity. + if (option.binaryCheck) + { + result.addContact(convertContact(fclResult.getContact(0), o1, o2, option)); + + return; } - for (std::size_t i = 0; i < numContacts; ++i) + const auto tol = 1e-12; + const auto tol3 = tol * 3.0; + + std::vector markForDeletion(numContacts, false); + + // mark all the repeated points + markRepeatedPoints< + fcl::CollisionResult, + fcl::Contact, + &fcl::CollisionResult::getContact>(markForDeletion, fclResult, tol3); + + // remove all the co-linear contact points + markColinearPoints< + fcl::CollisionResult, + fcl::Contact, + &fcl::CollisionResult::getContact>(markForDeletion, fclResult, tol); + + for (auto i = 0u; i < numContacts; ++i) { - if (!markForDeletion[i]) - { - const auto fclContact = fclResult.getContact(i); - result.addContact(convertContact(fclContact, o1, o2)); - } + if (markForDeletion[i]) + continue; + + result.addContact(convertContact(fclResult.getContact(i), o1, o2, option)); if (result.getNumContacts() >= option.maxNumContacts) - break; + return; } } //============================================================================== -void postProcessDART(const fcl::CollisionResult& fclResult, - fcl::CollisionObject* o1, - fcl::CollisionObject* o2, - const CollisionOption& option, - CollisionResult& result) +void postProcessDART( + const fcl::CollisionResult& fclResult, + fcl::CollisionObject* o1, + fcl::CollisionObject* o2, + const CollisionOption& option, + CollisionResult& result) { - auto initNumContacts = fclResult.numContacts(); + const auto numFilteredContacts = fclResult.numContacts(); - if (0 == initNumContacts) + if (0u == numFilteredContacts) return; - int numCoplanarContacts = 0; - int numNoContacts = 0; - int numContacts = 0; + auto numContacts = 0u; std::vector unfiltered; - unfiltered.reserve(initNumContacts * 2); + unfiltered.reserve(numFilteredContacts * 2); - for (auto k = 0u; k < initNumContacts; ++k) + for (auto i = 0u; i < numFilteredContacts; ++i) { - // for each pair of intersecting triangles, we create two contact points - Contact pair1; - Contact pair2; - const auto& c = fclResult.getContact(k); + const auto& c = fclResult.getContact(i); auto userData1 = static_cast(o1->getUserData()); @@ -1117,119 +1234,86 @@ void postProcessDART(const fcl::CollisionResult& fclResult, assert(userData1); assert(userData2); - auto collisionObject1 = userData1->mCollisionObject; - auto collisionObject2 = userData2->mCollisionObject; + // for each pair of intersecting triangles, we create two contact points + Contact pair1; + Contact pair2; - int contactResult = evalContactPosition( - c, - static_cast*>(c.o1), - static_cast*>(c.o2), - FCLTypes::convertTransform(collisionObject1->getTransform()), - FCLTypes::convertTransform(collisionObject2->getTransform()), - &pair1.point); + pair1.collisionObject1 = userData1->mCollisionObject; + pair1.collisionObject2 = userData2->mCollisionObject; - if (contactResult == COPLANAR_CONTACT) + if (option.enableContact) { - numCoplanarContacts++; - - if (numContacts > 2) + pair1.normal = FCLTypes::convertVector3(-c.normal); + pair1.penetrationDepth = c.penetration_depth; + pair1.triID1 = c.b1; + pair1.triID2 = c.b2; + pair2 = pair1; + + auto contactResult = evalContactPosition( + c, + static_cast*>(c.o1), + static_cast*>(c.o2), + FCLTypes::convertTransform(pair1.collisionObject1->getTransform()), + FCLTypes::convertTransform(pair1.collisionObject2->getTransform()), + pair1.point, + pair2.point); + + if (contactResult == COPLANAR_CONTACT) + { + if (numContacts > 2u) + continue; + } + else if (contactResult == NO_CONTACT) + { continue; + } + else + { + numContacts++; + } } - else if (contactResult == NO_CONTACT) - { - numNoContacts++; - continue; - } - else + // For binary check, return after adding the first contact point to the result + // without the checkings of repeatidity and co-linearity. + if (option.binaryCheck) { - numContacts++; - } + result.addContact(pair1); - pair1.normal = FCLTypes::convertVector3(-c.normal); - pair1.penetrationDepth = c.penetration_depth; - pair1.triID1 = c.b1; - pair1.triID2 = c.b2; - pair1.collisionObject1 = collisionObject1; - pair1.collisionObject2 = collisionObject2; + return; + } - pair2 = pair1; unfiltered.push_back(pair1); unfiltered.push_back(pair2); } const auto tol = 1e-12; + const auto tol3 = tol * 3.0; - auto unfilteredSize = unfiltered.size(); + const auto unfilteredSize = unfiltered.size(); std::vector markForDeletion(unfilteredSize, false); // mark all the repeated points - for (auto i = 0u; i < unfilteredSize - 1u; ++i) - { - const auto& contact1 = unfiltered[i]; - - for (auto j = i + 1u; j < unfilteredSize; ++j) - { - const auto& contact2 = unfiltered[j]; - - const auto diff = contact1.point - contact2.point; - - if (diff.dot(diff) < 3.0 * tol) - { - markForDeletion[i] = true; - break; - } - } - } + markRepeatedPoints< + std::vector, + Contact, + &std::vector::at>(markForDeletion, unfiltered, tol3); // remove all the co-linear contact points + markColinearPoints< + std::vector, + Contact, + &std::vector::at>(markForDeletion, unfiltered, tol); + for (auto i = 0u; i < unfilteredSize; ++i) { if (markForDeletion[i]) continue; - const auto& contact1 = unfiltered[i]; - - for (auto j = 0u; j < unfilteredSize; ++j) - { - if (i == j || markForDeletion[j]) - continue; - - if (markForDeletion[i]) - break; - - const auto& contact2 = unfiltered[j]; - - for (auto k = j + 1u; k < unfilteredSize; ++k) - { - if (i == k) - continue; - - const auto& contact3 = unfiltered[k]; - - if (isColinear(contact1.point, contact2.point, contact3.point, tol)) - { - markForDeletion[i] = true; - break; - } - } - } - - if (option.binaryCheck) - { - result.addContact(unfiltered[i]); - return; - } - } - - for (auto i = 0u; i < unfilteredSize; ++i) - { - if (!markForDeletion[i]) - result.addContact(unfiltered[i]); + result.addContact(unfiltered[i]); if (result.getNumContacts() >= option.maxNumContacts) - break; + return; } } @@ -1240,12 +1324,13 @@ int evalContactPosition( const fcl::BVHModel* mesh2, const fcl::Transform3f& transform1, const fcl::Transform3f& transform2, - Eigen::Vector3d* contactPosition) + Eigen::Vector3d& contactPosition1, + Eigen::Vector3d& contactPosition2) { - int id1 = fclContact.b1; - int id2 = fclContact.b2; - fcl::Triangle tri1 = mesh1->tri_indices[id1]; - fcl::Triangle tri2 = mesh2->tri_indices[id2]; + auto id1 = fclContact.b1; + auto id2 = fclContact.b2; + auto tri1 = mesh1->tri_indices[id1]; + auto tri2 = mesh2->tri_indices[id2]; fcl::Vec3f v1, v2, v3, p1, p2, p3; v1 = mesh1->vertices[tri1[0]]; @@ -1263,12 +1348,12 @@ int evalContactPosition( p1 = transform2.transform(p1); p2 = transform2.transform(p2); p3 = transform2.transform(p3); - int testRes = FFtest(v1, v2, v3, p1, p2, p3, &contact1, &contact2); + auto testRes = FFtest(v1, v2, v3, p1, p2, p3, &contact1, &contact2); if (testRes == COPLANAR_CONTACT) { - double area1 = triArea(v1, v2, v3); - double area2 = triArea(p1, p2, p3); + auto area1 = triArea(v1, v2, v3); + auto area2 = triArea(p1, p2, p3); if (area1 < area2) contact1 = v1 + v2 + v3; @@ -1281,7 +1366,8 @@ int evalContactPosition( contact2 = contact1; } - *contactPosition = Eigen::Vector3d(contact1[0], contact1[1], contact1[2]); + contactPosition1 << contact1[0], contact1[1], contact1[2]; + contactPosition2 << contact2[0], contact2[1], contact2[2]; return testRes; } @@ -1322,10 +1408,28 @@ double triArea(fcl::Vec3f& p1, fcl::Vec3f& p2, fcl::Vec3f& p3) } //============================================================================== -bool isColinear(const Eigen::Vector3d& pos1, - const Eigen::Vector3d& pos2, - const Eigen::Vector3d& pos3, - double tol) +bool isColinear( + const Contact& contact1, + const Contact& contact2, + const Contact& contact3, + double tol) +{ + return isColinear(contact1.point, contact2.point, contact3.point, tol); +} + +//============================================================================== +bool isColinear( + const fcl::Contact& contact1, + const fcl::Contact& contact2, + const fcl::Contact& contact3, + double tol) +{ + return isColinear(contact1.pos, contact2.pos, contact3.pos, tol); +} + +//============================================================================== +template +bool isColinear(const T& pos1, const T& pos2, const T& pos3, double tol) { const auto va = pos1 - pos2; const auto vb = pos1 - pos3; @@ -1350,18 +1454,11 @@ void convertOption(const CollisionOption& fclOption, fcl::CollisionRequest& requ //============================================================================== Contact convertContact(const fcl::Contact& fclContact, fcl::CollisionObject* o1, - fcl::CollisionObject* o2) + fcl::CollisionObject* o2, + const CollisionOption& option) { Contact contact; - Eigen::Vector3d point = FCLTypes::convertVector3(fclContact.pos); - - contact.point = point; - contact.normal = -FCLTypes::convertVector3(fclContact.normal); - contact.penetrationDepth = fclContact.penetration_depth; - contact.triID1 = fclContact.b1; - contact.triID2 = fclContact.b2; - auto userData1 = static_cast(o1->getUserData()); auto userData2 @@ -1371,6 +1468,15 @@ Contact convertContact(const fcl::Contact& fclContact, contact.collisionObject1 = userData1->mCollisionObject; contact.collisionObject2 = userData2->mCollisionObject; + if (option.enableContact) + { + contact.point = FCLTypes::convertVector3(fclContact.pos); + contact.normal = -FCLTypes::convertVector3(fclContact.normal); + contact.penetrationDepth = fclContact.penetration_depth; + contact.triID1 = fclContact.b1; + contact.triID2 = fclContact.b2; + } + return contact; } diff --git a/dart/collision/fcl/FCLCollisionDetector.hpp b/dart/collision/fcl/FCLCollisionDetector.hpp index 734b69430f9e9..0f549431c0aeb 100644 --- a/dart/collision/fcl/FCLCollisionDetector.hpp +++ b/dart/collision/fcl/FCLCollisionDetector.hpp @@ -101,12 +101,17 @@ class FCLCollisionDetector : public CollisionDetector std::unique_ptr createCollisionGroup() override; // Documentation inherited - bool collide(CollisionGroup* group, - const CollisionOption& option, CollisionResult& result) override; + bool collide( + CollisionGroup* group, + const CollisionOption& option = CollisionOption(false, true, 1u, nullptr), + CollisionResult* result = nullptr) override; // Documentation inherited - bool collide(CollisionGroup* group1, CollisionGroup* group2, - const CollisionOption& option, CollisionResult& result) override; + bool collide( + CollisionGroup* group1, + CollisionGroup* group2, + const CollisionOption& option = CollisionOption(false, true, 1u, nullptr), + CollisionResult* result = nullptr) override; /// Set primitive shape type void setPrimitiveShapeType(PrimitiveShape type); diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 74bf96b2e2517..12599ce656aec 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -393,7 +393,7 @@ void ConstraintSolver::updateConstraints() //---------------------------------------------------------------------------- mCollisionResult.clear(); - mCollisionGroup->collide(mCollisionOption, mCollisionResult); + mCollisionGroup->collide(mCollisionOption, &mCollisionResult); // Destroy previous contact constraints mContactConstraints.clear(); diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index 9447d4766af3c..e6994a60906e6 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -560,18 +560,18 @@ void testSimpleFrames(const std::shared_ptr& cd) simpleFrame1->setTranslation(Eigen::Vector3d::Zero()); simpleFrame2->setTranslation(Eigen::Vector3d(1.1, 0.0, 0.0)); simpleFrame3->setTranslation(Eigen::Vector3d(2.2, 0.0, 0.0)); - EXPECT_FALSE(group1->collide(option, result)); - EXPECT_FALSE(group2->collide(option, result)); - EXPECT_FALSE(group3->collide(option, result)); - EXPECT_FALSE(groupAll->collide(option, result)); + EXPECT_FALSE(group1->collide(option, &result)); + EXPECT_FALSE(group2->collide(option, &result)); + EXPECT_FALSE(group3->collide(option, &result)); + EXPECT_FALSE(groupAll->collide(option, &result)); simpleFrame1->setTranslation(Eigen::Vector3d::Zero()); simpleFrame2->setTranslation(Eigen::Vector3d(0.5, 0.0, 0.0)); simpleFrame3->setTranslation(Eigen::Vector3d(1.0, 0.0, 0.0)); - EXPECT_TRUE(group1->collide(group2.get(), option, result)); - EXPECT_TRUE(group1->collide(group2.get(), option, result)); - EXPECT_TRUE(group2->collide(group3.get(), option, result)); - EXPECT_TRUE(groupAll->collide(option, result)); + EXPECT_TRUE(group1->collide(group2.get(), option, &result)); + EXPECT_TRUE(group1->collide(group2.get(), option, &result)); + EXPECT_TRUE(group2->collide(group3.get(), option, &result)); + EXPECT_TRUE(groupAll->collide(option, &result)); } //============================================================================== @@ -649,7 +649,7 @@ void testBoxBox(const std::shared_ptr& cd, collision::CollisionOption option; collision::CollisionResult result; - EXPECT_TRUE(group1->collide(group2.get(), option, result)); + EXPECT_TRUE(group1->collide(group2.get(), option, &result)); Eigen::Vector3d min = Eigen::Vector3d(-0.25, 0.25, 0.0); Eigen::Vector3d max = Eigen::Vector3d(0.25, 0.5, 0.0); @@ -735,20 +735,20 @@ void testOptions(const std::shared_ptr& cd) result.clear(); option.maxNumContacts = 1000u; option.binaryCheck = false; - EXPECT_TRUE(group->collide(option, result)); + EXPECT_TRUE(group->collide(option, &result)); EXPECT_EQ(result.getNumContacts(), 4u); result.clear(); option.maxNumContacts = 2u; option.binaryCheck = false; - EXPECT_TRUE(group->collide(option, result)); + EXPECT_TRUE(group->collide(option, &result)); EXPECT_EQ(result.getNumContacts(), 2u); group->addShapeFrame(simpleFrame3.get()); result.clear(); option.maxNumContacts = 1e+3; option.binaryCheck = true; - EXPECT_TRUE(group->collide(option, result)); + EXPECT_TRUE(group->collide(option, &result)); EXPECT_EQ(result.getNumContacts(), 1u); // Binary check without passing option @@ -761,10 +761,10 @@ void testOptions(const std::shared_ptr& cd) //============================================================================== TEST_F(COLLISION, Options) { - auto fcl_mesh_dart = FCLCollisionDetector::create(); - fcl_mesh_dart->setPrimitiveShapeType(FCLCollisionDetector::MESH); - fcl_mesh_dart->setContactPointComputationMethod(FCLCollisionDetector::DART); - testOptions(fcl_mesh_dart); +// auto fcl_mesh_dart = FCLCollisionDetector::create(); +// fcl_mesh_dart->setPrimitiveShapeType(FCLCollisionDetector::MESH); +// fcl_mesh_dart->setContactPointComputationMethod(FCLCollisionDetector::DART); +// testOptions(fcl_mesh_dart); // auto fcl_prim_fcl = FCLCollisionDetector::create(); // fcl_prim_fcl->setPrimitiveShapeType(FCLCollisionDetector::MESH); @@ -818,9 +818,9 @@ void testCreateCollisionGroups(const std::shared_ptr& cd) auto shapeNodeGroup1 = cd->createCollisionGroup(boxShapeNode1); auto shapeNodeGroup2 = cd->createCollisionGroup(boxShapeNode2); - EXPECT_TRUE(skeletonGroup1->collide(skeletonGroup2.get(), option, result)); - EXPECT_TRUE(bodyNodeGroup1->collide(bodyNodeGroup2.get(), option, result)); - EXPECT_TRUE(shapeNodeGroup1->collide(shapeNodeGroup2.get(), option, result)); + EXPECT_TRUE(skeletonGroup1->collide(skeletonGroup2.get(), option, &result)); + EXPECT_TRUE(bodyNodeGroup1->collide(bodyNodeGroup2.get(), option, &result)); + EXPECT_TRUE(shapeNodeGroup1->collide(shapeNodeGroup2.get(), option, &result)); // Binary check without passing option auto oldBinaryCheckOption = option.binaryCheck; From c86bbff159807f9216040aa54104df1cde741c5c Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 6 May 2016 17:09:43 -0400 Subject: [PATCH 03/15] Change CollisionResult parameter to optional pointer of DARTCollisionDetector --- dart/collision/dart/DARTCollide.cpp | 3 + dart/collision/dart/DARTCollisionDetector.cpp | 131 +++++++++++------- dart/collision/dart/DARTCollisionDetector.hpp | 13 +- 3 files changed, 96 insertions(+), 51 deletions(-) diff --git a/dart/collision/dart/DARTCollide.cpp b/dart/collision/dart/DARTCollide.cpp index 4c53683f1c300..4771287fb6aaa 100644 --- a/dart/collision/dart/DARTCollide.cpp +++ b/dart/collision/dart/DARTCollide.cpp @@ -1255,6 +1255,9 @@ int collideCylinderPlane(CollisionObject* o1, CollisionObject* o2, const double& //============================================================================== int collide(CollisionObject* o1, CollisionObject* o2, CollisionResult& result) { + // TODO(JS): We could make the contact point computation as optional for + // the case that we want only binary check. + const dynamics::ConstShapePtr& shape0 = o1->getShape(); const dynamics::ConstShapePtr& shape1 = o2->getShape(); const Eigen::Isometry3d& T0 = o1->getTransform(); diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp index 52062c1c22d7c..6eeb61c4fc32d 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -50,7 +50,7 @@ namespace collision { namespace { bool checkPair(CollisionObject* o1, CollisionObject* o2, - const CollisionOption& option, CollisionResult& result); + const CollisionOption& option, CollisionResult* result = nullptr); bool isClose(const Eigen::Vector3d& pos1, const Eigen::Vector3d& pos2, double tol); @@ -96,75 +96,109 @@ DARTCollisionDetector::createCollisionGroup() } //============================================================================== -bool DARTCollisionDetector::collide( - CollisionGroup* group, - const CollisionOption& option, CollisionResult& result) +static void clearCollisionResult( + const CollisionOption& option, CollisionResult* result) { - result.clear(); + if (!result) + { + if (!option.binaryCheck) + { + dtwarn << "[DARTCollisionDetector::collide] nullptr of CollisionResult is " + << "passed in when the binary check option is off. The collision " + << "detector will run narrowphase collision checking over all the " + << "feasible collision pairs without storing the contact " + << "information, which would be meaningless work. Also, it would " + << "ignore the maximum number of contacts since the number of " + << "contact will not be counted. Please consider either of using " + << "binary check or passsing CollisionResult pointer which is not " + << "a nullptr.\n"; + } - if (this != group->getCollisionDetector().get()) + return; + } + + result->clear(); +} + +//============================================================================== +static bool checkGroupValidity(DARTCollisionDetector* cd, CollisionGroup* group) +{ + if (cd != group->getCollisionDetector().get()) { - dterr << "[DARTCollisionDetector::detect] Attempting to check collision " + dterr << "[DARTCollisionDetector::collide] Attempting to check collision " << "for a collision group that is created from a different collision " << "detector instance.\n"; return false; } + return true; +} + +//============================================================================== +bool DARTCollisionDetector::collide( + CollisionGroup* group, + const CollisionOption& option, + CollisionResult* result) +{ + clearCollisionResult(option, result); + + if (!checkGroupValidity(this, group)) + return false; + + const auto& binaryCheck = option.binaryCheck; + auto casted = static_cast(group); const auto& objects = casted->mCollisionObjects; if (objects.empty()) return false; - auto done = false; const auto& filter = option.collisionFilter; for (auto i = 0u; i < objects.size() - 1; ++i) { - auto collObj1 = objects[i]; + auto* collObj1 = objects[i]; for (auto j = i + 1u; j < objects.size(); ++j) { - auto collObj2 = objects[j]; + auto* collObj2 = objects[j]; if (filter && !filter->needCollision(collObj1, collObj2)) continue; - checkPair(collObj1, collObj2, option, result); + auto collisionFound = checkPair(collObj1, collObj2, option, result); - if ((option.binaryCheck && result.isCollision()) - || (result.getNumContacts() >= option.maxNumContacts)) - { - done = true; - break; - } - } + if (binaryCheck && collisionFound) + return true; - if (done) - break; + if (result->getNumContacts() >= option.maxNumContacts) + return result->isCollision(); + } } - return result.isCollision(); + if (binaryCheck) + return false; + else + return result->isCollision(); } //============================================================================== bool DARTCollisionDetector::collide( CollisionGroup* group1, CollisionGroup* group2, - const CollisionOption& option, CollisionResult& result) + const CollisionOption& option, + CollisionResult* result) { - result.clear(); + clearCollisionResult(option, result); - if ((this != group1->getCollisionDetector().get()) - || (this != group2->getCollisionDetector().get())) - { - dterr << "[DARTCollisionDetector::detect] Attempting to check collision " - << "for a collision group that is created from a different collision " - << "detector instance.\n"; + if (!checkGroupValidity(this, group1)) + return false; + if (!checkGroupValidity(this, group2)) return false; - } + + const auto& binaryCheck = option.binaryCheck; auto casted1 = static_cast(group1); auto casted2 = static_cast(group2); @@ -175,34 +209,33 @@ bool DARTCollisionDetector::collide( if (objects1.empty() || objects2.empty()) return false; - auto done = false; const auto& filter = option.collisionFilter; for (auto i = 0u; i < objects1.size(); ++i) { - auto collObj1 = objects1[i]; + auto* collObj1 = objects1[i]; for (auto j = 0u; j < objects2.size(); ++j) { - auto collObj2 = objects2[j]; + auto* collObj2 = objects2[j]; if (filter && !filter->needCollision(collObj1, collObj2)) continue; - checkPair(collObj1, collObj2, option, result); + auto collisionFound = checkPair(collObj1, collObj2, option, result); - if (result.getNumContacts() >= option.maxNumContacts) - { - done = true; - break; - } - } + if (binaryCheck && collisionFound) + return true; - if (done) - break; + if (result->getNumContacts() >= option.maxNumContacts) + return result->isCollision(); + } } - return result.isCollision(); + if (binaryCheck) + return false; + else + return result->isCollision(); } //============================================================================== @@ -256,16 +289,20 @@ namespace { //============================================================================== bool checkPair(CollisionObject* o1, CollisionObject* o2, - const CollisionOption& option, CollisionResult& result) + const CollisionOption& option, CollisionResult* result) { CollisionResult pairResult; // Perform narrow-phase detection - auto colliding = collide(o1, o2, pairResult); + collide(o1, o2, pairResult); + + // Early return for binary check + if (!result) + return pairResult.isCollision(); - postProcess(o1, o2, option, result, pairResult); + postProcess(o1, o2, option, *result, pairResult); - return colliding != 0; + return pairResult.isCollision(); } //============================================================================== diff --git a/dart/collision/dart/DARTCollisionDetector.hpp b/dart/collision/dart/DARTCollisionDetector.hpp index 77b78e1ac1f13..77277215ee351 100644 --- a/dart/collision/dart/DARTCollisionDetector.hpp +++ b/dart/collision/dart/DARTCollisionDetector.hpp @@ -64,12 +64,17 @@ class DARTCollisionDetector : public CollisionDetector std::unique_ptr createCollisionGroup() override; // Documentation inherited - bool collide(CollisionGroup* group, - const CollisionOption& option, CollisionResult& result) override; + bool collide( + CollisionGroup* group, + const CollisionOption& option = CollisionOption(false, true, 1u, nullptr), + CollisionResult* result = nullptr) override; // Documentation inherited - bool collide(CollisionGroup* group1, CollisionGroup* group2, - const CollisionOption& option, CollisionResult& result) override; + bool collide( + CollisionGroup* group1, + CollisionGroup* group2, + const CollisionOption& option = CollisionOption(false, true, 1u, nullptr), + CollisionResult* result = nullptr) override; protected: From 045fa7ceb57cfbc7d4f01ae0d24a55fbd4975e88 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 6 May 2016 17:30:05 -0400 Subject: [PATCH 04/15] Change World::checkCollision(~) to take collision option and result params. --- dart/simulation/World.cpp | 12 ++++++++---- dart/simulation/World.hpp | 26 +++++++++++++++++++++----- unittests/testCollision.cpp | 8 ++++---- 3 files changed, 33 insertions(+), 13 deletions(-) diff --git a/dart/simulation/World.cpp b/dart/simulation/World.cpp index 77857f3ffa288..46b839356982d 100644 --- a/dart/simulation/World.cpp +++ b/dart/simulation/World.cpp @@ -496,13 +496,17 @@ std::set World::removeAllSimpleFrames() bool World::checkCollision(bool checkAllCollisions) { collision::CollisionOption option; + if (checkAllCollisions) - option.enableContact = true; - else - option.enableContact = false; + option.binaryCheck = false; - collision::CollisionResult result; + return checkCollision(option); +} +//============================================================================== +bool World::checkCollision(const collision::CollisionOption& option, + collision::CollisionResult* result) +{ return mConstraintSolver->getCollisionGroup()->collide(option, result); } diff --git a/dart/simulation/World.hpp b/dart/simulation/World.hpp index ae4c2f6cea312..4bf52a532d75a 100644 --- a/dart/simulation/World.hpp +++ b/dart/simulation/World.hpp @@ -52,9 +52,10 @@ #include "dart/common/Timer.hpp" #include "dart/common/NameManager.hpp" #include "dart/common/Subject.hpp" -#include "dart/simulation/Recording.hpp" #include "dart/dynamics/SimpleFrame.hpp" #include "dart/dynamics/Skeleton.hpp" +#include "dart/collision/Option.hpp" +#include "dart/simulation/Recording.hpp" namespace dart { @@ -172,12 +173,27 @@ class World : public virtual common::Subject std::set removeAllSimpleFrames(); //-------------------------------------------------------------------------- - // Kinematics + // Collision checking //-------------------------------------------------------------------------- - /// Return whether there is any collision between bodies - bool checkCollision(bool _checkAllCollisions = false); - + /// Deprecated. Please use checkCollision(~) instead. + DEPRECATED(6.0) + bool checkCollision(bool checkAllCollisions); + + /// Perform collision checking with 'option' over all the feasible collision + /// pairs in this World, and the result will be stored 'result'. If no + /// argument is passed in then it will return just whether there is collision + /// or not without the contact information such as contact point, normal, and + /// penetration depth. + bool checkCollision( + const collision::CollisionOption& option + = collision::CollisionOption(false, true, 1u, nullptr), + collision::CollisionResult* result = nullptr); + + /// Return the collision checking result of the last simulation step. If this + /// world hasn't stepped forward yet, then the result would be empty. Note + /// that this function does not return the collision checking result of + /// World::checkCollision(). const collision::CollisionResult& getLastCollisionResult() const; //-------------------------------------------------------------------------- diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index e6994a60906e6..b48ced147c7ff 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -761,10 +761,10 @@ void testOptions(const std::shared_ptr& cd) //============================================================================== TEST_F(COLLISION, Options) { -// auto fcl_mesh_dart = FCLCollisionDetector::create(); -// fcl_mesh_dart->setPrimitiveShapeType(FCLCollisionDetector::MESH); -// fcl_mesh_dart->setContactPointComputationMethod(FCLCollisionDetector::DART); -// testOptions(fcl_mesh_dart); + auto fcl_mesh_dart = FCLCollisionDetector::create(); + fcl_mesh_dart->setPrimitiveShapeType(FCLCollisionDetector::MESH); + fcl_mesh_dart->setContactPointComputationMethod(FCLCollisionDetector::DART); + testOptions(fcl_mesh_dart); // auto fcl_prim_fcl = FCLCollisionDetector::create(); // fcl_prim_fcl->setPrimitiveShapeType(FCLCollisionDetector::MESH); From 62fb94b131aad7cf8330774733f9bf105283f901 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 7 May 2016 19:22:45 -0400 Subject: [PATCH 05/15] Check if result is nullptr in DARTCollisionDetector::collide --- dart/collision/dart/DARTCollisionDetector.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp index 6eeb61c4fc32d..3fa48afb1198e 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -172,7 +172,7 @@ bool DARTCollisionDetector::collide( if (binaryCheck && collisionFound) return true; - if (result->getNumContacts() >= option.maxNumContacts) + if (result && result->getNumContacts() >= option.maxNumContacts) return result->isCollision(); } } @@ -227,7 +227,7 @@ bool DARTCollisionDetector::collide( if (binaryCheck && collisionFound) return true; - if (result->getNumContacts() >= option.maxNumContacts) + if (result && result->getNumContacts() >= option.maxNumContacts) return result->isCollision(); } } From b6b31836628449bd8aaa027aa9443b5520f07cd9 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 7 May 2016 20:12:44 -0400 Subject: [PATCH 06/15] Change CollisionResult parameter to optional pointer of BulletCollisionDetector --- .../bullet/BulletCollisionDetector.cpp | 167 +++++++++++++----- .../bullet/BulletCollisionDetector.hpp | 13 +- dart/collision/fcl/FCLCollisionDetector.cpp | 42 ++--- 3 files changed, 152 insertions(+), 70 deletions(-) diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index f9ef16e219dda..f0bbe40294937 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -63,28 +63,24 @@ namespace { struct BulletOverlapFilterCallback : public btOverlapFilterCallback { - BulletOverlapFilterCallback(const CollisionOption& option, const CollisionResult& result) - : mOption(option), - mResult(result), - mDone(false) + 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 + bool needBroadphaseCollision( + btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const override { - if (mDone) + if (done) return false; - if ((mOption.binaryCheck && mResult.isCollision()) - || (mResult.getNumContacts() >= mOption.maxNumContacts)) - { - mDone = true; - return false; - } - assert((proxy0 != nullptr && proxy1 != nullptr) && "Bullet broadphase overlapping pair proxies are nullptr"); @@ -93,7 +89,7 @@ struct BulletOverlapFilterCallback : public btOverlapFilterCallback collide = collide && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask); - const auto& filter = mOption.collisionFilter; + const auto& filter = option.collisionFilter; if (collide && filter) { @@ -114,22 +110,28 @@ struct BulletOverlapFilterCallback : public btOverlapFilterCallback return collide; } - const CollisionOption& mOption; - const CollisionResult& mResult; + 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 mDone; + mutable bool done; }; Contact convertContact(const btManifoldPoint& bulletManifoldPoint, const BulletCollisionObject::UserData* userData1, const BulletCollisionObject::UserData* userData2); -void convertContacts( - btCollisionWorld* collWorld, const CollisionOption& option, CollisionResult& result); +void convertContacts(btCollisionWorld* collWorld, + BulletOverlapFilterCallback* overlapFilterCallback, + const CollisionOption& option, + CollisionResult& result); -btCollisionShape* -createBulletEllipsoidMesh(float sizeX, float sizeY, float sizeZ); +btCollisionShape* createBulletEllipsoidMesh( + float sizeX, float sizeY, float sizeZ); btCollisionShape* createBulletCollisionShapeFromAssimpScene( const Eigen::Vector3d& scale, const aiScene* scene); @@ -181,51 +183,115 @@ BulletCollisionDetector::createCollisionGroup() } //============================================================================== -bool BulletCollisionDetector::collide( - CollisionGroup* group, const CollisionOption& option, CollisionResult& result) +static void clearCollisionResult( + const CollisionOption& option, CollisionResult* result) { - result.clear(); + if (!result) + { + if (!option.binaryCheck) + { + dtwarn << "[BulletCollisionDetector::collide] nullptr of CollisionResult " + << "is passed in when the binary check option is off. The " + << "collision detector will run narrowphase collision checking " + << "over all the feasible collision pairs without storing the " + << "contact information, which would be meaningless work. Also, " + << "it would ignore the maximum number of contacts since the " + << "number of contact will not be counted. Please consider either " + << "of using binary check or passsing CollisionResult pointer " + << "which is not a nullptr.\n"; + } - if (this != group->getCollisionDetector().get()) + return; + } + + result->clear(); +} + +//============================================================================== +static bool checkGroupValidity( + BulletCollisionDetector* cd, CollisionGroup* group) +{ + if (cd != group->getCollisionDetector().get()) { - dterr << "[BulletCollisionDetector::detect] Attempting to check collision " + dterr << "[BulletCollisionDetector::collide] Attempting to check collision " << "for a collision group that is created from a different collision " << "detector instance.\n"; return false; } + return true; +} + +//============================================================================== +static bool isCollision(btCollisionWorld* world) +{ + assert(world); + + auto dispatcher = world->getDispatcher(); + assert(dispatcher); + + const auto numManifolds = dispatcher->getNumManifolds(); + + for (auto i = 0; i < numManifolds; ++i) + { + const auto* contactManifold = dispatcher->getManifoldByIndexInternal(i); + + if (contactManifold->getNumContacts() > 0) + return true; + } + + return false; +} + +//============================================================================== +bool BulletCollisionDetector::collide( + CollisionGroup* group, + const CollisionOption& option, + CollisionResult* result) +{ + clearCollisionResult(option, result); + + if (!checkGroupValidity(this, group)) + return false; + auto castedData = static_cast(group); castedData->updateEngineData(); auto bulletCollisionWorld = castedData->getBulletCollisionWorld(); auto bulletPairCache = bulletCollisionWorld->getPairCache(); auto filterCallback = new BulletOverlapFilterCallback(option, result); +// auto dispatcher = static_cast( +// bulletCollisionWorld->getDispatcher()); +// dispatcher->setNearCallback(nullptr); bulletPairCache->setOverlapFilterCallback(filterCallback); bulletCollisionWorld->performDiscreteCollisionDetection(); - convertContacts(bulletCollisionWorld, option, result); + if (result) + { + convertContacts(bulletCollisionWorld, filterCallback, option, *result); - return result.isCollision(); + return result->isCollision(); + } + else + { + return isCollision(bulletCollisionWorld); + } } //============================================================================== bool BulletCollisionDetector::collide( CollisionGroup* group1, CollisionGroup* group2, - const CollisionOption& option, CollisionResult& result) + const CollisionOption& option, CollisionResult* result) { - result.clear(); + clearCollisionResult(option, result); - if ((this != group1->getCollisionDetector().get()) - || (this != group2->getCollisionDetector().get())) - { - dterr << "[BulletCollisionDetector::detect] Attempting to check collision " - << "for a collision group that is created from a different collision " - << "detector instance.\n"; + if (!checkGroupValidity(this, group1)) + return false; + if (!checkGroupValidity(this, group2)) return false; - } auto group = common::make_unique(shared_from_this()); group->addShapeFramesOf(group1, group2); @@ -238,9 +304,16 @@ bool BulletCollisionDetector::collide( bulletPairCache->setOverlapFilterCallback(filterCallback); bulletCollisionWorld->performDiscreteCollisionDetection(); - convertContacts(bulletCollisionWorld, option, result); + if (result) + { + convertContacts(bulletCollisionWorld, filterCallback, option, *result); - return result.isCollision(); + return result->isCollision(); + } + else + { + return isCollision(bulletCollisionWorld); + } } //============================================================================== @@ -455,7 +528,10 @@ Contact convertContact(const btManifoldPoint& bulletManifoldPoint, //============================================================================== void convertContacts( - btCollisionWorld* collWorld, const CollisionOption& option, CollisionResult& result) + btCollisionWorld* collWorld, + BulletOverlapFilterCallback* overlapFilterCallback, + const CollisionOption& option, + CollisionResult& result) { assert(collWorld); @@ -486,11 +562,12 @@ void convertContacts( result.addContact(convertContact(cp, userDataA, userDataB)); - if (option.binaryCheck) + if (option.binaryCheck + || result.getNumContacts() >= option.maxNumContacts) + { + overlapFilterCallback->done = true; return; - - if (result.getNumContacts() >= option.maxNumContacts) - break; + } } } } diff --git a/dart/collision/bullet/BulletCollisionDetector.hpp b/dart/collision/bullet/BulletCollisionDetector.hpp index d482abeebe962..14deeeed78da0 100644 --- a/dart/collision/bullet/BulletCollisionDetector.hpp +++ b/dart/collision/bullet/BulletCollisionDetector.hpp @@ -74,12 +74,17 @@ class BulletCollisionDetector : public CollisionDetector std::unique_ptr createCollisionGroup() override; // Documentation inherited - bool collide(CollisionGroup* group, - const CollisionOption& option, CollisionResult& result) override; + bool collide( + CollisionGroup* group, + const CollisionOption& option = CollisionOption(false, true, 1u, nullptr), + CollisionResult* result = nullptr) override; // Documentation inherited - bool collide(CollisionGroup* group1, CollisionGroup* group2, - const CollisionOption& option, CollisionResult& result) override; + bool collide( + CollisionGroup* group1, + CollisionGroup* group2, + const CollisionOption& option = CollisionOption(false, true, 1u, nullptr), + CollisionResult* result = nullptr) override; protected: diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 5a5b749c78bd2..0d418c943528a 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -132,33 +132,33 @@ Contact convertContact( struct FCLCollisionCallbackData { /// FCL collision request - fcl::CollisionRequest mFclRequest; + fcl::CollisionRequest fclRequest; /// FCL collision result - fcl::CollisionResult mFclResult; + fcl::CollisionResult fclResult; /// Collision option of DART - const CollisionOption& mOption; + const CollisionOption& option; /// Collision result of DART - CollisionResult* mResult; + 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; - FCLCollisionDetector::PrimitiveShape mPrimitiveShapeType; + FCLCollisionDetector::PrimitiveShape primitiveShapeType; FCLCollisionDetector::ContactPointComputationMethod - mContactPointComputationMethod; + contactPointComputationMethod; /// Whether the collision iteration can stop bool done; bool isCollision() const { - if (mResult) - return mResult->isCollision(); + if (result) + return result->isCollision(); else return foundCollision; } @@ -171,17 +171,17 @@ struct FCLCollisionCallbackData = FCLCollisionDetector::MESH, FCLCollisionDetector::ContactPointComputationMethod method = FCLCollisionDetector::DART) - : mOption(option), - mResult(result), + : option(option), + result(result), foundCollision(false), - mPrimitiveShapeType(type), - mContactPointComputationMethod(method), + primitiveShapeType(type), + contactPointComputationMethod(method), done(false) { - convertOption(mOption, mFclRequest); + convertOption(option, fclRequest); - mFclRequest.num_max_contacts = std::max(static_cast(100u), - mOption.maxNumContacts); + fclRequest.num_max_contacts = std::max(static_cast(100u), + option.maxNumContacts); // Since some contact points can be filtered out in the post process, we ask // more than the demend. 100 is randomly picked. } @@ -1007,10 +1007,10 @@ bool collisionCallback( if (collData->done) return true; - const auto& fclRequest = collData->mFclRequest; - auto& fclResult = collData->mFclResult; - auto* result = collData->mResult; - const auto& option = collData->mOption; + const auto& fclRequest = collData->fclRequest; + auto& fclResult = collData->fclResult; + auto* result = collData->result; + const auto& option = collData->option; const auto& filter = option.collisionFilter; const auto& binaryCheck = option.binaryCheck; @@ -1042,8 +1042,8 @@ bool collisionCallback( if (result) { // Post processing -- converting fcl contact information to ours if needed - if (FCLCollisionDetector::DART == collData->mContactPointComputationMethod - && FCLCollisionDetector::MESH == collData->mPrimitiveShapeType) + if (FCLCollisionDetector::DART == collData->contactPointComputationMethod + && FCLCollisionDetector::MESH == collData->primitiveShapeType) { postProcessDART(fclResult, o1, o2, option, *result); } From 4be219ef71ad805a20b7f75e18c30a76bd1a04dc Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 7 May 2016 21:17:14 -0400 Subject: [PATCH 07/15] Update use of collide functions --- tutorials/tutorialCollisions-Finished.cpp | 2 +- tutorials/tutorialDominoes-Finished.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/tutorials/tutorialCollisions-Finished.cpp b/tutorials/tutorialCollisions-Finished.cpp index 044e8736c42e4..f5eba4487c450 100644 --- a/tutorials/tutorialCollisions-Finished.cpp +++ b/tutorials/tutorialCollisions-Finished.cpp @@ -221,7 +221,7 @@ class MyWindow : public dart::gui::SimWindow dart::collision::CollisionOption option; dart::collision::CollisionResult result; - bool collision = collisionGroup->collide(newGroup.get(), option, result); + bool collision = collisionGroup->collide(newGroup.get(), option, &result); // If the new object is not in collision if(!collision) diff --git a/tutorials/tutorialDominoes-Finished.cpp b/tutorials/tutorialDominoes-Finished.cpp index 3e08077bbaeac..10e60b02890ad 100644 --- a/tutorials/tutorialDominoes-Finished.cpp +++ b/tutorials/tutorialDominoes-Finished.cpp @@ -272,7 +272,7 @@ class MyWindow : public dart::gui::SimWindow dart::collision::CollisionOption option; dart::collision::CollisionResult result; bool dominoCollision - = collisionGroup->collide(newGroup.get(), option, result); + = collisionGroup->collide(newGroup.get(), option, &result); // If the new domino is not penetrating an existing one if(!dominoCollision) From 08b5dc3868a55c9ad7dd8a26bc1b42eeacbc49ff Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sun, 8 May 2016 10:59:51 -0400 Subject: [PATCH 08/15] Remove Option::binaryCheck. Set Option::maxNumContacts == 1u instead. --- dart/collision/CollisionDetector.hpp | 4 +- dart/collision/CollisionGroup.hpp | 4 +- dart/collision/Option.cpp | 2 - dart/collision/Option.hpp | 21 ++-- .../bullet/BulletCollisionDetector.cpp | 40 ++----- .../bullet/BulletCollisionDetector.hpp | 4 +- dart/collision/dart/DARTCollisionDetector.cpp | 100 ++++++++---------- dart/collision/dart/DARTCollisionDetector.hpp | 4 +- dart/collision/fcl/FCLCollisionDetector.cpp | 54 ++++------ dart/collision/fcl/FCLCollisionDetector.hpp | 4 +- dart/constraint/ConstraintSolver.cpp | 2 +- dart/simulation/World.cpp | 4 +- dart/simulation/World.hpp | 2 +- unittests/testCollision.cpp | 23 ++-- 14 files changed, 115 insertions(+), 153 deletions(-) diff --git a/dart/collision/CollisionDetector.hpp b/dart/collision/CollisionDetector.hpp index 910169162170d..21f5ac2354103 100644 --- a/dart/collision/CollisionDetector.hpp +++ b/dart/collision/CollisionDetector.hpp @@ -103,7 +103,7 @@ class CollisionDetector : public std::enable_shared_from_this /// collision of not. virtual bool collide( CollisionGroup* group, - const CollisionOption& option = CollisionOption(false, true, 1u, nullptr), + const CollisionOption& option = CollisionOption(false, 1u, nullptr), CollisionResult* result = nullptr) = 0; /// Perform collision check for two groups. If nullptr is passed to @@ -112,7 +112,7 @@ class CollisionDetector : public std::enable_shared_from_this virtual bool collide( CollisionGroup* group1, CollisionGroup* group2, - const CollisionOption& option = CollisionOption(false, true, 1u, nullptr), + const CollisionOption& option = CollisionOption(false, 1u, nullptr), CollisionResult* result = nullptr) = 0; protected: diff --git a/dart/collision/CollisionGroup.hpp b/dart/collision/CollisionGroup.hpp index 2f83232554f28..979539772a6e6 100644 --- a/dart/collision/CollisionGroup.hpp +++ b/dart/collision/CollisionGroup.hpp @@ -173,7 +173,7 @@ class CollisionGroup /// Perform collision check within this CollisionGroup. bool collide( - const CollisionOption& option = CollisionOption(false, true, 1u, nullptr), + const CollisionOption& option = CollisionOption(false, 1u, nullptr), CollisionResult* result = nullptr); /// Perform collision check with other CollisionGroup. @@ -182,7 +182,7 @@ class CollisionGroup /// from this CollisionObject engine. bool collide( CollisionGroup* group, - const CollisionOption& option = CollisionOption(false, true, 1u, nullptr), + const CollisionOption& option = CollisionOption(false, 1u, nullptr), CollisionResult* result = nullptr); protected: diff --git a/dart/collision/Option.cpp b/dart/collision/Option.cpp index e473ac149d5f0..5818bed241d5c 100644 --- a/dart/collision/Option.cpp +++ b/dart/collision/Option.cpp @@ -42,11 +42,9 @@ namespace collision { //============================================================================== CollisionOption::CollisionOption( bool enableContact, - bool binaryCheck, std::size_t maxNumContacts, const std::shared_ptr& collisionFilter) : enableContact(enableContact), - binaryCheck(binaryCheck), maxNumContacts(maxNumContacts), collisionFilter(collisionFilter) { diff --git a/dart/collision/Option.hpp b/dart/collision/Option.hpp index b332c3c0c4390..1bd0fa705bf10 100644 --- a/dart/collision/Option.hpp +++ b/dart/collision/Option.hpp @@ -47,16 +47,17 @@ class CollisionFilter; struct CollisionOption { - /// Flag whether compute contact information such as point, normal, and - /// penetration depth. If this flag is set to false, the Engine returns only - /// simple information whether there is a collision of not. - bool enableContact; +public: - /// If true, the collision checking will terminate as soon as the first - /// contact is found. - bool binaryCheck; + /// Set whether the collision detector computes contact information (contact + /// point, normal, and penetration depth). If it is set to false, only the + /// result of that which pairs are colliding will be stored in the + /// CollisionResult without the contact information. + bool enableContact; - /// Maximum number of contacts to detect + /// Maximum number of contacts to detect. Once the contacts are found up to + /// this number, the collision checking will terminate at that moment. Set + /// this to 1 for binary check. std::size_t maxNumContacts; /// CollisionFilter @@ -65,9 +66,9 @@ struct CollisionOption /// Constructor CollisionOption( bool enableContact = true, - bool binaryCheck = false, - std::size_t maxNumContacts = 100, + std::size_t maxNumContacts = 1000u, const std::shared_ptr& collisionFilter = nullptr); + }; } // namespace collision diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index f0bbe40294937..ea856f3953135 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -182,31 +182,6 @@ BulletCollisionDetector::createCollisionGroup() return common::make_unique(shared_from_this()); } -//============================================================================== -static void clearCollisionResult( - const CollisionOption& option, CollisionResult* result) -{ - if (!result) - { - if (!option.binaryCheck) - { - dtwarn << "[BulletCollisionDetector::collide] nullptr of CollisionResult " - << "is passed in when the binary check option is off. The " - << "collision detector will run narrowphase collision checking " - << "over all the feasible collision pairs without storing the " - << "contact information, which would be meaningless work. Also, " - << "it would ignore the maximum number of contacts since the " - << "number of contact will not be counted. Please consider either " - << "of using binary check or passsing CollisionResult pointer " - << "which is not a nullptr.\n"; - } - - return; - } - - result->clear(); -} - //============================================================================== static bool checkGroupValidity( BulletCollisionDetector* cd, CollisionGroup* group) @@ -250,7 +225,11 @@ bool BulletCollisionDetector::collide( const CollisionOption& option, CollisionResult* result) { - clearCollisionResult(option, result); + if (result) + result->clear(); + + if (0u == option.maxNumContacts) + return false; if (!checkGroupValidity(this, group)) return false; @@ -285,7 +264,11 @@ bool BulletCollisionDetector::collide( CollisionGroup* group1, CollisionGroup* group2, const CollisionOption& option, CollisionResult* result) { - clearCollisionResult(option, result); + if (result) + result->clear(); + + if (0u == option.maxNumContacts) + return false; if (!checkGroupValidity(this, group1)) return false; @@ -562,8 +545,7 @@ void convertContacts( result.addContact(convertContact(cp, userDataA, userDataB)); - if (option.binaryCheck - || result.getNumContacts() >= option.maxNumContacts) + if (result.getNumContacts() >= option.maxNumContacts) { overlapFilterCallback->done = true; return; diff --git a/dart/collision/bullet/BulletCollisionDetector.hpp b/dart/collision/bullet/BulletCollisionDetector.hpp index 14deeeed78da0..3cc1ab258f4c9 100644 --- a/dart/collision/bullet/BulletCollisionDetector.hpp +++ b/dart/collision/bullet/BulletCollisionDetector.hpp @@ -76,14 +76,14 @@ class BulletCollisionDetector : public CollisionDetector // Documentation inherited bool collide( CollisionGroup* group, - const CollisionOption& option = CollisionOption(false, true, 1u, nullptr), + const CollisionOption& option = CollisionOption(false, 1u, nullptr), CollisionResult* result = nullptr) override; // Documentation inherited bool collide( CollisionGroup* group1, CollisionGroup* group2, - const CollisionOption& option = CollisionOption(false, true, 1u, nullptr), + const CollisionOption& option = CollisionOption(false, 1u, nullptr), CollisionResult* result = nullptr) override; protected: diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp index 3fa48afb1198e..aa743c263f156 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -95,31 +95,6 @@ DARTCollisionDetector::createCollisionGroup() return common::make_unique(shared_from_this()); } -//============================================================================== -static void clearCollisionResult( - const CollisionOption& option, CollisionResult* result) -{ - if (!result) - { - if (!option.binaryCheck) - { - dtwarn << "[DARTCollisionDetector::collide] nullptr of CollisionResult is " - << "passed in when the binary check option is off. The collision " - << "detector will run narrowphase collision checking over all the " - << "feasible collision pairs without storing the contact " - << "information, which would be meaningless work. Also, it would " - << "ignore the maximum number of contacts since the number of " - << "contact will not be counted. Please consider either of using " - << "binary check or passsing CollisionResult pointer which is not " - << "a nullptr.\n"; - } - - return; - } - - result->clear(); -} - //============================================================================== static bool checkGroupValidity(DARTCollisionDetector* cd, CollisionGroup* group) { @@ -141,12 +116,14 @@ bool DARTCollisionDetector::collide( const CollisionOption& option, CollisionResult* result) { - clearCollisionResult(option, result); + if (result) + result->clear(); - if (!checkGroupValidity(this, group)) + if (0u == option.maxNumContacts) return false; - const auto& binaryCheck = option.binaryCheck; + if (!checkGroupValidity(this, group)) + return false; auto casted = static_cast(group); const auto& objects = casted->mCollisionObjects; @@ -154,6 +131,7 @@ bool DARTCollisionDetector::collide( if (objects.empty()) return false; + auto collisionFound = false; const auto& filter = option.collisionFilter; for (auto i = 0u; i < objects.size() - 1; ++i) @@ -167,20 +145,24 @@ bool DARTCollisionDetector::collide( if (filter && !filter->needCollision(collObj1, collObj2)) continue; - auto collisionFound = checkPair(collObj1, collObj2, option, result); - - if (binaryCheck && collisionFound) - return true; + collisionFound = checkPair(collObj1, collObj2, option, result); - if (result && result->getNumContacts() >= option.maxNumContacts) - return result->isCollision(); + if (result) + { + if (result->getNumContacts() >= option.maxNumContacts) + return true; + } + else + { + // If no result is passed, stop checking when the first contact is found + if (collisionFound) + return true; + } } } - if (binaryCheck) - return false; - else - return result->isCollision(); + // Either no collision found or not reached the maximum number of contacts + return collisionFound; } //============================================================================== @@ -190,7 +172,11 @@ bool DARTCollisionDetector::collide( const CollisionOption& option, CollisionResult* result) { - clearCollisionResult(option, result); + if (result) + result->clear(); + + if (0u == option.maxNumContacts) + return false; if (!checkGroupValidity(this, group1)) return false; @@ -198,8 +184,6 @@ bool DARTCollisionDetector::collide( if (!checkGroupValidity(this, group2)) return false; - const auto& binaryCheck = option.binaryCheck; - auto casted1 = static_cast(group1); auto casted2 = static_cast(group2); @@ -209,6 +193,7 @@ bool DARTCollisionDetector::collide( if (objects1.empty() || objects2.empty()) return false; + auto collisionFound = false; const auto& filter = option.collisionFilter; for (auto i = 0u; i < objects1.size(); ++i) @@ -222,20 +207,24 @@ bool DARTCollisionDetector::collide( if (filter && !filter->needCollision(collObj1, collObj2)) continue; - auto collisionFound = checkPair(collObj1, collObj2, option, result); + collisionFound = checkPair(collObj1, collObj2, option, result); - if (binaryCheck && collisionFound) - return true; - - if (result && result->getNumContacts() >= option.maxNumContacts) - return result->isCollision(); + if (result) + { + if (result->getNumContacts() >= option.maxNumContacts) + return true; + } + else + { + // If no result is passed, stop checking when the first contact is found + if (collisionFound) + return true; + } } } - if (binaryCheck) - return false; - else - return result->isCollision(); + // Either no collision found or not reached the maximum number of contacts + return collisionFound; } //============================================================================== @@ -313,9 +302,11 @@ bool isClose(const Eigen::Vector3d& pos1, const Eigen::Vector3d& pos2, } //============================================================================== -void postProcess(CollisionObject* o1, CollisionObject* o2, +void postProcess(CollisionObject* o1, + CollisionObject* o2, const CollisionOption& option, - CollisionResult& totalResult, const CollisionResult& pairResult) + CollisionResult& totalResult, + const CollisionResult& pairResult) { if (!pairResult.isCollision()) return; @@ -344,9 +335,6 @@ void postProcess(CollisionObject* o1, CollisionObject* o2, contact.collisionObject2 = o2; totalResult.addContact(contact); - if (option.binaryCheck) - break; - if (totalResult.getNumContacts() >= option.maxNumContacts) break; } diff --git a/dart/collision/dart/DARTCollisionDetector.hpp b/dart/collision/dart/DARTCollisionDetector.hpp index 77277215ee351..ef379334aa62f 100644 --- a/dart/collision/dart/DARTCollisionDetector.hpp +++ b/dart/collision/dart/DARTCollisionDetector.hpp @@ -66,14 +66,14 @@ class DARTCollisionDetector : public CollisionDetector // Documentation inherited bool collide( CollisionGroup* group, - const CollisionOption& option = CollisionOption(false, true, 1u, nullptr), + const CollisionOption& option = CollisionOption(false, 1u, nullptr), CollisionResult* result = nullptr) override; // Documentation inherited bool collide( CollisionGroup* group1, CollisionGroup* group2, - const CollisionOption& option = CollisionOption(false, true, 1u, nullptr), + const CollisionOption& option = CollisionOption(false, 1u, nullptr), CollisionResult* result = nullptr) override; protected: diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 0d418c943528a..9a38b44f8130a 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -643,31 +643,6 @@ FCLCollisionDetector::createCollisionGroup() return common::make_unique(shared_from_this()); } -//============================================================================== -static void clearCollisionResult( - const CollisionOption& option, CollisionResult* result) -{ - if (!result) - { - if (!option.binaryCheck) - { - dtwarn << "[FCLCollisionDetector::collide] nullptr of CollisionResult is " - << "passed in when the binary check option is off. The collision " - << "detector will run narrowphase collision checking over all the " - << "feasible collision pairs without storing the contact " - << "information, which would be meaningless work. Also, it would " - << "ignore the maximum number of contacts since the number of " - << "contact will not be counted. Please consider either of using " - << "binary check or passsing CollisionResult pointer which is not " - << "a nullptr.\n"; - } - - return; - } - - result->clear(); -} - //============================================================================== static bool checkGroupValidity(FCLCollisionDetector* cd, CollisionGroup* group) { @@ -689,7 +664,11 @@ bool FCLCollisionDetector::collide( const CollisionOption& option, CollisionResult* result) { - clearCollisionResult(option, result); + if (result) + result->clear(); + + if (0u == option.maxNumContacts) + return false; if (!checkGroupValidity(this, group)) return false; @@ -711,7 +690,11 @@ bool FCLCollisionDetector::collide( CollisionGroup* group1, CollisionGroup* group2, const CollisionOption& option, CollisionResult* result) { - clearCollisionResult(option, result); + if (result) + result->clear(); + + if (0u == option.maxNumContacts) + return false; if (!checkGroupValidity(this, group1)) return false; @@ -1012,7 +995,6 @@ bool collisionCallback( auto* result = collData->result; const auto& option = collData->option; const auto& filter = option.collisionFilter; - const auto& binaryCheck = option.binaryCheck; // Filtering if (filter) @@ -1056,12 +1038,14 @@ bool collisionCallback( if (result->getNumContacts() >= option.maxNumContacts) collData->done = true; } - - // Check satisfaction of the stopping conditions - if (binaryCheck && fclResult.isCollision()) + else { - collData->foundCollision = true; - collData->done = true; + // If no result is passed, stop checking when the first contact is found + if (fclResult.isCollision()) + { + collData->foundCollision = true; + collData->done = true; + } } return collData->done; @@ -1169,7 +1153,7 @@ void postProcessFCL( // For binary check, return after adding the first contact point to the result // without the checkings of repeatidity and co-linearity. - if (option.binaryCheck) + if (1u == option.maxNumContacts) { result.addContact(convertContact(fclResult.getContact(0), o1, o2, option)); @@ -1275,7 +1259,7 @@ void postProcessDART( // For binary check, return after adding the first contact point to the result // without the checkings of repeatidity and co-linearity. - if (option.binaryCheck) + if (1u == option.maxNumContacts) { result.addContact(pair1); diff --git a/dart/collision/fcl/FCLCollisionDetector.hpp b/dart/collision/fcl/FCLCollisionDetector.hpp index 0f549431c0aeb..f7f90ab625e90 100644 --- a/dart/collision/fcl/FCLCollisionDetector.hpp +++ b/dart/collision/fcl/FCLCollisionDetector.hpp @@ -103,14 +103,14 @@ class FCLCollisionDetector : public CollisionDetector // Documentation inherited bool collide( CollisionGroup* group, - const CollisionOption& option = CollisionOption(false, true, 1u, nullptr), + const CollisionOption& option = CollisionOption(false, 1u, nullptr), CollisionResult* result = nullptr) override; // Documentation inherited bool collide( CollisionGroup* group1, CollisionGroup* group2, - const CollisionOption& option = CollisionOption(false, true, 1u, nullptr), + const CollisionOption& option = CollisionOption(false, 1u, nullptr), CollisionResult* result = nullptr) override; /// Set primitive shape type diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 12599ce656aec..cc3edca036e75 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -69,7 +69,7 @@ ConstraintSolver::ConstraintSolver(double timeStep) mCollisionGroup(mCollisionDetector->createCollisionGroupAsSharedPtr()), mCollisionOption( collision::CollisionOption( - true, false, 100, std::make_shared())), + true, 1000u, std::make_shared())), mTimeStep(timeStep), mLCPSolver(new DantzigLCPSolver(mTimeStep)) { diff --git a/dart/simulation/World.cpp b/dart/simulation/World.cpp index 46b839356982d..e66827dc63677 100644 --- a/dart/simulation/World.cpp +++ b/dart/simulation/World.cpp @@ -498,7 +498,9 @@ bool World::checkCollision(bool checkAllCollisions) collision::CollisionOption option; if (checkAllCollisions) - option.binaryCheck = false; + option.maxNumContacts = 1e+3; + else + option.maxNumContacts = 1u; return checkCollision(option); } diff --git a/dart/simulation/World.hpp b/dart/simulation/World.hpp index 4bf52a532d75a..a90d594977e4d 100644 --- a/dart/simulation/World.hpp +++ b/dart/simulation/World.hpp @@ -187,7 +187,7 @@ class World : public virtual common::Subject /// penetration depth. bool checkCollision( const collision::CollisionOption& option - = collision::CollisionOption(false, true, 1u, nullptr), + = collision::CollisionOption(false, 1u, nullptr), collision::CollisionResult* result = nullptr); /// Return the collision checking result of the last simulation step. If this diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index b48ced147c7ff..e6df88fe5a519 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -734,28 +734,35 @@ void testOptions(const std::shared_ptr& cd) result.clear(); option.maxNumContacts = 1000u; - option.binaryCheck = false; EXPECT_TRUE(group->collide(option, &result)); EXPECT_EQ(result.getNumContacts(), 4u); result.clear(); option.maxNumContacts = 2u; - option.binaryCheck = false; EXPECT_TRUE(group->collide(option, &result)); EXPECT_EQ(result.getNumContacts(), 2u); group->addShapeFrame(simpleFrame3.get()); result.clear(); - option.maxNumContacts = 1e+3; - option.binaryCheck = true; + option.maxNumContacts = 1u; EXPECT_TRUE(group->collide(option, &result)); EXPECT_EQ(result.getNumContacts(), 1u); - // Binary check without passing option + // Binary check without passing result EXPECT_TRUE(group->collide(option)); // Binary check without passing option and result EXPECT_TRUE(group->collide()); + + // Zero maximum number of contacts + option.maxNumContacts = 0u; + option.enableContact = true; + EXPECT_TRUE(group->collide()); + EXPECT_FALSE(group->collide(option)); + EXPECT_FALSE(group->collide(option, nullptr)); + EXPECT_FALSE(group->collide(option, &result)); + EXPECT_EQ(result.getNumContacts(), 0u); + EXPECT_FALSE(result.isCollision()); } //============================================================================== @@ -823,12 +830,12 @@ void testCreateCollisionGroups(const std::shared_ptr& cd) EXPECT_TRUE(shapeNodeGroup1->collide(shapeNodeGroup2.get(), option, &result)); // Binary check without passing option - auto oldBinaryCheckOption = option.binaryCheck; - option.binaryCheck = true; + auto oldMaxNumContacts = option.maxNumContacts; + option.maxNumContacts = 1u; EXPECT_TRUE(skeletonGroup1->collide(skeletonGroup2.get(), option)); EXPECT_TRUE(bodyNodeGroup1->collide(bodyNodeGroup2.get(), option)); EXPECT_TRUE(shapeNodeGroup1->collide(shapeNodeGroup2.get(), option)); - option.binaryCheck = oldBinaryCheckOption; + option.maxNumContacts = oldMaxNumContacts; // Binary check without passing option and result EXPECT_TRUE(skeletonGroup1->collide(skeletonGroup2.get())); From 93ab34b5922dbdeda1430307258199a88c5ca41b Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sun, 8 May 2016 11:42:22 -0400 Subject: [PATCH 09/15] Add test for collision filter -- failing with BulletCollisionDetector --- dart/collision/Option.hpp | 3 +- unittests/testCollision.cpp | 74 ++++++++++++++++++++++++++++++++++--- 2 files changed, 69 insertions(+), 8 deletions(-) diff --git a/dart/collision/Option.hpp b/dart/collision/Option.hpp index 1bd0fa705bf10..f648bd4e23a9a 100644 --- a/dart/collision/Option.hpp +++ b/dart/collision/Option.hpp @@ -47,9 +47,8 @@ class CollisionFilter; struct CollisionOption { -public: - /// Set whether the collision detector computes contact information (contact + /// Flag whether the collision detector computes contact information (contact /// point, normal, and penetration depth). If it is set to false, only the /// result of that which pairs are colliding will be stored in the /// CollisionResult without the contact information. diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index e6df88fe5a519..fc3dc8d1571e6 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -44,13 +44,8 @@ #include "dart/config.hpp" #include "dart/common/common.hpp" #include "dart/math/math.hpp" -#include "dart/collision/CollisionGroup.hpp" -#include "dart/collision/fcl/FCLCollisionDetector.hpp" -#include "dart/collision/dart/DARTCollisionDetector.hpp" -#if HAVE_BULLET_COLLISION - #include "dart/collision/bullet/BulletCollisionDetector.hpp" -#endif #include "dart/dynamics/dynamics.hpp" +#include "dart/collision/collision.hpp" #include "dart/simulation/simulation.hpp" #include "dart/utils/utils.hpp" #include "TestHelpers.hpp" @@ -797,6 +792,73 @@ TEST_F(COLLISION, Options) testOptions(dart); } +//============================================================================== +void testFilter(const std::shared_ptr& cd) +{ + // Create two bodies skeleton. The two bodies are placed at the same position + // with the same size shape so that they collide by default. + auto skel = Skeleton::create(); + auto shape = std::make_shared(Eigen::Vector3d(1, 1, 1)); + auto pair0 = skel->createJointAndBodyNodePair(nullptr); + auto* body0 = pair0.second; + body0->createShapeNodeWith(shape); + auto pair1 = body0->createChildJointAndBodyNodePair(); + auto* body1 = pair1.second; + body1->createShapeNodeWith(shape); + + // Create a collision group from the skeleton + auto group = cd->createCollisionGroup(skel.get()); + EXPECT_EQ(group->getNumShapeFrames(), 2u); + + // Default collision filter for Skeleton + CollisionOption option; + option.collisionFilter = std::make_shared(); + + skel->enableSelfCollision(true); + EXPECT_TRUE(group->collide()); // without filer, always collision + EXPECT_TRUE(group->collide(option)); + + skel->enableSelfCollision(false); + EXPECT_TRUE(group->collide()); + EXPECT_FALSE(group->collide(option)); + + skel->disableSelfCollision(); + EXPECT_TRUE(group->collide()); + EXPECT_FALSE(group->collide(option)); +} + +//============================================================================== +TEST_F(COLLISION, Filter) +{ + auto fcl_mesh_dart = FCLCollisionDetector::create(); + fcl_mesh_dart->setPrimitiveShapeType(FCLCollisionDetector::MESH); + fcl_mesh_dart->setContactPointComputationMethod(FCLCollisionDetector::DART); + testFilter(fcl_mesh_dart); + + // auto fcl_prim_fcl = FCLCollisionDetector::create(); + // fcl_prim_fcl->setPrimitiveShapeType(FCLCollisionDetector::MESH); + // fcl_prim_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL); + // testFilter(fcl_prim_fcl); + + // auto fcl_mesh_fcl = FCLCollisionDetector::create(); + // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE); + // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::DART); + // testFilter(fcl_mesh_fcl); + + // auto fcl_mesh_fcl = FCLCollisionDetector::create(); + // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE); + // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL); + // testFilter(fcl_mesh_fcl); + +#if HAVE_BULLET_COLLISION + auto bullet = BulletCollisionDetector::create(); + testFilter(bullet); +#endif + + auto dart = DARTCollisionDetector::create(); + testFilter(dart); +} + //============================================================================== void testCreateCollisionGroups(const std::shared_ptr& cd) { From 7299ba7105f730fc2bdd68c5f06528788639f46f Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sun, 8 May 2016 14:50:49 -0400 Subject: [PATCH 10/15] Workaround to enable collision filter for BulletCollisionDetector --- .../bullet/BulletCollisionDetector.cpp | 35 ++++++++++++------- 1 file changed, 23 insertions(+), 12 deletions(-) diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index ea856f3953135..1bfcabc3b2b2d 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -234,17 +234,26 @@ bool BulletCollisionDetector::collide( if (!checkGroupValidity(this, group)) return false; - auto castedData = static_cast(group); - castedData->updateEngineData(); - - auto bulletCollisionWorld = castedData->getBulletCollisionWorld(); + // 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 newGroup = common::make_unique(shared_from_this()); + auto bulletCollisionWorld = newGroup->getBulletCollisionWorld(); auto bulletPairCache = bulletCollisionWorld->getPairCache(); auto filterCallback = new BulletOverlapFilterCallback(option, result); -// auto dispatcher = static_cast( -// bulletCollisionWorld->getDispatcher()); -// dispatcher->setNearCallback(nullptr); - bulletPairCache->setOverlapFilterCallback(filterCallback); + + newGroup->addShapeFramesOf(group); + newGroup->updateEngineData(); + bulletCollisionWorld->performDiscreteCollisionDetection(); if (result) @@ -276,15 +285,17 @@ bool BulletCollisionDetector::collide( if (!checkGroupValidity(this, group2)) return false; - auto group = common::make_unique(shared_from_this()); - group->addShapeFramesOf(group1, group2); - group->updateEngineData(); + // TODO(JS): The collision checking does not distinguish the two groups. + auto group = common::make_unique(shared_from_this()); auto bulletCollisionWorld = group->getBulletCollisionWorld(); auto bulletPairCache = bulletCollisionWorld->getPairCache(); auto filterCallback = new BulletOverlapFilterCallback(option, result); - bulletPairCache->setOverlapFilterCallback(filterCallback); + + group->addShapeFramesOf(group1, group2); + group->updateEngineData(); + bulletCollisionWorld->performDiscreteCollisionDetection(); if (result) From 5bfe8d429205791586dbf4b3b973231f008e97e1 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sun, 8 May 2016 14:51:04 -0400 Subject: [PATCH 11/15] Add test of group-group collision checking --- .../collision/bullet/BulletCollisionGroup.cpp | 8 ++++++- .../collision/bullet/BulletCollisionGroup.hpp | 5 ++++- unittests/testCollision.cpp | 21 ++++++++++++++++++- 3 files changed, 31 insertions(+), 3 deletions(-) diff --git a/dart/collision/bullet/BulletCollisionGroup.cpp b/dart/collision/bullet/BulletCollisionGroup.cpp index 5910cfc0060dd..50bddd7704029 100644 --- a/dart/collision/bullet/BulletCollisionGroup.cpp +++ b/dart/collision/bullet/BulletCollisionGroup.cpp @@ -117,7 +117,13 @@ void BulletCollisionGroup::updateCollisionGroupEngineData() } //============================================================================== -btCollisionWorld* BulletCollisionGroup::getBulletCollisionWorld() const +btCollisionWorld* BulletCollisionGroup::getBulletCollisionWorld() +{ + return mBulletCollisionWorld.get(); +} + +//============================================================================== +const btCollisionWorld* BulletCollisionGroup::getBulletCollisionWorld() const { return mBulletCollisionWorld.get(); } diff --git a/dart/collision/bullet/BulletCollisionGroup.hpp b/dart/collision/bullet/BulletCollisionGroup.hpp index f18e937ea62b3..64521cb0dc23d 100644 --- a/dart/collision/bullet/BulletCollisionGroup.hpp +++ b/dart/collision/bullet/BulletCollisionGroup.hpp @@ -81,7 +81,10 @@ class BulletCollisionGroup : public CollisionGroup void updateCollisionGroupEngineData() override; /// Return Bullet collision world - btCollisionWorld* getBulletCollisionWorld() const; + btCollisionWorld* getBulletCollisionWorld(); + + /// Return Bullet collision world + const btCollisionWorld* getBulletCollisionWorld() const; protected: diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index fc3dc8d1571e6..48c18c61218d4 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -567,6 +567,25 @@ void testSimpleFrames(const std::shared_ptr& cd) EXPECT_TRUE(group1->collide(group2.get(), option, &result)); EXPECT_TRUE(group2->collide(group3.get(), option, &result)); EXPECT_TRUE(groupAll->collide(option, &result)); + + auto group23 = cd->createCollisionGroup( + simpleFrame2.get(), simpleFrame3.get()); + simpleFrame1->setTranslation(Eigen::Vector3d::Zero()); + simpleFrame2->setTranslation(Eigen::Vector3d(1.1, 0.0, 0.0)); + simpleFrame3->setTranslation(Eigen::Vector3d(1.6, 0.0, 0.0)); + EXPECT_FALSE(group1->collide(group2.get())); + EXPECT_FALSE(group1->collide(group3.get())); + EXPECT_TRUE(group2->collide(group3.get())); + EXPECT_TRUE(group23->collide()); + if (cd->getType() == BulletCollisionDetector::getStaticType()) + { + dtwarn << "Skipping group-group test for 'bullet' collision detector. " + << "Please see Issue #717 for the detail.\n"; + } + else + { + EXPECT_FALSE(group1->collide(group23.get())); + } } //============================================================================== @@ -815,7 +834,7 @@ void testFilter(const std::shared_ptr& cd) option.collisionFilter = std::make_shared(); skel->enableSelfCollision(true); - EXPECT_TRUE(group->collide()); // without filer, always collision + EXPECT_TRUE(group->collide()); // without filter, always collision EXPECT_TRUE(group->collide(option)); skel->enableSelfCollision(false); From 6a318a7bfc9e7a64f1d152f63a1da98c3a327e52 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 9 May 2016 15:25:00 -0400 Subject: [PATCH 12/15] Suppress warning by not using deprecated API in testCollision.cpp --- unittests/testCollision.cpp | 22 +++++++++++++++++++--- 1 file changed, 19 insertions(+), 3 deletions(-) diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index 47b6f46f5379e..206b1bda850ac 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -833,15 +833,31 @@ void testFilter(const std::shared_ptr& cd) CollisionOption option; option.collisionFilter = std::make_shared(); - skel->enableSelfCollision(true); + skel->enableSelfCollisionCheck(); + skel->enableAdjacentBodyCheck(); + EXPECT_TRUE(skel->isEnabledSelfCollisionCheck()); + EXPECT_TRUE(skel->isEnabledAdjacentBodyCheck()); EXPECT_TRUE(group->collide()); // without filter, always collision EXPECT_TRUE(group->collide(option)); - skel->enableSelfCollision(false); + skel->enableSelfCollisionCheck(); + skel->disableAdjacentBodyCheck(); + EXPECT_TRUE(skel->isEnabledSelfCollisionCheck()); + EXPECT_FALSE(skel->isEnabledAdjacentBodyCheck()); EXPECT_TRUE(group->collide()); EXPECT_FALSE(group->collide(option)); - skel->disableSelfCollision(); + skel->disableSelfCollisionCheck(); + skel->enableAdjacentBodyCheck(); + EXPECT_FALSE(skel->isEnabledSelfCollisionCheck()); + EXPECT_TRUE(skel->isEnabledAdjacentBodyCheck()); + EXPECT_TRUE(group->collide()); + EXPECT_FALSE(group->collide(option)); + + skel->disableSelfCollisionCheck(); + skel->disableAdjacentBodyCheck(); + EXPECT_FALSE(skel->isEnabledSelfCollisionCheck()); + EXPECT_FALSE(skel->isEnabledAdjacentBodyCheck()); EXPECT_TRUE(group->collide()); EXPECT_FALSE(group->collide(option)); } From 584d942e27d3b0f86665ad4e19c27a457a657c71 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 9 May 2016 23:26:36 -0400 Subject: [PATCH 13/15] Add a warning to BulletCollisionDetector::collide(group1, group2) --- dart/collision/bullet/BulletCollisionDetector.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index 1bfcabc3b2b2d..139c2c9e71f04 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -285,7 +285,12 @@ bool BulletCollisionDetector::collide( if (!checkGroupValidity(this, group2)) return false; - // TODO(JS): The collision checking does not distinguish the two groups. + dtwarn << "[BulletCollisionDetector::collide] collide(group1, group2) " + << "supposed to check collisions of the objects in group1 against the " + << "objects in group2. However, the current implementation of this " + << "function checks for all the objects against each other of both " + << "group1 and group2, which is an incorrect behavior. This bug will " + << "be fixed in the next patch release. (see #717 for the details)\n"; auto group = common::make_unique(shared_from_this()); auto bulletCollisionWorld = group->getBulletCollisionWorld(); From fe08fec3e6bdff235b0660725121ea82279d10dc Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 10 May 2016 11:40:23 -0400 Subject: [PATCH 14/15] Fix segfault of BulletCollisionDetector on Mac --- .../bullet/BulletCollisionDetector.cpp | 20 +++++++++---------- .../bullet/BulletCollisionDetector.hpp | 3 +++ dart/constraint/ConstraintSolver.cpp | 16 +++++++-------- 3 files changed, 20 insertions(+), 19 deletions(-) diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index 139c2c9e71f04..f43692b81d06c 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -245,14 +245,14 @@ bool BulletCollisionDetector::collide( // btCollisionWorld to generate unnecessary pairs, which is very inefficient // way. - auto newGroup = common::make_unique(shared_from_this()); - auto bulletCollisionWorld = newGroup->getBulletCollisionWorld(); + mGroupForFiltering.reset(new BulletCollisionGroup(shared_from_this())); + auto bulletCollisionWorld = mGroupForFiltering->getBulletCollisionWorld(); auto bulletPairCache = bulletCollisionWorld->getPairCache(); auto filterCallback = new BulletOverlapFilterCallback(option, result); bulletPairCache->setOverlapFilterCallback(filterCallback); - newGroup->addShapeFramesOf(group); - newGroup->updateEngineData(); + mGroupForFiltering->addShapeFramesOf(group); + mGroupForFiltering->updateEngineData(); bulletCollisionWorld->performDiscreteCollisionDetection(); @@ -292,14 +292,14 @@ bool BulletCollisionDetector::collide( << "group1 and group2, which is an incorrect behavior. This bug will " << "be fixed in the next patch release. (see #717 for the details)\n"; - auto group = common::make_unique(shared_from_this()); - auto bulletCollisionWorld = group->getBulletCollisionWorld(); + mGroupForFiltering.reset(new BulletCollisionGroup(shared_from_this())); + auto bulletCollisionWorld = mGroupForFiltering->getBulletCollisionWorld(); auto bulletPairCache = bulletCollisionWorld->getPairCache(); auto filterCallback = new BulletOverlapFilterCallback(option, result); bulletPairCache->setOverlapFilterCallback(filterCallback); - group->addShapeFramesOf(group1, group2); - group->updateEngineData(); + mGroupForFiltering->addShapeFramesOf(group1, group2); + mGroupForFiltering->updateEngineData(); bulletCollisionWorld->performDiscreteCollisionDetection(); @@ -549,9 +549,9 @@ void convertContacts( auto userPointer1 = bulletCollObj1->getUserPointer(); auto userDataA - = static_cast(userPointer1); - auto userDataB = static_cast(userPointer0); + auto userDataB + = static_cast(userPointer1); auto numContacts = contactManifold->getNumContacts(); diff --git a/dart/collision/bullet/BulletCollisionDetector.hpp b/dart/collision/bullet/BulletCollisionDetector.hpp index 3cc1ab258f4c9..666acb94f3a59 100644 --- a/dart/collision/bullet/BulletCollisionDetector.hpp +++ b/dart/collision/bullet/BulletCollisionDetector.hpp @@ -44,6 +44,7 @@ #include #include #include "dart/collision/CollisionDetector.hpp" +#include "dart/collision/bullet/BulletCollisionGroup.hpp" namespace dart { namespace collision { @@ -114,6 +115,8 @@ class BulletCollisionDetector : public CollisionDetector std::map> mShapeMap; + std::unique_ptr mGroupForFiltering; + }; } // namespace collision diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index cc3edca036e75..f9b5fc3a4f186 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -96,20 +96,18 @@ void ConstraintSolver::addSkeleton(const SkeletonPtr& skeleton) assert(skeleton && "Null pointer skeleton is now allowed to add to ConstraintSover."); - if (!containSkeleton(skeleton)) - { - auto group = mCollisionDetector->createCollisionGroup(skeleton.get()); - mCollisionGroup->addShapeFramesOf(group.get()); - - mSkeletons.push_back(skeleton); - mConstrainedGroups.reserve(mSkeletons.size()); - } - else + if (containSkeleton(skeleton)) { dtwarn << "[ConstraintSolver::addSkeleton] Attempting to add " << "skeleton '" << skeleton->getName() << "', which already exists in the ConstraintSolver.\n"; + + return; } + + mCollisionGroup->addShapeFramesOf(skeleton.get()); + mSkeletons.push_back(skeleton); + mConstrainedGroups.reserve(mSkeletons.size()); } //============================================================================== From 863151c2d1f0370d959201578969b47b82d83945 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 10 May 2016 11:43:58 -0400 Subject: [PATCH 15/15] Update minor code style in ConstraintSolver::removeSkeleton(~) --- dart/constraint/ConstraintSolver.cpp | 16 ++++++---------- 1 file changed, 6 insertions(+), 10 deletions(-) diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index f9b5fc3a4f186..5a1b904212eb8 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -123,21 +123,17 @@ void ConstraintSolver::removeSkeleton(const SkeletonPtr& skeleton) assert(skeleton && "Null pointer skeleton is now allowed to add to ConstraintSover."); - if (containSkeleton(skeleton)) - { - auto group = mCollisionDetector->createCollisionGroup(skeleton.get()); - mCollisionGroup->removeShapeFramesOf(group.get()); - - mSkeletons.erase(remove(mSkeletons.begin(), mSkeletons.end(), skeleton), - mSkeletons.end()); - mConstrainedGroups.reserve(mSkeletons.size()); - } - else + if (!containSkeleton(skeleton)) { dtwarn << "[ConstraintSolver::removeSkeleton] Attempting to remove " << "skeleton '" << skeleton->getName() << "', which doesn't exist in the ConstraintSolver.\n"; } + + mCollisionGroup->removeShapeFramesOf(skeleton.get()); + mSkeletons.erase(remove(mSkeletons.begin(), mSkeletons.end(), skeleton), + mSkeletons.end()); + mConstrainedGroups.reserve(mSkeletons.size()); } //==============================================================================