Commit 6732f696 authored by Florent Lamiraux's avatar Florent Lamiraux Committed by Florent Lamiraux florent@laas.fr
Browse files

Add constructor with less parameters in CollisionRequest

  - old constructor is deprecated.
parent ba6bd8a3
......@@ -164,11 +164,15 @@ struct CollisionRequest
size_t num_max_cost_sources_ = 1,
bool enable_cost_ = false,
bool use_approximate_cost_ = true,
GJKSolverType gjk_solver_type_ = GST_INDEP) :
num_max_contacts(num_max_contacts_),
GJKSolverType gjk_solver_type_ = GST_INDEP)
HPP_FCL_DEPRECATED;
CollisionRequest(bool enable_contact_, size_t num_max_contacts_,
bool enable_distance_lower_bound_) :
num_max_contacts(num_max_contacts_),
enable_contact(enable_contact_),
enable_distance_lower_bound (enable_distance_lower_bound_),
gjk_solver_type(gjk_solver_type_),
gjk_solver_type(GST_INDEP),
security_margin (0),
break_distance (1e-3)
{
......
......@@ -50,4 +50,19 @@ bool DistanceRequest::isSatisfied(const DistanceResult& result) const
return (result.min_distance <= 0);
}
CollisionRequest::CollisionRequest
(size_t num_max_contacts_, bool enable_contact_,
bool enable_distance_lower_bound_, size_t /*num_max_cost_sources_*/,
bool /*enable_cost_*/, bool /*use_approximate_cost_*/,
GJKSolverType gjk_solver_type_) :
num_max_contacts(num_max_contacts_),
enable_contact(enable_contact_),
enable_distance_lower_bound (enable_distance_lower_bound_),
gjk_solver_type(gjk_solver_type_),
security_margin (0),
break_distance (1e-3)
{
enable_cached_gjk_guess = false;
cached_gjk_guess = Vec3f(1, 0, 0);
}
}
......@@ -117,8 +117,8 @@ double collide (const std::vector<Transform3f>& tf,
Transform3f pose2;
CollisionResult local_result;
CollisionRequest request;
TraversalNode node(false);
CollisionRequest request (false, 1, false);
TraversalNode node (request);
node.enable_statistics = verbose;
......@@ -129,7 +129,6 @@ double collide (const std::vector<Transform3f>& tf,
bool success (initialize(node, m1, tf[i], m2, pose2, local_result));
assert (success);
CollisionRequest request (1, true, true);
CollisionResult result;
collide(&node, request, result);
}
......
......@@ -36,8 +36,7 @@ int main(int argc, char** argv)
static const int num_max_contacts = std::numeric_limits<int>::max();
static const bool enable_contact = true;
fcl::CollisionResult result;
fcl::CollisionRequest request(num_max_contacts,
enable_contact);
fcl::CollisionRequest request(enable_contact, num_max_contacts, false);
CollisionObject co0(box0, tf0);
......@@ -51,4 +50,4 @@ int main(int argc, char** argv)
BOOST_FOREACH(Contact& contact, contacts) {
cout << "position: " << contact.pos << endl;
}
}
\ No newline at end of file
}
......@@ -833,7 +833,7 @@ bool collide_Test2(const Transform3f& tf,
Transform3f pose1, pose2;
CollisionResult local_result;
CollisionRequest request (num_max_contacts, enable_contact);
CollisionRequest request (enable_contact, num_max_contacts, false);
MeshCollisionTraversalNode<BV> node (request);
bool success = initialize <BV> (node, m1, pose1, m2, pose2, local_result);
......@@ -891,7 +891,7 @@ bool collide_Test(const Transform3f& tf,
Transform3f pose1(tf), pose2;
CollisionResult local_result;
CollisionRequest request (num_max_contacts, enable_contact);
CollisionRequest request (enable_contact, num_max_contacts, false);
MeshCollisionTraversalNode<BV> node (request);
bool success = initialize <BV> (node, m1, pose1, m2, pose2, local_result);
......@@ -948,7 +948,7 @@ bool collide_Test_Oriented(const Transform3f& tf,
Transform3f pose1(tf), pose2;
CollisionResult local_result;
CollisionRequest request (num_max_contacts, enable_contact);
CollisionRequest request (enable_contact, num_max_contacts, false);
TraversalNode node (request);
bool success = initialize (node, (const BVHModel<BV>&)m1, pose1,
(const BVHModel<BV>&)m2, pose2, local_result);
......@@ -1007,7 +1007,7 @@ bool test_collide_func(const Transform3f& tf,
std::vector<Contact> contacts;
CollisionRequest request(num_max_contacts, enable_contact);
CollisionRequest request (enable_contact, num_max_contacts, false);
CollisionResult result;
int num_contacts = collide(&m1, pose1, &m2, pose2,
request, result);
......
......@@ -413,7 +413,7 @@ bool collide_Test_OBB(const Transform3f& tf,
m2.endModel();
CollisionResult local_result;
CollisionRequest request (1, true, true);
CollisionRequest request (true, 1, true);
MeshCollisionTraversalNodeOBB node (request);
bool success (initialize(node, (const BVHModel<OBB>&)m1, tf,
(const BVHModel<OBB>&)m2, Transform3f(),
......
......@@ -68,7 +68,7 @@ bool testDistanceLowerBound (const Transform3f& tf,
{
Transform3f pose1(tf), pose2;
CollisionRequest request;
CollisionRequest request (false, 1, false);
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;
CollisionRequest request (false, 1, false);
request.enable_distance_lower_bound = false;
CollisionResult result;
......@@ -141,7 +141,6 @@ BOOST_AUTO_TEST_CASE(mesh_mesh)
std::vector<Transform3f> transforms;
FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
std::size_t n = 100;
bool verbose = false;
generateRandomTransforms(extents, transforms, n);
......
......@@ -229,7 +229,7 @@ bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2,
Transform3f pose1, pose2;
CollisionResult local_result;
CollisionRequest request (std::numeric_limits<int>::max(), false);
CollisionRequest request (false, 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 (std::numeric_limits<int>::max(), false);
CollisionRequest request (false, 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 (std::numeric_limits<int>::max(), false);
CollisionRequest request (false, std::numeric_limits<int>::max());
MeshCollisionTraversalNode<BV> node (request);
bool success = initialize <BV>(node, m1, pose1, m2, pose2, local_result);
......
......@@ -228,14 +228,7 @@ int main(int argc, char** argv)
std::vector<Transform3f> transforms;
CollisionRequest request;
// request.num_max_contacts = 1;
// request.enable_contact = false;
// request.enable_distance_lower_bound = false;
// request.num_max_cost_sources = 1;
// request.enable_cost = false;
// request.use_approximate_cost = true;
// request.gjk_solver_type = GST_INDEP;
CollisionRequest request (false, 1, false);
if (argc > 1) {
int iarg = 1;
......
......@@ -808,7 +808,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_libccd)
generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
generateBVHModel(s2_obb, s2, Transform3f(), 16, 16);
CollisionRequest request;
CollisionRequest request (false, 1, false);
CollisionResult result;
bool res;
......@@ -1026,7 +1026,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_libccd)
generateBVHModel(s1_obb, s1, Transform3f());
generateBVHModel(s2_obb, s2, Transform3f());
CollisionRequest request;
CollisionRequest request (false, 1, false);
CollisionResult result;
bool res;
......@@ -1146,7 +1146,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_libccd)
generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
generateBVHModel(s2_obb, s2, Transform3f());
CollisionRequest request;
CollisionRequest request (false, 1, false);
CollisionResult result;
bool res;
......@@ -1266,7 +1266,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder_libccd)
generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
generateBVHModel(s2_obb, s2, Transform3f(), 16, 16);
CollisionRequest request;
CollisionRequest request (false, 1, false);
CollisionResult result;
bool res;
......@@ -1353,7 +1353,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone_libccd)
generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
generateBVHModel(s2_obb, s2, Transform3f(), 16, 16);
CollisionRequest request;
CollisionRequest request (false, 1, false);
CollisionResult result;
bool res;
......@@ -1506,7 +1506,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK)
generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
generateBVHModel(s2_obb, s2, Transform3f(), 16, 16);
CollisionRequest request;
CollisionRequest request (false, 1, false);
request.gjk_solver_type = GST_INDEP;
CollisionResult result;
......@@ -1726,7 +1726,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_GJK)
generateBVHModel(s1_obb, s1, Transform3f());
generateBVHModel(s2_obb, s2, Transform3f());
CollisionRequest request;
CollisionRequest request (false, 1, false);
request.gjk_solver_type = GST_INDEP;
CollisionResult result;
......@@ -1848,7 +1848,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_GJK)
generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
generateBVHModel(s2_obb, s2, Transform3f());
CollisionRequest request;
CollisionRequest request (false, 1, false);
request.gjk_solver_type = GST_INDEP;
CollisionResult result;
......@@ -1970,7 +1970,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder_GJK)
generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
generateBVHModel(s2_obb, s2, Transform3f(), 16, 16);
CollisionRequest request;
CollisionRequest request (false, 1, false);
request.gjk_solver_type = GST_INDEP;
CollisionResult result;
......@@ -2059,7 +2059,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone_GJK)
generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
generateBVHModel(s2_obb, s2, Transform3f(), 16, 16);
CollisionRequest request;
CollisionRequest request (false, 1, false);
request.gjk_solver_type = GST_INDEP;
CollisionResult result;
......
......@@ -113,7 +113,7 @@ struct DistanceRes
/// @brief Collision data stores the collision request and the result given by collision algorithm.
struct CollisionData
{
CollisionData()
CollisionData() : request (false, 1, false)
{
done = false;
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment