Skip to content

Commit

Permalink
Showing 3 changed files with 73 additions and 40 deletions.
47 changes: 41 additions & 6 deletions dart/collision/CollisionDetector.cpp
Original file line number Diff line number Diff line change
@@ -215,15 +215,15 @@ 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,
dynamics::BodyNode* _node2) {
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<bool>::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 true;

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)
{
15 changes: 10 additions & 5 deletions dart/collision/CollisionDetector.h
Original file line number Diff line number Diff line change
@@ -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<Contact> mContacts;

@@ -187,8 +187,13 @@ class CollisionDetector
bool containSkeleton(const dynamics::Skeleton* _skeleton);

/// \brief
std::vector<bool>::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,
51 changes: 22 additions & 29 deletions dart/collision/bullet/BulletCollisionDetector.cpp
Original file line number Diff line number Diff line change
@@ -78,35 +78,28 @@ struct CollisionFilter : public btOverlapFilterCallback
collide = collide && (_proxy1->m_collisionFilterGroup &
_proxy0->m_collisionFilterMask);

btCollisionObject* collObj0 =
static_cast<btCollisionObject*>(_proxy0->m_clientObject);
btCollisionObject* collObj1 =
static_cast<btCollisionObject*>(_proxy1->m_clientObject);

BulletUserData* userData0 =
static_cast<BulletUserData*>(collObj0->getUserPointer());
BulletUserData* userData1 =
static_cast<BulletUserData*>(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<btCollisionObject*>(_proxy0->m_clientObject);
btCollisionObject* collObj1 =
static_cast<btCollisionObject*>(_proxy1->m_clientObject);

BulletUserData* userData0 =
static_cast<BulletUserData*>(collObj0->getUserPointer());
BulletUserData* userData1 =
static_cast<BulletUserData*>(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;
}

0 comments on commit 7bec658

Please sign in to comment.