Commit d5fa63ad authored by jpan's avatar jpan
Browse files

More change on request/result interface.


git-svn-id: https://kforge.ros.org/fcl/fcl_ros@141 253336fb-580f-4252-a368-f3cef5a2a82b
parent c90f59ef
......@@ -49,7 +49,7 @@ namespace fcl
/** \brief Main collision interface: given two collision objects, and the requirements for contacts, including num of max contacts, whether perform exhaustive collision (i.e., returning
* returning all the contact points), whether return detailed contact information (i.e., normal, contact point, depth; otherwise only contact primitive id is returned), this function
* performs the collision between them.
* Return value is the number of contacts returned
* Return value is the number of contacts generated between the two objects.
*/
template<typename NarrowPhaseSolver>
......
......@@ -103,9 +103,10 @@ struct CollisionRequest
struct CollisionResult
{
private:
std::vector<Contact> contacts;
std::vector<CostSource> cost_sources;
public:
CollisionResult()
{
}
......@@ -135,6 +136,34 @@ struct CollisionResult
return cost_sources.size();
}
const Contact& getContact(size_t i) const
{
if(i < contacts.size())
return contacts[i];
else
return contacts.back();
}
const CostSource& getCostSource(size_t i) const
{
if(i < cost_sources.size())
return cost_sources[i];
else
return cost_sources.back();
}
void getContacts(std::vector<Contact>& contacts_)
{
contacts_.resize(contacts.size());
std::copy(contacts.begin(), contacts.end(), contacts_.begin());
}
void getCostSources(std::vector<CostSource>& cost_sources_)
{
cost_sources_.resize(cost_sources.size());
std::copy(cost_sources.begin(), cost_sources.end(), cost_sources_.begin());
}
void clear()
{
contacts.clear();
......@@ -159,24 +188,81 @@ struct CollisionData
struct DistanceRequest
{
bool enable_nearest_points;
DistanceRequest() : enable_nearest_points(false)
DistanceRequest(bool enable_nearest_points_ = false) : enable_nearest_points(enable_nearest_points_)
{
}
};
struct DistanceResult
{
FCL_REAL min_distance;
Vec3f nearest_points[2];
const CollisionGeometry* o1;
const CollisionGeometry* o2;
int b1;
int b2;
static const int NONE = -1;
DistanceResult() : min_distance(std::numeric_limits<FCL_REAL>::max())
DistanceResult(FCL_REAL min_distance_ = std::numeric_limits<FCL_REAL>::max()) : min_distance(min_distance_),
o1(NULL),
o2(NULL),
b1(-1),
b2(-1)
{
}
void update(FCL_REAL distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_)
{
if(min_distance > distance)
{
min_distance = distance;
o1 = o1_;
o2 = o2_;
b1 = b1_;
b2 = b2_;
}
}
void update(FCL_REAL distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_, const Vec3f& p1, const Vec3f& p2)
{
if(min_distance > distance)
{
min_distance = distance;
o1 = o1_;
o2 = o2_;
b1 = b1_;
b2 = b2_;
nearest_points[0] = p1;
nearest_points[1] = p2;
}
}
void update(const DistanceResult& other_result)
{
if(min_distance > other_result.min_distance)
{
min_distance = other_result.min_distance;
o1 = other_result.o1;
o2 = other_result.o2;
b1 = other_result.b1;
b2 = other_result.b2;
nearest_points[0] = other_result.nearest_points[0];
nearest_points[1] = other_result.nearest_points[1];
}
}
void clear()
{
min_distance = std::numeric_limits<FCL_REAL>::max();
o1 = NULL;
o2 = NULL;
b1 = -1;
b2 = -1;
}
};
......
......@@ -75,9 +75,11 @@ bool initialize(OcTreeDistanceTraversalNode<NarrowPhaseSolver>& node,
const OcTree& model1, const SimpleTransform& tf1,
const OcTree& model2, const SimpleTransform& tf2,
const OcTreeSolver<NarrowPhaseSolver>* otsolver,
const DistanceRequest& request)
const DistanceRequest& request,
DistanceResult& result)
{
node.request = request;
node.result = &result;
node.model1 = &model1;
node.model2 = &model2;
......@@ -139,9 +141,11 @@ bool initialize(ShapeOcTreeDistanceTraversalNode<S, NarrowPhaseSolver>& node,
const S& model1, const SimpleTransform& tf1,
const OcTree& model2, const SimpleTransform& tf2,
const OcTreeSolver<NarrowPhaseSolver>* otsolver,
const DistanceRequest& request)
const DistanceRequest& request,
DistanceResult& result)
{
node.request = request;
node.result = &result;
node.model1 = &model1;
node.model2 = &model2;
......@@ -160,9 +164,11 @@ bool initialize(OcTreeShapeDistanceTraversalNode<S, NarrowPhaseSolver>& node,
const OcTree& model1, const SimpleTransform& tf1,
const S& model2, const SimpleTransform& tf2,
const OcTreeSolver<NarrowPhaseSolver>* otsolver,
const DistanceRequest& request)
const DistanceRequest& request,
DistanceResult& result)
{
node.request = request;
node.result = &result;
node.model1 = &model1;
node.model2 = &model2;
......@@ -224,9 +230,11 @@ bool initialize(MeshOcTreeDistanceTraversalNode<BV, NarrowPhaseSolver>& node,
const BVHModel<BV>& model1, const SimpleTransform& tf1,
const OcTree& model2, const SimpleTransform& tf2,
const OcTreeSolver<NarrowPhaseSolver>* otsolver,
const DistanceRequest& request)
const DistanceRequest& request,
DistanceResult& result)
{
node.request = request;
node.result = &result;
node.model1 = &model1;
node.model2 = &model2;
......@@ -245,9 +253,11 @@ bool initialize(OcTreeMeshDistanceTraversalNode<BV, NarrowPhaseSolver>& node,
const OcTree& model1, const SimpleTransform& tf1,
const BVHModel<BV>& model2, const SimpleTransform& tf2,
const OcTreeSolver<NarrowPhaseSolver>* otsolver,
const DistanceRequest& request)
const DistanceRequest& request,
DistanceResult& result)
{
node.request = request;
node.result = &result;
node.model1 = &model1;
node.model2 = &model2;
......@@ -823,9 +833,11 @@ bool initialize(ShapeDistanceTraversalNode<S1, S2, NarrowPhaseSolver>& node,
const S1& shape1, const SimpleTransform& tf1,
const S2& shape2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
const DistanceRequest& request)
const DistanceRequest& request,
DistanceResult& result)
{
node.request = request;
node.result = &result;
node.model1 = &shape1;
node.tf1 = tf1;
......@@ -842,6 +854,7 @@ bool initialize(MeshDistanceTraversalNode<BV>& node,
BVHModel<BV>& model1, SimpleTransform& tf1,
BVHModel<BV>& model2, SimpleTransform& tf2,
const DistanceRequest& request,
DistanceResult& result,
bool use_refit = false, bool refit_bottomup = false)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
......@@ -882,6 +895,7 @@ bool initialize(MeshDistanceTraversalNode<BV>& node,
}
node.request = request;
node.result = &result;
node.model1 = &model1;
node.tf1 = tf1;
......@@ -902,18 +916,21 @@ bool initialize(MeshDistanceTraversalNode<BV>& node,
bool initialize(MeshDistanceTraversalNodeRSS& node,
const BVHModel<RSS>& model1, const SimpleTransform& tf1,
const BVHModel<RSS>& model2, const SimpleTransform& tf2,
const DistanceRequest& request);
const DistanceRequest& request,
DistanceResult& result);
bool initialize(MeshDistanceTraversalNodekIOS& node,
const BVHModel<kIOS>& model1, const SimpleTransform& tf1,
const BVHModel<kIOS>& model2, const SimpleTransform& tf2,
const DistanceRequest& request);
const DistanceRequest& request,
DistanceResult& result);
bool initialize(MeshDistanceTraversalNodeOBBRSS& node,
const BVHModel<OBBRSS>& model1, const SimpleTransform& tf1,
const BVHModel<OBBRSS>& model2, const SimpleTransform& tf2,
const DistanceRequest& request);
const DistanceRequest& request,
DistanceResult& result);
template<typename BV, typename S, typename NarrowPhaseSolver>
......@@ -922,6 +939,7 @@ bool initialize(MeshShapeDistanceTraversalNode<BV, S, NarrowPhaseSolver>& node,
const S& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
const DistanceRequest& request,
DistanceResult& result,
bool use_refit = false, bool refit_bottomup = false)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES)
......@@ -945,6 +963,7 @@ bool initialize(MeshShapeDistanceTraversalNode<BV, S, NarrowPhaseSolver>& node,
}
node.request = request;
node.result = &result;
node.model1 = &model1;
node.tf1 = tf1;
......@@ -967,6 +986,7 @@ bool initialize(ShapeMeshDistanceTraversalNode<S, BV, NarrowPhaseSolver>& node,
BVHModel<BV>& model2, SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
const DistanceRequest& request,
DistanceResult& result,
bool use_refit = false, bool refit_bottomup = false)
{
if(model2.getModelType() != BVH_MODEL_TRIANGLES)
......@@ -990,6 +1010,7 @@ bool initialize(ShapeMeshDistanceTraversalNode<S, BV, NarrowPhaseSolver>& node,
}
node.request = request;
node.result = &result;
node.model1 = &model1;
node.tf1 = tf1;
......@@ -1014,12 +1035,14 @@ static inline bool setupMeshShapeDistanceOrientedNode(OrientedNode<S, NarrowPhas
const BVHModel<BV>& model1, const SimpleTransform& tf1,
const S& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
const DistanceRequest& request)
const DistanceRequest& request,
DistanceResult& result)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES)
return false;
node.request = request;
node.result = &result;
node.model1 = &model1;
node.tf1 = tf1;
......@@ -1042,9 +1065,10 @@ bool initialize(MeshShapeDistanceTraversalNodeRSS<S, NarrowPhaseSolver>& node,
const BVHModel<RSS>& model1, const SimpleTransform& tf1,
const S& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
const DistanceRequest& request)
const DistanceRequest& request,
DistanceResult& result)
{
return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
}
template<typename S, typename NarrowPhaseSolver>
......@@ -1052,9 +1076,10 @@ bool initialize(MeshShapeDistanceTraversalNodekIOS<S, NarrowPhaseSolver>& node,
const BVHModel<kIOS>& model1, const SimpleTransform& tf1,
const S& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
const DistanceRequest& request)
const DistanceRequest& request,
DistanceResult& result)
{
return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
}
template<typename S, typename NarrowPhaseSolver>
......@@ -1062,9 +1087,10 @@ bool initialize(MeshShapeDistanceTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node
const BVHModel<OBBRSS>& model1, const SimpleTransform& tf1,
const S& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
const DistanceRequest& request)
const DistanceRequest& request,
DistanceResult& result)
{
return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
}
......@@ -1075,12 +1101,14 @@ static inline bool setupShapeMeshDistanceOrientedNode(OrientedNode<S, NarrowPhas
const S& model1, const SimpleTransform& tf1,
const BVHModel<BV>& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
const DistanceRequest& request)
const DistanceRequest& request,
DistanceResult& result)
{
if(model2.getModelType() != BVH_MODEL_TRIANGLES)
return false;
node.request = request;
node.result = &result;
node.model1 = &model1;
node.tf1 = tf1;
......@@ -1106,9 +1134,10 @@ bool initialize(ShapeMeshDistanceTraversalNodeRSS<S, NarrowPhaseSolver>& node,
const S& model1, const SimpleTransform& tf1,
const BVHModel<RSS>& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
const DistanceRequest& request)
const DistanceRequest& request,
DistanceResult& result)
{
return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
}
template<typename S, typename NarrowPhaseSolver>
......@@ -1116,9 +1145,10 @@ bool initialize(ShapeMeshDistanceTraversalNodekIOS<S, NarrowPhaseSolver>& node,
const S& model1, const SimpleTransform& tf1,
const BVHModel<kIOS>& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
const DistanceRequest& request)
const DistanceRequest& request,
DistanceResult& result)
{
return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
}
template<typename S, typename NarrowPhaseSolver>
......@@ -1126,9 +1156,10 @@ bool initialize(ShapeMeshDistanceTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node
const S& model1, const SimpleTransform& tf1,
const BVHModel<OBBRSS>& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
const DistanceRequest& request)
const DistanceRequest& request,
DistanceResult& result)
{
return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
}
......
......@@ -179,7 +179,7 @@ public:
if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, NULL, NULL, NULL))
{
is_intersect = true;
this->result->contacts.push_back(Contact(this->model1, this->model2, primitive_id, Contact::NONE));
this->result->addContact(Contact(this->model1, this->model2, primitive_id, Contact::NONE));
}
}
else
......@@ -187,24 +187,24 @@ public:
if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, &contactp, &penetration, &normal))
{
is_intersect = true;
this->result->contacts.push_back(Contact(this->model1, this->model2, primitive_id, Contact::NONE, contactp, -normal, penetration));
this->result->addContact(Contact(this->model1, this->model2, primitive_id, Contact::NONE, contactp, -normal, penetration));
}
}
if(is_intersect && this->request.enable_cost && (this->request.num_max_cost_sources > this->result->cost_sources.size()))
if(is_intersect && this->request.enable_cost && (this->request.num_max_cost_sources > this->result->numCostSources()))
{
AABB overlap_part;
AABB shape_aabb;
computeBV<AABB, S>(*(this->model2), this->tf2, shape_aabb);
AABB(p1, p2, p3).overlap(shape_aabb, overlap_part);
this->result->cost_sources.push_back(CostSource(overlap_part.min_, overlap_part.max_, cost_density));
this->result->addCostSource(CostSource(overlap_part.min_, overlap_part.max_, cost_density));
}
}
bool canStop() const
{
return (this->result->contacts.size() > 0) && (!this->request.exhaustive) && (this->request.num_max_contacts <= this->result->contacts.size()) && (this->request.num_max_cost_sources <= this->result->cost_sources.size()) &&
( (!this->request.enable_cost) || (this->request.num_max_cost_sources <= this->result->cost_sources.size()) );
return (this->result->numContacts() > 0) && (!this->request.exhaustive) && (this->request.num_max_contacts <= this->result->numContacts()) && (this->request.num_max_cost_sources <= this->result->numCostSources()) &&
( (!this->request.enable_cost) || (this->request.num_max_cost_sources <= this->result->numCostSources()) );
}
Vec3f* vertices;
......@@ -253,7 +253,7 @@ static inline void meshShapeCollisionOrientedNodeLeafTesting(int b1, int b2,
if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1, NULL, NULL, NULL))
{
is_intersect = true;
result.contacts.push_back(Contact(model1, &model2, primitive_id, Contact::NONE));
result.addContact(Contact(model1, &model2, primitive_id, Contact::NONE));
}
}
else
......@@ -261,17 +261,17 @@ static inline void meshShapeCollisionOrientedNodeLeafTesting(int b1, int b2,
if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1, &contactp, &penetration, &normal))
{
is_intersect = true;
result.contacts.push_back(Contact(model1, &model2, primitive_id, Contact::NONE, contactp, -normal, penetration));
result.addContact(Contact(model1, &model2, primitive_id, Contact::NONE, contactp, -normal, penetration));
}
}
if(is_intersect && request.enable_cost && (request.num_max_cost_sources > result.cost_sources.size()))
if(is_intersect && request.enable_cost && (request.num_max_cost_sources > result.numCostSources()))
{
AABB overlap_part;
AABB shape_aabb;
computeBV<AABB, S>(model2, tf2, shape_aabb);
AABB(tf1.transform(p1), tf1.transform(p2), tf1.transform(p2)).overlap(shape_aabb, overlap_part);
result.cost_sources.push_back(CostSource(overlap_part.min_, overlap_part.max_, cost_density));
result.addCostSource(CostSource(overlap_part.min_, overlap_part.max_, cost_density));
}
}
......@@ -401,7 +401,7 @@ public:
if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, NULL, NULL, NULL))
{
is_intersect = true;
this->result->contacts.push_back(Contact(this->model1, this->model2, Contact::NONE, primitive_id));
this->result->addContact(Contact(this->model1, this->model2, Contact::NONE, primitive_id));
}
}
else
......@@ -409,24 +409,24 @@ public:
if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, &contactp, &penetration, &normal))
{
is_intersect = true;
this->result->contacts.push_back(Contact(this->model1, this->model2, Contact::NONE, primitive_id, contactp, normal, penetration));
this->result->addContact(Contact(this->model1, this->model2, Contact::NONE, primitive_id, contactp, normal, penetration));
}
}
if(is_intersect && this->request.enable_cost && (this->request.num_max_cost_sources > this->result->cost_sources.size()))
if(is_intersect && this->request.enable_cost && (this->request.num_max_cost_sources > this->result->numCostSources()))
{
AABB overlap_part;
AABB shape_aabb;
computeBV<AABB, S>(*(this->model1), this->tf1, shape_aabb);
AABB(p1, p2, p3).overlap(shape_aabb, overlap_part);
this->result->cost_sources.push_back(CostSource(overlap_part.min_, overlap_part.max_, cost_density));
this->result->addCostSource(CostSource(overlap_part.min_, overlap_part.max_, cost_density));
}
}
bool canStop() const
{
return (this->result->contacts.size() > 0) && (!this->request.exhaustive) && (this->request.num_max_contacts <= this->result->contacts.size()) &&
( (!this->request.enable_cost) || (this->request.num_max_cost_sources <= this->result->cost_sources.size()) );
return (this->result->numContacts() > 0) && (!this->request.exhaustive) && (this->request.num_max_contacts <= this->result->numContacts()) &&
( (!this->request.enable_cost) || (this->request.num_max_cost_sources <= this->result->numCostSources()) );
}
Vec3f* vertices;
......@@ -633,13 +633,9 @@ public:
vertices = NULL;
tri_indices = NULL;
last_tri_id = 0;
rel_err = 0;
abs_err = 0;
min_distance = std::numeric_limits<FCL_REAL>::max();
nsolver = NULL;
}
......@@ -660,17 +656,12 @@ public:
FCL_REAL d;
nsolver->shapeTriangleDistance(*(this->model2), this->tf2, p1, p2, p3, &d);
if(d < min_distance)
{
min_distance = d;
last_tri_id = primitive_id;
}
this->result->update(d, this->model1, this->model2, primitive_id, DistanceResult::NONE);
}
bool canStop(FCL_REAL c) const
{
if((c >= min_distance - abs_err) && (c * (1 + rel_err) >= min_distance))
if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance))
return true;
return false;
}
......@@ -680,10 +671,7 @@ public:
FCL_REAL rel_err;
FCL_REAL abs_err;
mutable FCL_REAL min_distance;
mutable int last_tri_id;
const NarrowPhaseSolver* nsolver;
};
......@@ -699,8 +687,8 @@ void meshShapeDistanceOrientedNodeLeafTesting(int b1, int b2,
const NarrowPhaseSolver* nsolver,
bool enable_statistics,
int & num_leaf_tests,
int& last_tri_id,
FCL_REAL& min_distance)
const DistanceRequest& request,
DistanceResult& result)
{
if(enable_statistics) num_leaf_tests++;
......@@ -712,51 +700,38 @@ void meshShapeDistanceOrientedNodeLeafTesting(int b1, int b2,
const Vec3f& p2 = vertices[tri_id[1]];
const Vec3f& p3 = vertices[tri_id[2]];
FCL_REAL dist;
nsolver->shapeTriangleDistance(model2, tf2, p1, p2, p3, tf1, &dist);
if(dist < min_distance)
{
min_distance = dist;
last_tri_id = primitive_id;
}
}
FCL_REAL distance;
nsolver->shapeTriangleDistance(model2, tf2, p1, p2, p3, tf1, &distance);
result.update(distance, model1, &model2, primitive_id, DistanceResult::NONE);
}
template<typename S, typename NarrowPhaseSolver>
static inline void distance_preprocess(Vec3f* vertices, Triangle* tri_indices, int last_tri_id,
const S& s, const SimpleTransform& tf1, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
FCL_REAL& min_distance)
template<typename BV, typename S, typename NarrowPhaseSolver>
static inline void distancePreprocessOrientedNode(const BVHModel<BV>* model1,
Vec3f* vertices, Triangle* tri_indices, int init_tri_id,
const S& model2, const SimpleTransform& tf1, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
const DistanceRequest& request,
DistanceResult& result)
{
const Triangle& last_tri = tri_indices[last_tri_id