//==============================================================================
bool FCLCollisionDetector::detectCollision(bool /*_checkAllCollisions*/,
bool _calculateContactPoints)
{
// TODO(JS): _checkAllCollisions
clearAllContacts();
// Set all the body nodes are not in colliding
for (size_t i = 0; i < mCollisionNodes.size(); i++)
mCollisionNodes[i]->getBodyNode()->setColliding(false);
// Update all the transformations of the collision nodes
for (auto& collNode : mCollisionNodes)
static_cast<FCLCollisionNode*>(collNode)->updateFCLCollisionObjects();
mBroadPhaseAlg->update();
CollisionData collData;
collData.request.enable_contact = _calculateContactPoints;
// TODO: Uncomment below once we strict to use fcl 0.3.0 or greater
// collData.request.gjk_solver_type = fcl::GST_LIBCCD;
collData.request.num_max_contacts = getNumMaxContacts();
collData.collisionDetector = this;
// Perform broad-phase collision detection. Narrow-phase collision detection
// will be handled by the collision callback function.
mBroadPhaseAlg->collide(&collData, collisionCallBack);
const size_t numContacts = collData.result.numContacts();
for (size_t m = 0; m < numContacts; ++m)
{
const fcl::Contact& contact = collData.result.getContact(m);
Eigen::Vector3d point = FCLTypes::convertVector3(contact.pos);
if (hasClosePoint(mContacts, point))
continue;
Contact contactPair;
contactPair.point = point;
contactPair.normal = -FCLTypes::convertVector3(contact.normal);
contactPair.bodyNode1 = findCollisionNode(contact.o1)->getBodyNode();
contactPair.bodyNode2 = findCollisionNode(contact.o2)->getBodyNode();
contactPair.triID1 = contact.b1;
contactPair.triID2 = contact.b2;
contactPair.penetrationDepth = contact.penetration_depth;
assert(contactPair.bodyNode1.lock());
assert(contactPair.bodyNode2.lock());
mContacts.push_back(contactPair);
}
for (size_t i = 0; i < mContacts.size(); ++i)
{
// Set these two bodies are in colliding
mContacts[i].bodyNode1.lock()->setColliding(true);
mContacts[i].bodyNode2.lock()->setColliding(true);
}
return !mContacts.empty();
}
CollisionNode *SceneGraph::findCollisionNode(char *name) {
if (!name || strlen(name) <= 0)
return NULL;
for (CollisionNode *node = findCollisionNode(); node; node = node->nextTraversal()) {
const char *nodeName = node->getName();
if (nodeName && strlen(nodeName)) {
if (!strcmp(name, nodeName))
return node;
}
}
return NULL;
}
