From 3440c17576741f6a3f6f0fab7b721792acabc726 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 25 Sep 2014 17:45:12 -0400 Subject: [PATCH 1/2] Enable adjacent body check option for bullet collision detector --- dart/collision/CollisionDetector.cpp | 47 ++++++++++++++--- dart/collision/CollisionDetector.h | 15 ++++-- .../bullet/BulletCollisionDetector.cpp | 51 ++++++++----------- 3 files changed, 73 insertions(+), 40 deletions(-) diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp index 76b4b0195dd53..c17af0e5f1fbb 100644 --- a/dart/collision/CollisionDetector.cpp +++ b/dart/collision/CollisionDetector.cpp @@ -215,7 +215,7 @@ void CollisionDetector::enablePair(dynamics::BodyNode* _node1, CollisionNode* collisionNode1 = getCollisionNode(_node1); CollisionNode* collisionNode2 = getCollisionNode(_node2); if (collisionNode1 && collisionNode2) - getPairCollidable(collisionNode1, collisionNode2) = true; + setPairCollidable(collisionNode1, collisionNode2, true); } void CollisionDetector::disablePair(dynamics::BodyNode* _node1, @@ -223,7 +223,7 @@ void CollisionDetector::disablePair(dynamics::BodyNode* _node1, CollisionNode* collisionNode1 = getCollisionNode(_node1); CollisionNode* collisionNode2 = getCollisionNode(_node2); if (collisionNode1 && collisionNode2) - getPairCollidable(collisionNode1, collisionNode2) = false; + setPairCollidable(collisionNode1, collisionNode2, false); } //============================================================================== @@ -271,16 +271,51 @@ bool CollisionDetector::containSkeleton(const dynamics::Skeleton* _skeleton) return false; } -std::vector::reference CollisionDetector::getPairCollidable( - const CollisionNode* _node1, const CollisionNode* _node2) { +bool CollisionDetector::getPairCollidable(const CollisionNode* _node1, + const CollisionNode* _node2) +{ assert(_node1 != _node2); - int index1 = _node1->getIndex(); - int index2 = _node2->getIndex(); + + size_t index1 = _node1->getIndex(); + size_t index2 = _node2->getIndex(); + if (index1 < index2) std::swap(index1, index2); + + // TODO(JS): Workaround + // If this fuction is called before all the body nodes in the world are not + // added to the collision detector than it cause seg fault. We just return + // false in that case. + if (index1 > mCollidablePairs.size() - 1 + || index2 > mCollidablePairs.size() - 1) + return false; + return mCollidablePairs[index1][index2]; } +void CollisionDetector::setPairCollidable(const CollisionNode* _node1, + const CollisionNode* _node2, + bool _val) +{ + assert(_node1 != _node2); + + size_t index1 = _node1->getIndex(); + size_t index2 = _node2->getIndex(); + + if (index1 < index2) + std::swap(index1, index2); + + // TODO(JS): Workaround + // If this fuction is called before all the body nodes in the world are not + // added to the collision detector than it cause seg fault. We just return in + // that case. + if (index1 > mCollidablePairs.size() - 1 + || index2 > mCollidablePairs.size() - 1) + return; + + mCollidablePairs[index1][index2] = _val; +} + bool CollisionDetector::isAdjacentBodies(const dynamics::BodyNode* _bodyNode1, const dynamics::BodyNode* _bodyNode2) { diff --git a/dart/collision/CollisionDetector.h b/dart/collision/CollisionDetector.h index c931bdcfebbc5..d1c3d4c79bed9 100644 --- a/dart/collision/CollisionDetector.h +++ b/dart/collision/CollisionDetector.h @@ -162,14 +162,14 @@ class CollisionDetector /// \brief void setNumMaxContacs(int _num); + /// \brief + bool isCollidable(const CollisionNode* _node1, const CollisionNode* _node2); + protected: /// \brief virtual bool detectCollision(CollisionNode* _node1, CollisionNode* _node2, bool _calculateContactPoints) = 0; - /// \brief - bool isCollidable(const CollisionNode* _node1, const CollisionNode* _node2); - /// \brief std::vector mContacts; @@ -187,8 +187,13 @@ class CollisionDetector bool containSkeleton(const dynamics::Skeleton* _skeleton); /// \brief - std::vector::reference getPairCollidable(const CollisionNode* _node1, - const CollisionNode* _node2); + bool getPairCollidable(const CollisionNode* _node1, + const CollisionNode* _node2); + + /// \brief + void setPairCollidable(const CollisionNode* _node1, + const CollisionNode* _node2, + bool _val); /// \brief Return true if _bodyNode1 and _bodyNode2 are adjacent bodies bool isAdjacentBodies(const dynamics::BodyNode* _bodyNode1, diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index 8ced062dcd57b..90caacf6a51db 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -78,35 +78,28 @@ struct CollisionFilter : public btOverlapFilterCallback collide = collide && (_proxy1->m_collisionFilterGroup & _proxy0->m_collisionFilterMask); - btCollisionObject* collObj0 = - static_cast(_proxy0->m_clientObject); - btCollisionObject* collObj1 = - static_cast(_proxy1->m_clientObject); - - BulletUserData* userData0 = - static_cast(collObj0->getUserPointer()); - BulletUserData* userData1 = - static_cast(collObj1->getUserPointer()); - -// if (!userData0->btCollDet->isCollidable(userData0->btCollNode, -// userData1->btCollNode)) { -// std::cout<< "false.\n"; -// return false; -// } - - if (userData0->bodyNode == userData1->bodyNode) - return false; - - if (!userData0->bodyNode->isCollidable()) - return false; - - if (!userData1->bodyNode->isCollidable()) - return false; - - if (userData0->bodyNode->getSkeleton() == - userData1->bodyNode->getSkeleton()) - if (!userData0->bodyNode->getSkeleton()->isEnabledSelfCollisionCheck()) - return false; + if (collide) + { + btCollisionObject* collObj0 = + static_cast(_proxy0->m_clientObject); + btCollisionObject* collObj1 = + static_cast(_proxy1->m_clientObject); + + BulletUserData* userData0 = + static_cast(collObj0->getUserPointer()); + BulletUserData* userData1 = + static_cast(collObj1->getUserPointer()); + + // Assume single collision detector + assert(userData0->btCollDet == userData1->btCollDet); + + CollisionDetector* cd = userData0->btCollDet; + + CollisionNode* cn1 = userData0->btCollNode; + CollisionNode* cn2 = userData1->btCollNode; + + collide = cd->isCollidable(cn1, cn2); + } return collide; } From 1e77d7c0ca7c7931563fb927fbb6f0b971ca374f Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 25 Sep 2014 17:58:27 -0400 Subject: [PATCH 2/2] Return true for colliable pair check of not added to the collision detector yet --- dart/collision/CollisionDetector.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp index c17af0e5f1fbb..f64d7b81f563d 100644 --- a/dart/collision/CollisionDetector.cpp +++ b/dart/collision/CollisionDetector.cpp @@ -288,7 +288,7 @@ bool CollisionDetector::getPairCollidable(const CollisionNode* _node1, // false in that case. if (index1 > mCollidablePairs.size() - 1 || index2 > mCollidablePairs.size() - 1) - return false; + return true; return mCollidablePairs[index1][index2]; }