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 ...@@ -55,25 +55,24 @@ namespace fcl
template<typename NarrowPhaseSolver> template<typename NarrowPhaseSolver>
int collide(const CollisionObject* o1, const CollisionObject* o2, int collide(const CollisionObject* o1, const CollisionObject* o2,
const NarrowPhaseSolver* nsolver, const NarrowPhaseSolver* nsolver,
int num_max_contacts, bool exhaustive, bool enable_contact, const CollisionRequest& request,
std::vector<Contact>& contacts); CollisionResult& result);
template<typename NarrowPhaseSolver> template<typename NarrowPhaseSolver>
int collide(const CollisionGeometry* o1, const SimpleTransform& tf1, int collide(const CollisionGeometry* o1, const SimpleTransform& tf1,
const CollisionGeometry* o2, const SimpleTransform& tf2, const CollisionGeometry* o2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver, const NarrowPhaseSolver* nsolver,
int num_max_contacts, bool exhaustive, bool enable_contact, const CollisionRequest& request,
std::vector<Contact>& contacts); CollisionResult& result);
int collide(const CollisionObject* o1, const CollisionObject* o2, int collide(const CollisionObject* o1, const CollisionObject* o2,
int num_max_contacts, bool exhaustive, bool enable_contact, const CollisionRequest& request,
std::vector<Contact>& contacts); CollisionResult& result);
int collide(const CollisionGeometry* o1, const SimpleTransform& tf1, int collide(const CollisionGeometry* o1, const SimpleTransform& tf1,
const CollisionGeometry* o2, const SimpleTransform& tf2, const CollisionGeometry* o2, const SimpleTransform& tf2,
int num_max_contacts, bool exhaustive, bool enable_contact, const CollisionRequest& request,
std::vector<Contact>& contacts); CollisionResult& result);
} }
#endif #endif
...@@ -49,36 +49,104 @@ struct Contact ...@@ -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 struct CollisionData
{ {
CollisionData() CollisionData()
{ {
done = false; done = false;
is_collision = false;
num_max_contacts = 1;
enable_contact = false;
exhaustive = false;
} }
CollisionRequest request;
CollisionResult result;
bool done; 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 struct DistanceData
{ {
DistanceData() DistanceData()
{ {
min_distance = std::numeric_limits<FCL_REAL>::max();
done = false; done = false;
} }
FCL_REAL min_distance;
bool done; bool done;
DistanceRequest request;
DistanceResult result;
}; };
......
...@@ -48,7 +48,7 @@ namespace fcl ...@@ -48,7 +48,7 @@ namespace fcl
template<typename NarrowPhaseSolver> template<typename NarrowPhaseSolver>
struct CollisionFunctionMatrix 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]; CollisionFunc collision_matrix[NODE_COUNT][NODE_COUNT];
......
...@@ -52,8 +52,8 @@ int conservativeAdvancement(const CollisionGeometry* o1, ...@@ -52,8 +52,8 @@ int conservativeAdvancement(const CollisionGeometry* o1,
MotionBase<BV>* motion1, MotionBase<BV>* motion1,
const CollisionGeometry* o2, const CollisionGeometry* o2,
MotionBase<BV>* motion2, MotionBase<BV>* motion2,
int num_max_contacts, bool exhaustive, bool enable_contact, const CollisionRequest& request,
std::vector<Contact>& contacts, CollisionResult& result,
FCL_REAL& toc); FCL_REAL& toc);
} }
......
...@@ -38,24 +38,29 @@ ...@@ -38,24 +38,29 @@
#define FCL_DISTANCE_H #define FCL_DISTANCE_H
#include "fcl/collision_object.h" #include "fcl/collision_object.h"
#include "fcl/collision_data.h"
namespace fcl namespace fcl
{ {
template<typename NarrowPhaseSolver> 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> template<typename NarrowPhaseSolver>
FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
const CollisionGeometry* o2, const SimpleTransform& tf2, 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, 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 #endif
...@@ -46,7 +46,8 @@ namespace fcl ...@@ -46,7 +46,8 @@ namespace fcl
template<typename NarrowPhaseSolver> template<typename NarrowPhaseSolver>
struct DistanceFunctionMatrix 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]; DistanceFunc distance_matrix[NODE_COUNT][NODE_COUNT];
......
This diff is collapsed.
...@@ -39,6 +39,7 @@ ...@@ -39,6 +39,7 @@
#include "fcl/primitive.h" #include "fcl/primitive.h"
#include "fcl/transform.h" #include "fcl/transform.h"
#include "fcl/collision_data.h"
/** \brief Main namespace */ /** \brief Main namespace */
namespace fcl namespace fcl
...@@ -100,6 +101,8 @@ public: ...@@ -100,6 +101,8 @@ public:
/** \brief Check whether the traversal can stop */ /** \brief Check whether the traversal can stop */
virtual bool canStop() const; virtual bool canStop() const;
CollisionRequest request;
}; };
class DistanceTraversalNodeBase : public TraversalNodeBase class DistanceTraversalNodeBase : public TraversalNodeBase
...@@ -114,6 +117,8 @@ public: ...@@ -114,6 +117,8 @@ public:
virtual void leafTesting(int b1, int b2) const; virtual void leafTesting(int b1, int b2) const;
virtual bool canStop(FCL_REAL c) const; virtual bool canStop(FCL_REAL c) const;
DistanceRequest request;
}; };
} }
......
...@@ -38,6 +38,7 @@ ...@@ -38,6 +38,7 @@
#ifndef FCL_TRAVERSAL_NODE_MESH_SHAPE_H #ifndef FCL_TRAVERSAL_NODE_MESH_SHAPE_H
#define 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/geometric_shapes.h"
#include "fcl/traversal_node_base.h" #include "fcl/traversal_node_base.h"
#include "fcl/BVH_model.h" #include "fcl/BVH_model.h"
...@@ -183,10 +184,6 @@ public: ...@@ -183,10 +184,6 @@ public:
vertices = NULL; vertices = NULL;
tri_indices = NULL; tri_indices = NULL;
num_max_contacts = 1;
exhaustive = false;
enable_contact = false;
nsolver = NULL; nsolver = NULL;
} }
...@@ -207,7 +204,7 @@ public: ...@@ -207,7 +204,7 @@ public:
Vec3f normal; Vec3f normal;
Vec3f contactp; 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)) if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, NULL, NULL, NULL))
{ {
...@@ -225,15 +222,13 @@ public: ...@@ -225,15 +222,13 @@ public:
bool canStop() const 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; Vec3f* vertices;
Triangle* tri_indices; Triangle* tri_indices;
int num_max_contacts; CollisionRequest request;
bool exhaustive;
bool enable_contact;
mutable std::vector<BVHShapeCollisionPair> pairs; mutable std::vector<BVHShapeCollisionPair> pairs;
...@@ -251,9 +246,7 @@ static inline void meshShapeCollisionOrientedNodeLeafTesting(int b1, int b2, ...@@ -251,9 +246,7 @@ static inline void meshShapeCollisionOrientedNodeLeafTesting(int b1, int b2,
const SimpleTransform& tf2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver, const NarrowPhaseSolver* nsolver,
bool enable_statistics, bool enable_statistics,
bool enable_contact, const CollisionRequest& request,
bool exhaustive,
int num_max_contacts,
int& num_leaf_tests, int& num_leaf_tests,
std::vector<BVHShapeCollisionPair>& pairs) std::vector<BVHShapeCollisionPair>& pairs)
...@@ -273,7 +266,7 @@ static inline void meshShapeCollisionOrientedNodeLeafTesting(int b1, int b2, ...@@ -273,7 +266,7 @@ static inline void meshShapeCollisionOrientedNodeLeafTesting(int b1, int b2,
Vec3f normal; Vec3f normal;
Vec3f contactp; 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)) if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, R, T, NULL, NULL, NULL))
{ {
...@@ -309,8 +302,7 @@ public: ...@@ -309,8 +302,7 @@ public:
void leafTesting(int b1, int b2) const void leafTesting(int b1, int b2) const
{ {
details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices, 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, R, T, this->tf2, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
} }
// R, T is the transform of bvh model // R, T is the transform of bvh model
...@@ -336,8 +328,7 @@ public: ...@@ -336,8 +328,7 @@ public:
void leafTesting(int b1, int b2) const void leafTesting(int b1, int b2) const
{ {
details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices, 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, R, T, this->tf2, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
} }
// R, T is the transform of bvh model // R, T is the transform of bvh model
...@@ -363,8 +354,7 @@ public: ...@@ -363,8 +354,7 @@ public:
void leafTesting(int b1, int b2) const void leafTesting(int b1, int b2) const
{ {
details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices, 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, R, T, this->tf2, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
} }
// R, T is the transform of bvh model // R, T is the transform of bvh model
...@@ -390,8 +380,7 @@ public: ...@@ -390,8 +380,7 @@ public:
void leafTesting(int b1, int b2) const void leafTesting(int b1, int b2) const
{ {
details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices, 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, R, T, this->tf2, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
} }
// R, T is the transform of bvh model // R, T is the transform of bvh model
...@@ -408,10 +397,6 @@ public: ...@@ -408,10 +397,6 @@ public:
vertices = NULL; vertices = NULL;
tri_indices = NULL; tri_indices = NULL;
num_max_contacts = 1;
exhaustive = false;
enable_contact = false;
nsolver = NULL; nsolver = NULL;
} }
...@@ -432,7 +417,7 @@ public: ...@@ -432,7 +417,7 @@ public:
Vec3f normal; Vec3f normal;
Vec3f contactp; 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)) if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, NULL, NULL, NULL))
{ {
...@@ -450,15 +435,13 @@ public: ...@@ -450,15 +435,13 @@ public:
bool canStop() const 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; Vec3f* vertices;
Triangle* tri_indices; Triangle* tri_indices;
int num_max_contacts; CollisionRequest request;
bool exhaustive;
bool enable_contact;
mutable std::vector<BVHShapeCollisionPair> pairs; mutable std::vector<BVHShapeCollisionPair> pairs;
...@@ -483,8 +466,7 @@ public: ...@@ -483,8 +466,7 @@ public:
void leafTesting(int b1, int b2) const void leafTesting(int b1, int b2) const
{ {
details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices, 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, R, T, this->tf1, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
// may need to change the order in pairs // may need to change the order in pairs
} }
...@@ -513,8 +495,7 @@ public: ...@@ -513,8 +495,7 @@ public:
void leafTesting(int b1, int b2) const void leafTesting(int b1, int b2) const
{ {
details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices, 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, R, T, this->tf1, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
// may need to change the order in pairs // may need to change the order in pairs
} }
...@@ -543,8 +524,7 @@ public: ...@@ -543,8 +524,7 @@ public:
void leafTesting(int b1, int b2) const void leafTesting(int b1, int b2) const
{ {
details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices, 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, R, T, this->tf1, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
// may need to change the order in pairs // may need to change the order in pairs
} }
...@@ -573,8 +553,7 @@ public: ...@@ -573,8 +553,7 @@ public:
void leafTesting(int b1, int b2) const void leafTesting(int b1, int b2) const
{ {
details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices, 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, R, T, this->tf1, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
// may need to change the order in pairs // may need to change the order in pairs
} }
......
...@@ -38,6 +38,7 @@ ...@@ -38,6 +38,7 @@
#ifndef FCL_TRAVERSAL_NODE_MESHES_H #ifndef FCL_TRAVERSAL_NODE_MESHES_H