diff --git a/include/hpp/fcl/collision_data.h b/include/hpp/fcl/collision_data.h index 1f69995cca72d9accbb22c7aeea87fc0384048ed..081dbc617e3e13adb6960af7c892e827a96a02cb 100644 --- a/include/hpp/fcl/collision_data.h +++ b/include/hpp/fcl/collision_data.h @@ -131,6 +131,14 @@ struct Contact struct CollisionResult; +/// @brief flag declaration for specifying required params in CollisionResult +enum CollisionRequestFlag +{ + CONTACT = 0x00001, + DISTANCE_LOWER_BOUND = 0x00002, + NO_REQUEST = 0x01000 +}; + /// @brief request to the collision algorithm struct CollisionRequest { @@ -158,7 +166,7 @@ struct CollisionRequest /// @brief Distance below which bounding volumes are break down FCL_REAL break_distance; - CollisionRequest(size_t num_max_contacts_ = 1, + CollisionRequest(size_t num_max_contacts_, bool enable_contact_ = false, bool enable_distance_lower_bound_ = false, size_t num_max_cost_sources_ = 1, @@ -167,11 +175,10 @@ struct CollisionRequest GJKSolverType gjk_solver_type_ = GST_INDEP) HPP_FCL_DEPRECATED; - CollisionRequest(bool enable_contact_, size_t num_max_contacts_, - bool enable_distance_lower_bound_) : + explicit CollisionRequest(const CollisionRequestFlag flag, size_t num_max_contacts_) : num_max_contacts(num_max_contacts_), - enable_contact(enable_contact_), - enable_distance_lower_bound (enable_distance_lower_bound_), + enable_contact(flag & CONTACT), + enable_distance_lower_bound (flag & DISTANCE_LOWER_BOUND), gjk_solver_type(GST_INDEP), security_margin (0), break_distance (1e-3) @@ -180,6 +187,18 @@ struct CollisionRequest cached_gjk_guess = Vec3f(1, 0, 0); } + CollisionRequest() : + num_max_contacts(1), + enable_contact(false), + enable_distance_lower_bound (false), + gjk_solver_type(GST_INDEP), + security_margin (0), + break_distance (1e-3) + { + enable_cached_gjk_guess = false; + cached_gjk_guess = Vec3f(1, 0, 0); + } + bool isSatisfied(const CollisionResult& result) const; }; @@ -414,6 +433,28 @@ public: }; + +inline CollisionRequestFlag operator~(CollisionRequestFlag a) +{return static_cast<CollisionRequestFlag>(~static_cast<const int>(a));} + +inline CollisionRequestFlag operator|(CollisionRequestFlag a, CollisionRequestFlag b) +{return static_cast<CollisionRequestFlag>(static_cast<const int>(a) | static_cast<const int>(b));} + +inline CollisionRequestFlag operator&(CollisionRequestFlag a, CollisionRequestFlag b) +{return static_cast<CollisionRequestFlag>(static_cast<const int>(a) & static_cast<const int>(b));} + +inline CollisionRequestFlag operator^(CollisionRequestFlag a, CollisionRequestFlag b) +{return static_cast<CollisionRequestFlag>(static_cast<const int>(a) ^ static_cast<const int>(b));} + +inline CollisionRequestFlag& operator|=(CollisionRequestFlag& a, CollisionRequestFlag b) +{return (CollisionRequestFlag&)((int&)(a) |= static_cast<const int>(b));} + +inline CollisionRequestFlag& operator&=(CollisionRequestFlag& a, CollisionRequestFlag b) +{return (CollisionRequestFlag&)((int&)(a) &= static_cast<const int>(b));} + +inline CollisionRequestFlag& operator^=(CollisionRequestFlag& a, CollisionRequestFlag b) +{return (CollisionRequestFlag&)((int&)(a) ^= static_cast<const int>(b));} + } #endif diff --git a/test/benchmark.cpp b/test/benchmark.cpp index eae31f235e24fc86c8316cab8f11af7be1a625f2..ffe08ad79e6caad782f3055f4df3f405bc250273 100644 --- a/test/benchmark.cpp +++ b/test/benchmark.cpp @@ -117,7 +117,7 @@ double collide (const std::vector<Transform3f>& tf, Transform3f pose2; CollisionResult local_result; - CollisionRequest request (false, 1, false); + CollisionRequest request; TraversalNode node (request); node.enable_statistics = verbose; diff --git a/test/test_fcl_collision.cpp b/test/test_fcl_collision.cpp index 7159a12ae49d8bb363ae1be9476c436e122a7baa..63b173dc10ad763847dba98d8445a506011339da 100644 --- a/test/test_fcl_collision.cpp +++ b/test/test_fcl_collision.cpp @@ -75,7 +75,6 @@ bool test_collide_func(const Transform3f& tf, const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method); int num_max_contacts = std::numeric_limits<int>::max(); -bool enable_contact = true; std::vector<Contact> global_pairs; std::vector<Contact> global_pairs_now; @@ -855,7 +854,7 @@ bool collide_Test2(const Transform3f& tf, Transform3f pose1, pose2; CollisionResult local_result; - CollisionRequest request (enable_contact, num_max_contacts, false); + CollisionRequest request (CONTACT, num_max_contacts); MeshCollisionTraversalNode<BV> node (request); bool success = initialize <BV> (node, m1, pose1, m2, pose2, local_result); @@ -915,7 +914,7 @@ bool collide_Test(const Transform3f& tf, Transform3f pose1(tf), pose2; CollisionResult local_result; - CollisionRequest request (enable_contact, num_max_contacts, false); + CollisionRequest request (CONTACT, num_max_contacts); MeshCollisionTraversalNode<BV> node (request); bool success = initialize <BV> (node, m1, pose1, m2, pose2, local_result); @@ -974,7 +973,7 @@ bool collide_Test_Oriented(const Transform3f& tf, Transform3f pose1(tf), pose2; CollisionResult local_result; - CollisionRequest request (enable_contact, num_max_contacts, false); + CollisionRequest request (CONTACT, num_max_contacts); TraversalNode node (request); bool success = initialize (node, (const BVHModel<BV>&)m1, pose1, (const BVHModel<BV>&)m2, pose2, local_result); @@ -1033,7 +1032,7 @@ bool test_collide_func(const Transform3f& tf, std::vector<Contact> contacts; - CollisionRequest request (enable_contact, num_max_contacts, false); + CollisionRequest request (CONTACT, num_max_contacts); CollisionResult result; int num_contacts = collide(&m1, pose1, &m2, pose2, request, result); diff --git a/test/test_fcl_distance.cpp b/test/test_fcl_distance.cpp index 891f091efa539e8cb3f191194fc8c6cba10c5191..c9019c7c368ddf8cdd7a11421daf5fa37be74dba 100644 --- a/test/test_fcl_distance.cpp +++ b/test/test_fcl_distance.cpp @@ -413,7 +413,7 @@ bool collide_Test_OBB(const Transform3f& tf, m2.endModel(); CollisionResult local_result; - CollisionRequest request (true, 1, true); + CollisionRequest request (CONTACT | DISTANCE_LOWER_BOUND, 1); MeshCollisionTraversalNodeOBB node (request); bool success (initialize(node, (const BVHModel<OBB>&)m1, tf, (const BVHModel<OBB>&)m2, Transform3f(), diff --git a/test/test_fcl_distance_lower_bound.cpp b/test/test_fcl_distance_lower_bound.cpp index df1547b85757321545bd181de9853dd83ce8dd1e..a64e1a4a5141756b8dd3f0335681ffb8831ed43d 100644 --- a/test/test_fcl_distance_lower_bound.cpp +++ b/test/test_fcl_distance_lower_bound.cpp @@ -68,7 +68,7 @@ bool testDistanceLowerBound (const Transform3f& tf, { Transform3f pose1(tf), pose2; - CollisionRequest request (false, 1, false); + CollisionRequest request (fcl::NO_REQUEST, 1); request.enable_distance_lower_bound = true; CollisionResult result; @@ -86,7 +86,7 @@ bool testCollide (const Transform3f& tf, const CollisionGeometryPtr_t& m1, { Transform3f pose1(tf), pose2; - CollisionRequest request (false, 1, false); + CollisionRequest request (fcl::NO_REQUEST, 1); request.enable_distance_lower_bound = false; CollisionResult result; diff --git a/test/test_fcl_frontlist.cpp b/test/test_fcl_frontlist.cpp index 7845d40b68dff381d51d1d214965c21f4652d841..ea7a50e8124eed879d99848d75cfd3ae311cb7f4 100644 --- a/test/test_fcl_frontlist.cpp +++ b/test/test_fcl_frontlist.cpp @@ -229,7 +229,7 @@ bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2, Transform3f pose1, pose2; CollisionResult local_result; - CollisionRequest request (false, std::numeric_limits<int>::max()); + CollisionRequest request (NO_REQUEST, std::numeric_limits<int>::max()); MeshCollisionTraversalNode<BV> node (request); bool success = initialize <BV>(node, m1, pose1, m2, pose2, local_result); @@ -292,7 +292,7 @@ bool collide_front_list_Test_Oriented(const Transform3f& tf1, const Transform3f& Transform3f pose1(tf1), pose2; CollisionResult local_result; - CollisionRequest request (false, std::numeric_limits<int>::max()); + CollisionRequest request (NO_REQUEST, std::numeric_limits<int>::max()); TraversalNode node (request); bool success = initialize (node, (const BVHModel<BV>&)m1, pose1, @@ -343,7 +343,7 @@ bool collide_Test(const Transform3f& tf, Transform3f pose1(tf), pose2; CollisionResult local_result; - CollisionRequest request (false, std::numeric_limits<int>::max()); + CollisionRequest request (NO_REQUEST, std::numeric_limits<int>::max()); MeshCollisionTraversalNode<BV> node (request); bool success = initialize <BV>(node, m1, pose1, m2, pose2, local_result); diff --git a/test/test_fcl_octree.cpp b/test/test_fcl_octree.cpp index c8e64aba299809064677779e43d89c0490fb0e34..89c9c28ac52f4067283c428520a3c3dbb0a991ba 100644 --- a/test/test_fcl_octree.cpp +++ b/test/test_fcl_octree.cpp @@ -80,7 +80,7 @@ fcl::OcTree makeOctree (const BVHModel <OBBRSS>& mesh, Vec3f m (mesh.aabb_local.min_); Vec3f M (mesh.aabb_local.max_); fcl::Box box (resolution, resolution, resolution); - CollisionRequest request (true, 1, true); + CollisionRequest request (fcl::CONTACT | fcl::DISTANCE_LOWER_BOUND, 1); CollisionResult result; Transform3f tfBox; octomap::OcTreePtr_t octree (new octomap::OcTree (resolution)); @@ -138,7 +138,7 @@ BOOST_AUTO_TEST_CASE (OCTREE) generateRandomTransforms(extents, transforms, 2*N); - CollisionRequest request (true, 1, true); + CollisionRequest request (fcl::CONTACT | fcl::DISTANCE_LOWER_BOUND, 1); for (std::size_t i=0; i<N; ++i) { CollisionResult resultMesh; CollisionResult resultOctree; diff --git a/test/test_fcl_profiling.cpp b/test/test_fcl_profiling.cpp index 1a2f1e1bcdd13af33ff7b2be93f82d38f3b459e3..cc86d9fe761da66ad0bc547aafad163be6ca44b5 100644 --- a/test/test_fcl_profiling.cpp +++ b/test/test_fcl_profiling.cpp @@ -228,7 +228,7 @@ int main(int argc, char** argv) std::vector<Transform3f> transforms; - CollisionRequest request (false, 1, false); + CollisionRequest request; if (argc > 1) { int iarg = 1; diff --git a/test/test_fcl_utility.h b/test/test_fcl_utility.h index c1c29882855f1e1e9f41af56da62e0196535813a..0c2285cb1bec14102fdcb0f9128e23b582e778fa 100644 --- a/test/test_fcl_utility.h +++ b/test/test_fcl_utility.h @@ -120,7 +120,7 @@ struct DistanceRes /// @brief Collision data stores the collision request and the result given by collision algorithm. struct CollisionData { -CollisionData() : request (false, 1, false) +CollisionData() : request (NO_REQUEST, 1) { done = false; }