//============================================================================== 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; }