Commit 0d6aa703 authored by stevet's avatar stevet
Browse files

using flag structure for argurments to collision requests

parent 9f6e25b0
......@@ -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
......@@ -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;
......
......@@ -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);
......
......@@ -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(),
......
......@@ -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;
......
......@@ -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);
......
......@@ -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;
......
......@@ -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;
......
......@@ -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;
}
......
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