Commit bf945404 authored by jpan's avatar jpan
Browse files

change the interface of collide: the collision settings are now in a...

change the interface of collide: the collision settings are now in a CollisionRequest structure, and collision results are now in a CollisionResult structure. Distance has similar extension. This is for the convenience of adding cost and potential future extension.


git-svn-id: https://kforge.ros.org/fcl/fcl_ros@137 253336fb-580f-4252-a368-f3cef5a2a82b
parent d77c399b
......@@ -55,25 +55,24 @@ namespace fcl
template<typename NarrowPhaseSolver>
int collide(const CollisionObject* o1, const CollisionObject* o2,
const NarrowPhaseSolver* nsolver,
int num_max_contacts, bool exhaustive, bool enable_contact,
std::vector<Contact>& contacts);
const CollisionRequest& request,
CollisionResult& result);
template<typename NarrowPhaseSolver>
int collide(const CollisionGeometry* o1, const SimpleTransform& tf1,
const CollisionGeometry* o2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
int num_max_contacts, bool exhaustive, bool enable_contact,
std::vector<Contact>& contacts);
const CollisionRequest& request,
CollisionResult& result);
int collide(const CollisionObject* o1, const CollisionObject* o2,
int num_max_contacts, bool exhaustive, bool enable_contact,
std::vector<Contact>& contacts);
const CollisionRequest& request,
CollisionResult& result);
int collide(const CollisionGeometry* o1, const SimpleTransform& tf1,
const CollisionGeometry* o2, const SimpleTransform& tf2,
int num_max_contacts, bool exhaustive, bool enable_contact,
std::vector<Contact>& contacts);
const CollisionRequest& request,
CollisionResult& result);
}
#endif
......@@ -49,36 +49,104 @@ struct Contact
}
};
struct CostSource
{
Vec3f aabb_min;
Vec3f aabb_max;
FCL_REAL cost; // density
};
struct CollisionRequest
{
bool exhaustive;
int num_max_contacts;
bool enable_contact;
int num_max_cost_sources;
bool enable_cost;
CollisionRequest(bool exhaustive_ = false,
unsigned int num_max_contacts_ = 1,
bool enable_contact_ = false,
unsigned int num_max_cost_sources_ = 1,
bool enable_cost_ = false) : exhaustive(exhaustive_),
num_max_contacts(num_max_contacts_),
enable_contact(enable_contact_),
num_max_cost_sources(num_max_cost_sources_),
enable_cost(enable_cost_)
{
}
};
struct CollisionResult
{
std::vector<Contact> contacts;
std::vector<CostSource> cost_sources;
bool is_collision;
CollisionResult() : is_collision(false)
{
}
void clear()
{
contacts.clear();
cost_sources.clear();
is_collision = false;
}
};
struct CollisionData
{
CollisionData()
{
done = false;
is_collision = false;
num_max_contacts = 1;
enable_contact = false;
exhaustive = false;
}
CollisionRequest request;
CollisionResult result;
bool done;
bool is_collision;
bool exhaustive;
unsigned int num_max_contacts;
bool enable_contact;
};
std::vector<Contact> contacts;
struct DistanceRequest
{
bool enable_nearest_points;
DistanceRequest() : enable_nearest_points(false)
{
}
};
struct DistanceResult
{
FCL_REAL min_distance;
Vec3f nearest_points[2];
DistanceResult() : min_distance(std::numeric_limits<FCL_REAL>::max())
{
}
void clear()
{
min_distance = std::numeric_limits<FCL_REAL>::max();
}
};
struct DistanceData
{
DistanceData()
{
min_distance = std::numeric_limits<FCL_REAL>::max();
done = false;
}
FCL_REAL min_distance;
bool done;
DistanceRequest request;
DistanceResult result;
};
......
......@@ -48,7 +48,7 @@ namespace fcl
template<typename NarrowPhaseSolver>
struct CollisionFunctionMatrix
{
typedef int (*CollisionFunc)(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts);
typedef int (*CollisionFunc)(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result);
CollisionFunc collision_matrix[NODE_COUNT][NODE_COUNT];
......
......@@ -52,8 +52,8 @@ int conservativeAdvancement(const CollisionGeometry* o1,
MotionBase<BV>* motion1,
const CollisionGeometry* o2,
MotionBase<BV>* motion2,
int num_max_contacts, bool exhaustive, bool enable_contact,
std::vector<Contact>& contacts,
const CollisionRequest& request,
CollisionResult& result,
FCL_REAL& toc);
}
......
......@@ -38,24 +38,29 @@
#define FCL_DISTANCE_H
#include "fcl/collision_object.h"
#include "fcl/collision_data.h"
namespace fcl
{
template<typename NarrowPhaseSolver>
FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const NarrowPhaseSolver* nsolver);
FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result);
template<typename NarrowPhaseSolver>
FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
const CollisionGeometry* o2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver);
const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result);
FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2);
FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2,
const DistanceRequest& request, DistanceResult& result);
FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
const CollisionGeometry* o2, const SimpleTransform& tf2);
const CollisionGeometry* o2, const SimpleTransform& tf2,
const DistanceRequest& request, DistanceResult& result);
}
#endif
......@@ -46,7 +46,8 @@ namespace fcl
template<typename NarrowPhaseSolver>
struct DistanceFunctionMatrix
{
typedef FCL_REAL (*DistanceFunc)(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver);
typedef FCL_REAL (*DistanceFunc)(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result);
DistanceFunc distance_matrix[NODE_COUNT][NODE_COUNT];
......
This diff is collapsed.
......@@ -39,6 +39,7 @@
#include "fcl/primitive.h"
#include "fcl/transform.h"
#include "fcl/collision_data.h"
/** \brief Main namespace */
namespace fcl
......@@ -100,6 +101,8 @@ public:
/** \brief Check whether the traversal can stop */
virtual bool canStop() const;
CollisionRequest request;
};
class DistanceTraversalNodeBase : public TraversalNodeBase
......@@ -114,6 +117,8 @@ public:
virtual void leafTesting(int b1, int b2) const;
virtual bool canStop(FCL_REAL c) const;
DistanceRequest request;
};
}
......
......@@ -38,6 +38,7 @@
#ifndef FCL_TRAVERSAL_NODE_MESH_SHAPE_H
#define FCL_TRAVERSAL_NODE_MESH_SHAPE_H
#include "fcl/collision_data.h"
#include "fcl/geometric_shapes.h"
#include "fcl/traversal_node_base.h"
#include "fcl/BVH_model.h"
......@@ -183,10 +184,6 @@ public:
vertices = NULL;
tri_indices = NULL;
num_max_contacts = 1;
exhaustive = false;
enable_contact = false;
nsolver = NULL;
}
......@@ -207,7 +204,7 @@ public:
Vec3f normal;
Vec3f contactp;
if(!enable_contact) // only interested in collision or not
if(!request.enable_contact) // only interested in collision or not
{
if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, NULL, NULL, NULL))
{
......@@ -225,15 +222,13 @@ public:
bool canStop() const
{
return (pairs.size() > 0) && (!exhaustive) && (num_max_contacts <= (int)pairs.size());
return (pairs.size() > 0) && (!request.exhaustive) && (request.num_max_contacts <= (int)pairs.size());
}
Vec3f* vertices;
Triangle* tri_indices;
int num_max_contacts;
bool exhaustive;
bool enable_contact;
CollisionRequest request;
mutable std::vector<BVHShapeCollisionPair> pairs;
......@@ -251,9 +246,7 @@ static inline void meshShapeCollisionOrientedNodeLeafTesting(int b1, int b2,
const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
bool enable_statistics,
bool enable_contact,
bool exhaustive,
int num_max_contacts,
const CollisionRequest& request,
int& num_leaf_tests,
std::vector<BVHShapeCollisionPair>& pairs)
......@@ -273,7 +266,7 @@ static inline void meshShapeCollisionOrientedNodeLeafTesting(int b1, int b2,
Vec3f normal;
Vec3f contactp;
if(!enable_contact) // only interested in collision or not
if(!request.enable_contact) // only interested in collision or not
{
if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, R, T, NULL, NULL, NULL))
{
......@@ -309,8 +302,7 @@ public:
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
R, T, this->tf2, this->nsolver, this->enable_statistics, this->enable_contact,
this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
R, T, this->tf2, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
}
// R, T is the transform of bvh model
......@@ -336,8 +328,7 @@ public:
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
R, T, this->tf2, this->nsolver, this->enable_statistics, this->enable_contact,
this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
R, T, this->tf2, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
}
// R, T is the transform of bvh model
......@@ -363,8 +354,7 @@ public:
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
R, T, this->tf2, this->nsolver, this->enable_statistics, this->enable_contact,
this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
R, T, this->tf2, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
}
// R, T is the transform of bvh model
......@@ -390,8 +380,7 @@ public:
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
R, T, this->tf2, this->nsolver, this->enable_statistics, this->enable_contact,
this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
R, T, this->tf2, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
}
// R, T is the transform of bvh model
......@@ -408,10 +397,6 @@ public:
vertices = NULL;
tri_indices = NULL;
num_max_contacts = 1;
exhaustive = false;
enable_contact = false;
nsolver = NULL;
}
......@@ -432,7 +417,7 @@ public:
Vec3f normal;
Vec3f contactp;
if(!enable_contact) // only interested in collision or not
if(!request.enable_contact) // only interested in collision or not
{
if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, NULL, NULL, NULL))
{
......@@ -450,15 +435,13 @@ public:
bool canStop() const
{
return (pairs.size() > 0) && (!exhaustive) && (num_max_contacts <= (int)pairs.size());
return (pairs.size() > 0) && (!request.exhaustive) && (request.num_max_contacts <= (int)pairs.size());
}
Vec3f* vertices;
Triangle* tri_indices;
int num_max_contacts;
bool exhaustive;
bool enable_contact;
CollisionRequest request;
mutable std::vector<BVHShapeCollisionPair> pairs;
......@@ -483,8 +466,7 @@ public:
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
R, T, this->tf1, this->nsolver, this->enable_statistics, this->enable_contact,
this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
R, T, this->tf1, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
// may need to change the order in pairs
}
......@@ -513,8 +495,7 @@ public:
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
R, T, this->tf1, this->nsolver, this->enable_statistics, this->enable_contact,
this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
R, T, this->tf1, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
// may need to change the order in pairs
}
......@@ -543,8 +524,7 @@ public:
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
R, T, this->tf1, this->nsolver, this->enable_statistics, this->enable_contact,
this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
R, T, this->tf1, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
// may need to change the order in pairs
}
......@@ -573,8 +553,7 @@ public:
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
R, T, this->tf1, this->nsolver, this->enable_statistics, this->enable_contact,
this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
R, T, this->tf1, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
// may need to change the order in pairs
}
......
......@@ -38,6 +38,7 @@
#ifndef FCL_TRAVERSAL_NODE_MESHES_H
#define FCL_TRAVERSAL_NODE_MESHES_H
#include "fcl/collision_data.h"
#include "fcl/traversal_node_base.h"
#include "fcl/BV_node.h"
#include "fcl/BV.h"
......@@ -188,10 +189,6 @@ public:
vertices2 = NULL;
tri_indices1 = NULL;
tri_indices2 = NULL;
num_max_contacts = 1;
exhaustive = false;
enable_contact = false;
}
/** \brief Intersection testing between leaves (two triangles) */
......@@ -221,7 +218,7 @@ public:
Vec3f contacts[2];
if(!enable_contact) // only interested in collision or not
if(!this->request.enable_contact) // only interested in collision or not
{
if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3))
{
......@@ -238,7 +235,7 @@ public:
{
for(int i = 0; i < n_contacts; ++i)
{
if((!exhaustive) && (num_max_contacts <= (int)pairs.size())) break;
if((!this->request.exhaustive) && (this->request.num_max_contacts <= (int)pairs.size())) break;
pairs.push_back(BVHCollisionPair(primitive_id1, primitive_id2, contacts[i], normal, penetration));
}
}
......@@ -248,7 +245,7 @@ public:
/** \brief Whether the traversal process can stop early */
bool canStop() const
{
return (pairs.size() > 0) && (!exhaustive) && (num_max_contacts <= (int)pairs.size());
return (pairs.size() > 0) && (!this->request.exhaustive) && (this->request.num_max_contacts <= (int)pairs.size());
}
Vec3f* vertices1;
......@@ -257,10 +254,6 @@ public:
Triangle* tri_indices1;
Triangle* tri_indices2;
int num_max_contacts;
bool exhaustive;
bool enable_contact;
mutable std::vector<BVHCollisionPair> pairs;
};
......@@ -365,10 +358,6 @@ public:
vertices1 = NULL;
vertices2 = NULL;
num_max_contacts = 1;
exhaustive = false;
enable_contact = false;
collision_prob_threshold = 0.5;
max_collision_prob = 0;
leaf_size_threshold = 1;
......@@ -409,7 +398,7 @@ public:
bool canStop() const
{
return (pairs.size() > 0) && (!exhaustive) && (num_max_contacts <= (int)pairs.size());
return (pairs.size() > 0) && (!this->request.exhaustive) && (this->request.num_max_contacts <= (int)pairs.size());
}
Vec3f* vertices1;
......@@ -418,10 +407,6 @@ public:
boost::shared_array<Uncertainty> uc1;
boost::shared_array<Uncertainty> uc2;
int num_max_contacts;
bool exhaustive;
bool enable_contact;
mutable std::vector<BVHPointCollisionPair> pairs;
int leaf_size_threshold;
......@@ -474,10 +459,6 @@ public:
vertices2 = NULL;
tri_indices2 = NULL;
num_max_contacts = 1;
exhaustive = false;
enable_contact = false;
collision_prob_threshold = 0.5;
max_collision_prob = 0;
leaf_size_threshold = 1;
......@@ -515,7 +496,7 @@ public:
bool canStop() const
{
return (pairs.size() > 0) && (!exhaustive) && (num_max_contacts <= (int)pairs.size());
return (pairs.size() > 0) && (!this->request.exhaustive) && (this->request.num_max_contacts <= (int)pairs.size());
}
Vec3f* vertices1;
......@@ -524,10 +505,6 @@ public:
boost::shared_array<Uncertainty> uc1;
Triangle* tri_indices2;
int num_max_contacts;
bool exhaustive;
bool enable_contact;
mutable std::vector<BVHPointCollisionPair> pairs;
int leaf_size_threshold;
......@@ -596,10 +573,6 @@ public:
prev_vertices1 = NULL;
prev_vertices2 = NULL;
num_max_contacts = 1;
exhaustive = false;
enable_contact = false;
num_vf_tests = 0;
num_ee_tests = 0;
}
......@@ -688,7 +661,7 @@ public:
bool canStop() const
{
return (pairs.size() > 0) && (!exhaustive) && (num_max_contacts <= (int)pairs.size());
return (pairs.size() > 0) && (!this->request.exhaustive) && (this->request.num_max_contacts <= (int)pairs.size());
}
Vec3f* vertices1;
......@@ -700,10 +673,6 @@ public:
Vec3f* prev_vertices1;
Vec3f* prev_vertices2;
int num_max_contacts;
bool exhaustive;
bool enable_contact;
mutable int num_vf_tests;
mutable int num_ee_tests;
......@@ -723,10 +692,6 @@ public:
prev_vertices1 = NULL;
prev_vertices2 = NULL;
num_max_contacts = 1;
exhaustive = false;
enable_contact = false;
num_vf_tests = 0;
}
......@@ -781,7 +746,7 @@ public:
bool canStop() const
{
return (pairs.size() > 0) && (!exhaustive) && (num_max_contacts <= (int)pairs.size());
return (pairs.size() > 0) && (!this->request.exhaustive) && (this->request.num_max_contacts <= (int)pairs.size());
}
Vec3f* vertices1;
......@@ -792,10 +757,6 @@ public:
Vec3f* prev_vertices1;
Vec3f* prev_vertices2;
int num_max_contacts;
bool exhaustive;
bool enable_contact;
mutable int num_vf_tests;
mutable std::vector<BVHContinuousCollisionPair> pairs;
......@@ -814,10 +775,6 @@ public: