diff --git a/trunk/fcl/include/fcl/collision_data.h b/trunk/fcl/include/fcl/collision_data.h index 0457a500851556e4fcf4268b802d0d3d64d57b87..585f5231df77776757cd75a0b5fba72ec693e21d 100644 --- a/trunk/fcl/include/fcl/collision_data.h +++ b/trunk/fcl/include/fcl/collision_data.h @@ -87,18 +87,15 @@ struct CostSource struct CollisionRequest { - bool exhaustive; size_t num_max_contacts; bool enable_contact; size_t num_max_cost_sources; bool enable_cost; - CollisionRequest(bool exhaustive_ = false, - size_t num_max_contacts_ = 1, + CollisionRequest(size_t num_max_contacts_ = 1, bool enable_contact_ = false, size_t num_max_cost_sources_ = 1, - bool enable_cost_ = false) : exhaustive(exhaustive_), - num_max_contacts(num_max_contacts_), + bool enable_cost_ = false) : num_max_contacts(num_max_contacts_), enable_contact(enable_contact_), num_max_cost_sources(num_max_cost_sources_), enable_cost(enable_cost_) diff --git a/trunk/fcl/include/fcl/traversal_node_bvh_shape.h b/trunk/fcl/include/fcl/traversal_node_bvh_shape.h index 186ecb41e65b942fd9c0b3f0cceed98dd1411388..116a3ac672f6f2fc335860b9400d6d61fea92feb 100644 --- a/trunk/fcl/include/fcl/traversal_node_bvh_shape.h +++ b/trunk/fcl/include/fcl/traversal_node_bvh_shape.h @@ -219,7 +219,7 @@ public: bool canStop() const { - return (!this->request.exhaustive) && (!this->request.enable_cost) && (this->result->isCollision()) && (this->request.num_max_contacts <= this->result->numContacts()); + return (!this->request.enable_cost) && (this->result->isCollision()) && (this->request.num_max_contacts <= this->result->numContacts()); } Vec3f* vertices; @@ -469,7 +469,7 @@ public: bool canStop() const { - return (!this->request.exhaustive) && (!this->request.enable_cost) && (this->result->isCollision()) && (this->request.num_max_contacts <= this->result->numContacts()); + return (!this->request.enable_cost) && (this->result->isCollision()) && (this->request.num_max_contacts <= this->result->numContacts()); } Vec3f* vertices; diff --git a/trunk/fcl/include/fcl/traversal_node_bvhs.h b/trunk/fcl/include/fcl/traversal_node_bvhs.h index 38c3439ebea4991d8aa5660c4f43e76c42fefaf9..6b5d2fea2e52618fb96aabb7a76d3cbf3e62d32e 100644 --- a/trunk/fcl/include/fcl/traversal_node_bvhs.h +++ b/trunk/fcl/include/fcl/traversal_node_bvhs.h @@ -201,11 +201,9 @@ public: &normal)) { is_intersect = true; - if(!this->request.exhaustive) - { - if(this->request.num_max_contacts < n_contacts + this->result->numContacts()) - n_contacts = (this->request.num_max_contacts >= this->result->numContacts()) ? (this->request.num_max_contacts - this->result->numContacts()) : 0; - } + + if(this->request.num_max_contacts < n_contacts + this->result->numContacts()) + n_contacts = (this->request.num_max_contacts >= this->result->numContacts()) ? (this->request.num_max_contacts - this->result->numContacts()) : 0; for(unsigned int i = 0; i < n_contacts; ++i) { @@ -235,7 +233,7 @@ public: /** \brief Whether the traversal process can stop early */ bool canStop() const { - return (!this->request.exhaustive) && (!this->request.enable_cost) && (this->result->isCollision()) && (this->request.num_max_contacts <= this->result->numContacts()); + return (!this->request.enable_cost) && (this->result->isCollision()) && (this->request.num_max_contacts <= this->result->numContacts()); } Vec3f* vertices1; @@ -388,7 +386,7 @@ public: bool canStop() const { - return (pairs.size() > 0) && (!this->request.exhaustive) && (this->request.num_max_contacts <= pairs.size()); + return (pairs.size() > 0) && (this->request.num_max_contacts <= pairs.size()); } Vec3f* vertices1; @@ -486,7 +484,7 @@ public: bool canStop() const { - return (pairs.size() > 0) && (!this->request.exhaustive) && (this->request.num_max_contacts <= pairs.size()); + return (pairs.size() > 0) && (this->request.num_max_contacts <= pairs.size()); } Vec3f* vertices1; @@ -651,7 +649,7 @@ public: bool canStop() const { - return (pairs.size() > 0) && (!this->request.exhaustive) && (this->request.num_max_contacts <= pairs.size()); + return (pairs.size() > 0) && (this->request.num_max_contacts <= pairs.size()); } Vec3f* vertices1; @@ -736,7 +734,7 @@ public: bool canStop() const { - return (pairs.size() > 0) && (!this->request.exhaustive) && (this->request.num_max_contacts <= pairs.size()); + return (pairs.size() > 0) && (this->request.num_max_contacts <= pairs.size()); } Vec3f* vertices1; @@ -819,7 +817,7 @@ public: bool canStop() const { - return (pairs.size() > 0) && (!this->request.exhaustive) && (this->request.num_max_contacts <= pairs.size()); + return (pairs.size() > 0) && (this->request.num_max_contacts <= pairs.size()); } Vec3f* vertices1; diff --git a/trunk/fcl/include/fcl/traversal_node_octree.h b/trunk/fcl/include/fcl/traversal_node_octree.h index 9a81823fb4dbbbafc0f8a705743e3dafb6861a77..3748dfa48505f9d332cdf351c02decc01e3fde0f 100644 --- a/trunk/fcl/include/fcl/traversal_node_octree.h +++ b/trunk/fcl/include/fcl/traversal_node_octree.h @@ -325,7 +325,7 @@ private: cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * s.cost_density)); } - return (!crequest->exhaustive) && (!crequest->enable_cost) && (cresult->isCollision()) && (crequest->num_max_contacts <= cresult->numContacts()); + return (!crequest->enable_cost) && (cresult->isCollision()) && (crequest->num_max_contacts <= cresult->numContacts()); } else return false; } @@ -528,7 +528,7 @@ private: cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * tree2->cost_density)); } - return (!crequest->exhaustive) && (!crequest->enable_cost) && (cresult->isCollision()) && (crequest->num_max_contacts <= cresult->numContacts()); + return (!crequest->enable_cost) && (cresult->isCollision()) && (crequest->num_max_contacts <= cresult->numContacts()); } else return false; @@ -743,7 +743,7 @@ private: cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * root2->getOccupancy())); } - return (!crequest->exhaustive) && (!crequest->enable_cost) && (cresult->isCollision()) && (crequest->num_max_contacts <= cresult->numContacts()); + return (!crequest->enable_cost) && (cresult->isCollision()) && (crequest->num_max_contacts <= cresult->numContacts()); } else if(!tree1->isNodeFree(root1) && !tree2->isNodeFree(root2) && crequest->enable_cost) // uncertain area (here means both are uncertain or one uncertain and one occupied) { diff --git a/trunk/fcl/src/broad_phase_collision.cpp b/trunk/fcl/src/broad_phase_collision.cpp index ea6d13a1df7e89f6762af5a177d5a688724d7edf..0b65397f4a1f4ed78a4b7da83eea7f70b328f60d 100644 --- a/trunk/fcl/src/broad_phase_collision.cpp +++ b/trunk/fcl/src/broad_phase_collision.cpp @@ -59,7 +59,7 @@ bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cd collide(o1, o2, request, result); - if( (!request.exhaustive) && (result.isCollision()) && (result.numContacts() >= request.num_max_contacts)) + if( (result.isCollision()) && (result.numContacts() >= request.num_max_contacts)) cdata->done = true; return cdata->done; diff --git a/trunk/fcl/src/collision.cpp b/trunk/fcl/src/collision.cpp index ed42447e35173a197415dc149ca89e0cfd018f0b..42f7016a7963a84145543055aa2d72208d0f2b05 100644 --- a/trunk/fcl/src/collision.cpp +++ b/trunk/fcl/src/collision.cpp @@ -75,7 +75,7 @@ int collide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionFunctionMatrix<NarrowPhaseSolver>& looktable = getCollisionFunctionLookTable<NarrowPhaseSolver>(); int res; - if(request.num_max_contacts == 0 && !request.exhaustive) + if(request.num_max_contacts == 0) { std::cerr << "Warning: should stop early as num_max_contact is " << request.num_max_contacts << " !" << std::endl; res = 0; diff --git a/trunk/fcl/src/conservative_advancement.cpp b/trunk/fcl/src/conservative_advancement.cpp index dcad4af3dacc05f8321a6d07a3fcab3c6c258ffa..44a0d9317d15d2dba9323ce792b28de5b89302f1 100644 --- a/trunk/fcl/src/conservative_advancement.cpp +++ b/trunk/fcl/src/conservative_advancement.cpp @@ -55,7 +55,7 @@ int conservativeAdvancement(const CollisionGeometry* o1, CollisionResult& result, FCL_REAL& toc) { - if((request.num_max_contacts == 0) && !request.exhaustive) + if(request.num_max_contacts == 0) { std::cerr << "Warning: should stop early as num_max_contact is " << request.num_max_contacts << " !" << std::endl; return 0; diff --git a/trunk/fcl/src/traversal_node_bvhs.cpp b/trunk/fcl/src/traversal_node_bvhs.cpp index e85457c7ad18118cdeaff1755cefc09d4556607a..4488c2668406c1053a1d086498dc45da56b66c8a 100644 --- a/trunk/fcl/src/traversal_node_bvhs.cpp +++ b/trunk/fcl/src/traversal_node_bvhs.cpp @@ -100,12 +100,10 @@ static inline void meshCollisionOrientedNodeLeafTesting(int b1, int b2, &normal)) { is_intersect = true; - if(!request.exhaustive) - { - if(request.num_max_contacts < result.numContacts() + n_contacts) - n_contacts = (request.num_max_contacts > result.numContacts()) ? (request.num_max_contacts - result.numContacts()) : 0; - } - + + if(request.num_max_contacts < result.numContacts() + n_contacts) + n_contacts = (request.num_max_contacts > result.numContacts()) ? (request.num_max_contacts - result.numContacts()) : 0; + for(unsigned int i = 0; i < n_contacts; ++i) { result.addContact(Contact(model1, model2, primitive_id1, primitive_id2, tf1.transform(contacts[i]), tf1.getQuatRotation().transform(normal), penetration));