diff --git a/trunk/fcl/include/fcl/collision.h b/trunk/fcl/include/fcl/collision.h
index d9f59f8d9b44209d4ec8ab66c03834a08d29d302..41413ff0aba6b619ba8d14c9290b1f6b935612c8 100644
--- a/trunk/fcl/include/fcl/collision.h
+++ b/trunk/fcl/include/fcl/collision.h
@@ -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>
diff --git a/trunk/fcl/include/fcl/collision_data.h b/trunk/fcl/include/fcl/collision_data.h
index f42f76dee79907efce70e41aeb2653d97077265b..f72af25eab93362f95983b567ade2a0bfa5877b4 100644
--- a/trunk/fcl/include/fcl/collision_data.h
+++ b/trunk/fcl/include/fcl/collision_data.h
@@ -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;
}
};
diff --git a/trunk/fcl/include/fcl/simple_setup.h b/trunk/fcl/include/fcl/simple_setup.h
index 626ac30208a407d2f4f012ba4bdfb78fe89c84e2..d94045adcccf5d41f8f7ec9c21189da7ceaf9fdd 100644
--- a/trunk/fcl/include/fcl/simple_setup.h
+++ b/trunk/fcl/include/fcl/simple_setup.h
@@ -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);
}
diff --git a/trunk/fcl/include/fcl/traversal_node_bvh_shape.h b/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
index d91c6baf712523b4b7025046be7f6475c2fbc87f..b812b6e6ae4fcf8a1de644c23140b82216d49013 100644
--- a/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
+++ b/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
@@ -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];
-
- const Vec3f& p1 = vertices[last_tri[0]];
- const Vec3f& p2 = vertices[last_tri[1]];
- const Vec3f& p3 = vertices[last_tri[2]];
-
- Vec3f last_tri_P, last_tri_Q;
+ const Triangle& init_tri = tri_indices[init_tri_id];
- FCL_REAL dist;
- nsolver->shapeTriangleDistance(s, tf2, p1, p2, p3, tf1, &dist);
+ const Vec3f& p1 = vertices[init_tri[0]];
+ const Vec3f& p2 = vertices[init_tri[1]];
+ const Vec3f& p3 = vertices[init_tri[2]];
- min_distance = dist;
+ FCL_REAL distance;
+ nsolver->shapeTriangleDistance(model2, tf2, p1, p2, p3, tf1, &distance);
+
+ result.update(distance, model1, &model2, init_tri_id, DistanceResult::NONE);
}
-static inline void distance_postprocess(const SimpleTransform& tf, Vec3f& p2)
-{
- const SimpleQuaternion& inv_q = conj(tf.getQuatRotation());
- p2 = inv_q.transform(p2 - tf.getTranslation());
- // Vec3f u = p2 - T;
- // p2 = R.transposeTimes(u);
}
+
+
template<typename S, typename NarrowPhaseSolver>
class MeshShapeDistanceTraversalNodeRSS : public MeshShapeDistanceTraversalNode<RSS, S, NarrowPhaseSolver>
{
@@ -767,12 +742,12 @@ public:
void preprocess()
{
- distance_preprocess(this->vertices, this->tri_indices, this->last_tri_id, *(this->model2), this->tf1, this->tf2, this->nsolver, this->min_distance);
+ details::distancePreprocessOrientedNode(this->model1, this->vertices, this->tri_indices, 0,
+ *(this->model2), this->tf1, this->tf2, this->nsolver, this->request, *(this->result));
}
void postprocess()
{
-
}
FCL_REAL BVTesting(int b1, int b2) const
@@ -784,7 +759,7 @@ public:
void leafTesting(int b1, int b2) const
{
details::meshShapeDistanceOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
- this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
+ this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result));
}
};
@@ -799,12 +774,12 @@ public:
void preprocess()
{
- distance_preprocess(this->vertices, this->tri_indices, this->last_tri_id, *(this->model2), this->tf1, this->tf2, this->nsolver, this->min_distance);
+ details::distancePreprocessOrientedNode(this->model1, this->vertices, this->tri_indices, 0,
+ *(this->model2), this->tf1, this->tf2, this->nsolver, this->request, *(this->result));
}
void postprocess()
- {
-
+ {
}
FCL_REAL BVTesting(int b1, int b2) const
@@ -816,7 +791,7 @@ public:
void leafTesting(int b1, int b2) const
{
details::meshShapeDistanceOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
- this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
+ this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result));
}
};
@@ -831,7 +806,8 @@ public:
void preprocess()
{
- distance_preprocess(this->vertices, this->tri_indices, this->last_tri_id, *(this->model2), this->tf1, this->tf2, this->nsolver, this->min_distance);
+ details::distancePreprocessOrientedNode(this->model1, this->vertices, this->tri_indices, 0,
+ *(this->model2), this->tf1, this->tf2, this->nsolver, this->request, *(this->result));
}
void postprocess()
@@ -848,7 +824,7 @@ public:
void leafTesting(int b1, int b2) const
{
details::meshShapeDistanceOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
- this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
+ this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result));
}
};
@@ -863,13 +839,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;
}
@@ -887,20 +859,15 @@ public:
const Vec3f& p2 = vertices[tri_id[1]];
const Vec3f& p3 = vertices[tri_id[2]];
- FCL_REAL d;
- nsolver->shapeTriangleDistance(*(this->model1), this->tf1, p1, p2, p3, &d);
+ FCL_REAL distance;
+ nsolver->shapeTriangleDistance(*(this->model1), this->tf1, p1, p2, p3, &distance);
- if(d < min_distance)
- {
- min_distance = d;
-
- last_tri_id = primitive_id;
- }
+ this->result->update(distance, this->model1, this->model2, DistanceResult::NONE, primitive_id);
}
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;
}
@@ -910,10 +877,7 @@ public:
FCL_REAL rel_err;
FCL_REAL abs_err;
-
- mutable FCL_REAL min_distance;
- mutable int last_tri_id;
-
+
const NarrowPhaseSolver* nsolver;
};
@@ -927,12 +891,12 @@ public:
void preprocess()
{
- distance_preprocess(this->vertices, this->tri_indices, this->last_tri_id, *(this->model1), this->tf2, this->tf1, this->nsolver, this->min_distance);
+ details::distancePreprocessOrientedNode(this->model2, this->vertices, this->tri_indices, 0,
+ *(this->model1), this->tf2, this->tf1, this->nsolver, this->request, *(this->result));
}
void postprocess()
{
-
}
FCL_REAL BVTesting(int b1, int b2) const
@@ -944,7 +908,7 @@ public:
void leafTesting(int b1, int b2) const
{
details::meshShapeDistanceOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices,
- this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
+ this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result));
}
};
@@ -959,12 +923,12 @@ public:
void preprocess()
{
- distance_preprocess(this->vertices, this->tri_indices, this->last_tri_id, *(this->model1), this->tf2, this->tf1, this->nsolver, this->min_distance);
+ details::distancePreprocessOrientedNode(this->model2, this->vertices, this->tri_indices, 0,
+ *(this->model1), this->tf2, this->tf1, this->nsolver, *(this->result));
}
void postprocess()
{
-
}
FCL_REAL BVTesting(int b1, int b2) const
@@ -976,7 +940,7 @@ public:
void leafTesting(int b1, int b2) const
{
details::meshShapeDistanceOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices,
- this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
+ this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result));
}
};
@@ -991,12 +955,12 @@ public:
void preprocess()
{
- distance_preprocess(this->vertices, this->tri_indices, this->last_tri_id, *(this->model1), this->tf2, this->tf1, this->nsolver, this->min_distance);
+ details::distancePreprocessOrientedNode(this->model2, this->vertices, this->tri_indices, 0,
+ *(this->model1), this->tf2, this->tf1, this->nsolver, *(this->result));
}
void postprocess()
- {
-
+ {
}
FCL_REAL BVTesting(int b1, int b2) const
@@ -1008,7 +972,7 @@ public:
void leafTesting(int b1, int b2) const
{
details::meshShapeDistanceOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices,
- this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
+ this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result));
}
};
diff --git a/trunk/fcl/include/fcl/traversal_node_bvhs.h b/trunk/fcl/include/fcl/traversal_node_bvhs.h
index cb29948de1adcd7b54af64fce0dc77885fba8a36..f0bd374cd16007d6bd3f43f3ee9294b554bda708 100644
--- a/trunk/fcl/include/fcl/traversal_node_bvhs.h
+++ b/trunk/fcl/include/fcl/traversal_node_bvhs.h
@@ -187,7 +187,7 @@ public:
if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3))
{
is_intersect = true;
- this->result->contacts.push_back(Contact(this->model1, this->model2, primitive_id1, primitive_id2));
+ this->result->addContact(Contact(this->model1, this->model2, primitive_id1, primitive_id2));
}
}
else // need compute the contact information
@@ -201,31 +201,31 @@ public:
is_intersect = true;
if(!this->request.exhaustive)
{
- if(this->request.num_max_contacts < n_contacts + this->result->contacts.size())
- n_contacts = (this->request.num_max_contacts >= this->result->contacts.size()) ? (this->request.num_max_contacts - this->result->contacts.size()) : 0;
+ if(this->request.num_max_contacts < n_contacts + this->result->numContacts())
+ n_contacts = (this->request.num_max_contacts >= this->result->numContacts()) ? (this->request.num_max_contacts - this->result->numContacts()) : 0;
}
for(unsigned int i = 0; i < n_contacts; ++i)
{
- this->result->contacts.push_back(Contact(this->model1, this->model2, primitive_id1, primitive_id2, contacts[i], normal, penetration));
+ this->result->addContact(Contact(this->model1, this->model2, primitive_id1, primitive_id2, contacts[i], 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(p1, p2, p3).overlap(AABB(q1, q2, q3), 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));
}
}
/** \brief Whether the traversal process can stop early */
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* vertices1;
@@ -913,13 +913,8 @@ public:
tri_indices1 = NULL;
tri_indices2 = NULL;
- last_tri_id1 = 0;
- last_tri_id2 = 0;
-
rel_err = 0;
abs_err = 0;
-
- min_distance = std::numeric_limits<FCL_REAL>::max();
}
void leafTesting(int b1, int b2) const
@@ -949,21 +944,19 @@ public:
FCL_REAL d = TriangleDistance::triDistance(t11, t12, t13, t21, t22, t23,
P1, P2);
- if(d < min_distance)
+ if(this->request.enable_nearest_points)
{
- min_distance = d;
-
- p1 = P1;
- p2 = P2;
-
- last_tri_id1 = primitive_id1;
- last_tri_id2 = primitive_id2;
+ this->result->update(d, this->model1, this->model2, primitive_id1, primitive_id2, P1, P2);
}
- }
+ else
+ {
+ this->result->update(d, this->model1, this->model2, primitive_id1, primitive_id2);
+ }
+ }
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;
}
@@ -977,17 +970,6 @@ public:
/** \brief relative and absolute error, default value is 0.01 for both terms */
FCL_REAL rel_err;
FCL_REAL abs_err;
-
- /** \brief distance and points establishing the minimum distance for the models, within the relative and absolute error bounds specified.
- * p1 is in model1's local coordinate system while p2 is in model2's local coordinate system
- */
- mutable FCL_REAL min_distance;
- mutable Vec3f p1;
- mutable Vec3f p2;
-
- /** \brief Remember the nearest neighbor points */
- mutable int last_tri_id1;
- mutable int last_tri_id2;
};
@@ -1196,6 +1178,12 @@ public:
}
}
+ mutable FCL_REAL min_distance;
+
+ mutable Vec3f p1, p2;
+
+ mutable int last_tri_id1, last_tri_id2;
+
/** \brief CA controlling variable: early stop for the early iterations of CA */
FCL_REAL w;
diff --git a/trunk/fcl/include/fcl/traversal_node_octree.h b/trunk/fcl/include/fcl/traversal_node_octree.h
index fd2d082cfc9f4359ef19674c1bbc6da5c75eb919..2b61055754df7f348048f2f8a7271541db60a503 100644
--- a/trunk/fcl/include/fcl/traversal_node_octree.h
+++ b/trunk/fcl/include/fcl/traversal_node_octree.h
@@ -83,16 +83,17 @@ public:
}
- FCL_REAL OcTreeDistance(const OcTree* tree1, const OcTree* tree2,
- const SimpleTransform& tf1, const SimpleTransform& tf2) const
+ void OcTreeDistance(const OcTree* tree1, const OcTree* tree2,
+ const SimpleTransform& tf1, const SimpleTransform& tf2,
+ const DistanceRequest& request_,
+ DistanceResult& result_) const
{
- FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
+ drequest = &request_;
+ dresult = &result_;
OcTreeDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
tree2, tree2->getRoot(), tree2->getRootBV(),
- tf1, tf2, min_dist);
-
- return min_dist;
+ tf1, tf2);
}
template<typename BV>
@@ -110,16 +111,17 @@ public:
}
template<typename BV>
- FCL_REAL OcTreeMeshDistance(const OcTree* tree1, const BVHModel<BV>* tree2,
- const SimpleTransform& tf1, const SimpleTransform& tf2) const
+ void OcTreeMeshDistance(const OcTree* tree1, const BVHModel<BV>* tree2,
+ const SimpleTransform& tf1, const SimpleTransform& tf2,
+ const DistanceRequest& request_,
+ DistanceResult& result_) const
{
- FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
+ drequest = &request_;
+ dresult = &result_;
OcTreeMeshDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
tree2, 0,
- tf1, tf2, min_dist);
-
- return min_dist;
+ tf1, tf2);
}
@@ -140,16 +142,17 @@ public:
template<typename BV>
- FCL_REAL MeshOcTreeDistance(const BVHModel<BV>* tree1, const OcTree* tree2,
- const SimpleTransform& tf1, const SimpleTransform& tf2) const
+ void MeshOcTreeDistance(const BVHModel<BV>* tree1, const OcTree* tree2,
+ const SimpleTransform& tf1, const SimpleTransform& tf2,
+ const DistanceRequest& request_,
+ DistanceResult& result_) const
{
- FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
+ drequest = &request_;
+ dresult = &result_;
OcTreeMeshDistanceRecurse(tree1, 0,
tree2, tree2->getRoot(), tree2->getRootBV(),
- tf1, tf2, min_dist);
-
- return min_dist;
+ tf1, tf2);
}
template<typename S>
@@ -190,32 +193,35 @@ public:
}
template<typename S>
- FCL_REAL OcTreeShapeDistance(const OcTree* tree, const S& s,
- const SimpleTransform& tf1, const SimpleTransform& tf2) const
+ void OcTreeShapeDistance(const OcTree* tree, const S& s,
+ const SimpleTransform& tf1, const SimpleTransform& tf2,
+ const DistanceRequest& request_,
+ DistanceResult& result_) const
{
+ drequest = &request_;
+ dresult = &result_;
+
AABB aabb2;
computeBV<AABB>(s, tf2, aabb2);
- FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
-
OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(),
s, aabb2,
- tf1, tf2, min_dist);
- return min_dist;
+ tf1, tf2);
}
template<typename S>
- FCL_REAL ShapeOcTreeDistance(const S& s, const OcTree* tree,
- const SimpleTransform& tf1, const SimpleTransform& tf2) const
+ void ShapeOcTreeDistance(const S& s, const OcTree* tree,
+ const SimpleTransform& tf1, const SimpleTransform& tf2,
+ const DistanceRequest& request_,
+ DistanceResult& result_) const
{
+ drequest = &request_;
+ dresult = &result_;
+
AABB aabb1;
computeBV<AABB>(s, tf1, aabb1);
- FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
-
OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(),
s, aabb1,
- tf2, tf1, min_dist);
-
- return min_dist;
+ tf2, tf1);
}
@@ -223,8 +229,7 @@ private:
template<typename S>
bool OcTreeShapeDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
const S& s, const AABB& aabb2,
- const SimpleTransform& tf1, const SimpleTransform& tf2,
- FCL_REAL& min_dist) const
+ const SimpleTransform& tf1, const SimpleTransform& tf2) const
{
if(!root1->hasChildren())
{
@@ -236,9 +241,10 @@ private:
FCL_REAL dist;
solver->shapeDistance(box, box_tf, s, tf2, &dist);
- if(dist < min_dist) min_dist = dist;
- return (min_dist <= 0);
+ dresult->update(dist, tree1, &s, root1 - tree1->getRoot(), DistanceResult::NONE);
+
+ return (dresult->min_distance <= 0);
}
else
return false;
@@ -257,9 +263,9 @@ private:
AABB aabb1;
convertBV(child_bv, tf1, aabb1);
FCL_REAL d = aabb1.distance(aabb2);
- if(d < min_dist)
+ if(d < dresult->min_distance)
{
- if(OcTreeShapeDistanceRecurse(tree1, child, child_bv, s, aabb2, tf1, tf2, min_dist))
+ if(OcTreeShapeDistanceRecurse(tree1, child, child_bv, s, aabb2, tf1, tf2))
return true;
}
}
@@ -341,7 +347,7 @@ private:
template<typename BV>
bool OcTreeMeshDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
const BVHModel<BV>* tree2, int root2,
- const SimpleTransform& tf1, const SimpleTransform& tf2, FCL_REAL& min_dist) const
+ const SimpleTransform& tf1, const SimpleTransform& tf2) const
{
if(!root1->hasChildren() && tree2->getBV(root2).isLeaf())
{
@@ -359,8 +365,10 @@ private:
FCL_REAL dist;
solver->shapeTriangleDistance(box, box_tf, p1, p2, p3, tf2, &dist);
- if(dist < min_dist) min_dist = dist;
- return (min_dist <= 0);
+
+ dresult->update(dist, tree1, tree2, root1 - tree1->getRoot(), primitive_id);
+
+ return (dresult->min_distance <= 0);
}
else
return false;
@@ -384,9 +392,9 @@ private:
convertBV(tree2->getBV(root2).bv, tf2, aabb2);
d = aabb1.distance(aabb2);
- if(d < min_dist)
+ if(d < dresult->min_distance)
{
- if(OcTreeMeshDistanceRecurse(tree1, child, child_bv, tree2, root2, tf1, tf2, min_dist))
+ if(OcTreeMeshDistanceRecurse(tree1, child, child_bv, tree2, root2, tf1, tf2))
return true;
}
}
@@ -401,9 +409,9 @@ private:
convertBV(tree2->getBV(child).bv, tf2, aabb2);
d = aabb1.distance(aabb2);
- if(d < min_dist)
+ if(d < dresult->min_distance)
{
- if(OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, tf2, min_dist))
+ if(OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, tf2))
return true;
}
@@ -411,9 +419,9 @@ private:
convertBV(tree2->getBV(child).bv, tf2, aabb2);
d = aabb1.distance(aabb2);
- if(d < min_dist)
+ if(d < dresult->min_distance)
{
- if(OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, tf2, min_dist))
+ if(OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, tf2))
return true;
}
}
@@ -506,8 +514,7 @@ private:
bool OcTreeDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& bv2,
- const SimpleTransform& tf1, const SimpleTransform& tf2,
- FCL_REAL& min_dist) const
+ const SimpleTransform& tf1, const SimpleTransform& tf2) const
{
if(!root1->hasChildren() && !root2->hasChildren())
{
@@ -520,9 +527,10 @@ private:
FCL_REAL dist;
solver->shapeDistance(box1, box1_tf, box2, box2_tf, &dist);
- if(dist < min_dist) min_dist = dist;
+
+ dresult->update(dist, tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot());
- return (min_dist <= 0);
+ return (dresult->min_distance <= 0);
}
else
return false;
@@ -546,10 +554,10 @@ private:
convertBV(bv2, tf2, aabb2);
d = aabb1.distance(aabb2);
- if(d < min_dist)
+ if(d < dresult->min_distance)
{
- if(OcTreeDistanceRecurse(tree1, child, child_bv, tree2, root2, bv2, tf1, tf2, min_dist))
+ if(OcTreeDistanceRecurse(tree1, child, child_bv, tree2, root2, bv2, tf1, tf2))
return true;
}
}
@@ -571,9 +579,9 @@ private:
convertBV(bv2, tf2, aabb2);
d = aabb1.distance(aabb2);
- if(d < min_dist)
+ if(d < dresult->min_distance)
{
- if(OcTreeDistanceRecurse(tree1, root1, bv1, tree2, child, child_bv, tf1, tf2, min_dist))
+ if(OcTreeDistanceRecurse(tree1, root1, bv1, tree2, child, child_bv, tf1, tf2))
return true;
}
}
@@ -722,13 +730,11 @@ public:
void leafTesting(int, int) const
{
- min_distance = otsolver->OcTreeDistance(model1, model2, tf1, tf2);
+ otsolver->OcTreeDistance(model1, model2, tf1, tf2, request, *result);
}
const OcTree* model1;
const OcTree* model2;
-
- mutable FCL_REAL min_distance;
const OcTreeSolver<NarrowPhaseSolver>* otsolver;
};
@@ -812,13 +818,11 @@ public:
void leafTesting(int, int) const
{
- min_distance = otsolver->OcTreeShapeDistance(model2, *model1, tf2, tf1);
+ otsolver->OcTreeShapeDistance(model2, *model1, tf2, tf1, request, *result);
}
const S* model1;
const OcTree* model2;
-
- mutable FCL_REAL min_distance;
const OcTreeSolver<NarrowPhaseSolver>* otsolver;
};
@@ -842,13 +846,11 @@ public:
void leafTesting(int, int) const
{
- min_distance = otsolver->OcTreeShapeDistance(model1, *model2, tf1, tf2);
+ otsolver->OcTreeShapeDistance(model1, *model2, tf1, tf2, request, *result);
}
const OcTree* model1;
const S* model2;
-
- mutable FCL_REAL min_distance;
const OcTreeSolver<NarrowPhaseSolver>* otsolver;
};
@@ -934,13 +936,11 @@ public:
void leafTesting(int, int) const
{
- min_distance = otsolver->OcTreeMeshDistance(model2, model1, tf2, tf1);
+ otsolver->OcTreeMeshDistance(model2, model1, tf2, tf1, request, *result);
}
const BVHModel<BV>* model1;
const OcTree* model2;
-
- mutable FCL_REAL min_distance;
const OcTreeSolver<NarrowPhaseSolver>* otsolver;
@@ -965,13 +965,11 @@ public:
void leafTesting(int, int) const
{
- min_distance = otsolver->OcTreeMeshDistance(model1, model2, tf1, tf2);
+ otsolver->OcTreeMeshDistance(model1, model2, tf1, tf2, request, *result);
}
const OcTree* model1;
const BVHModel<BV>* model2;
-
- mutable FCL_REAL min_distance;
const OcTreeSolver<NarrowPhaseSolver>* otsolver;
diff --git a/trunk/fcl/include/fcl/traversal_node_shapes.h b/trunk/fcl/include/fcl/traversal_node_shapes.h
index 9f8065f9e30f33e15c545768b7dbb27c24b14cbd..2c2da37921ef0e960620a27b267433e715178e30 100644
--- a/trunk/fcl/include/fcl/traversal_node_shapes.h
+++ b/trunk/fcl/include/fcl/traversal_node_shapes.h
@@ -73,7 +73,7 @@ public:
if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, &contact_point, &penetration_depth, &normal))
{
is_collision = true;
- result->contacts.push_back(Contact(model1, model2, Contact::NONE, Contact::NONE, contact_point, normal, penetration_depth));
+ result->addContact(Contact(model1, model2, Contact::NONE, Contact::NONE, contact_point, normal, penetration_depth));
}
}
else
@@ -81,7 +81,7 @@ public:
if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, NULL, NULL, NULL))
{
is_collision = true;
- result->contacts.push_back(Contact(model1, model2, Contact::NONE, Contact::NONE));
+ result->addContact(Contact(model1, model2, Contact::NONE, Contact::NONE));
}
}
@@ -92,7 +92,7 @@ public:
computeBV<AABB, S2>(*model2, tf2, aabb2);
AABB overlap_part;
aabb1.overlap(aabb2, 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));
}
}
@@ -123,18 +123,14 @@ public:
void leafTesting(int, int) const
{
- is_collision = !nsolver->shapeDistance(*model1, tf1, *model2, tf2, &min_distance);
+ FCL_REAL distance;
+ !nsolver->shapeDistance(*model1, tf1, *model2, tf2, &distance);
+ result->update(distance, model1, model2, DistanceResult::NONE, DistanceResult::NONE);
}
const S1* model1;
const S2* model2;
- mutable FCL_REAL min_distance;
- mutable Vec3f p1;
- mutable Vec3f p2;
-
- mutable bool is_collision;
-
const NarrowPhaseSolver* nsolver;
};
diff --git a/trunk/fcl/src/broad_phase_collision.cpp b/trunk/fcl/src/broad_phase_collision.cpp
index 3145339624175af33712f988fc8bbe40227594c3..cab7b423a375c881ae26a34600e2e0eec9a07960 100644
--- a/trunk/fcl/src/broad_phase_collision.cpp
+++ b/trunk/fcl/src/broad_phase_collision.cpp
@@ -57,18 +57,8 @@ bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cd
if(cdata->done) return true;
- CollisionResult local_result;
- int num_contacts = collide(o1, o2, request, local_result);
+ collide(o1, o2, request, result);
- std::vector<Contact>& out_contacts = result.contacts;
- const std::vector<Contact>& in_contacts = local_result.contacts;
- for(int i = 0; i < num_contacts; ++i)
- {
- out_contacts.push_back(in_contacts[i]);
- // result.contacts.push_back(local_result.contacts[i]);
- }
-
- // set done flag
if( (!request.exhaustive) && (result.isCollision()) && (result.numContacts() >= request.num_max_contacts))
cdata->done = true;
diff --git a/trunk/fcl/src/collision_func_matrix.cpp b/trunk/fcl/src/collision_func_matrix.cpp
index 74b9bd18df05fdbd141eb8811bfae8058db40ad6..5b88cea63c144e61445774a0253bcccb30048a20 100644
--- a/trunk/fcl/src/collision_func_matrix.cpp
+++ b/trunk/fcl/src/collision_func_matrix.cpp
@@ -45,39 +45,23 @@
namespace fcl
{
-template<typename T_SH>
-static inline int OcTreeShapeContactCollection(const std::vector<Contact>& pairs, const OcTree* obj1, const T_SH* obj2,
- const CollisionRequest& request, CollisionResult& result)
-{
- int num_contacts = pairs.size();
- if(num_contacts > 0)
- {
- if((!request.exhaustive) && (num_contacts > request.num_max_contacts)) num_contacts = request.num_max_contacts;
- std::vector<Contact>& contacts = result.contacts;
- contacts.resize(num_contacts);
- for(int i = 0; i < num_contacts; ++i)
- contacts[i] = pairs[i];
- }
-
- return num_contacts;
-}
-
template<typename T_SH, typename NarrowPhaseSolver>
int ShapeOcTreeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
+ if(request.num_max_contacts <= result.numContacts()) return 0;
+ size_t num_contacts_old = result.numContacts();
+
ShapeOcTreeCollisionTraversalNode<T_SH, NarrowPhaseSolver> node;
const T_SH* obj1 = static_cast<const T_SH*>(o1);
const OcTree* obj2 = static_cast<const OcTree*>(o2);
OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
- CollisionResult local_result;
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, local_result);
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
collide(&node);
- int num_contacts = OcTreeShapeContactCollection(local_result.contacts, obj2, obj1, request, result);
- return num_contacts;
+ return result.numContacts() - num_contacts_old;
}
template<typename T_SH, typename NarrowPhaseSolver>
@@ -85,33 +69,18 @@ int OcTreeShapeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
+ if(request.num_max_contacts <= result.numContacts()) return 0;
+ size_t num_contacts_old = result.numContacts();
+
OcTreeShapeCollisionTraversalNode<T_SH, NarrowPhaseSolver> node;
const OcTree* obj1 = static_cast<const OcTree*>(o1);
const T_SH* obj2 = static_cast<const T_SH*>(o2);
OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
- CollisionResult local_result;
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, local_result);
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
collide(&node);
- int num_contacts = OcTreeShapeContactCollection(local_result.contacts, obj1, obj2, request, result);
-
- return num_contacts;
-}
-static inline int OcTreeContactCollection(const std::vector<Contact>& pairs, const OcTree* obj1, const OcTree* obj2,
- const CollisionRequest& request, CollisionResult& result)
-{
- int num_contacts = pairs.size();
- if(num_contacts > 0)
- {
- if((!request.exhaustive) && (num_contacts > request.num_max_contacts)) num_contacts = request.num_max_contacts;
- std::vector<Contact>& contacts = result.contacts;
- contacts.resize(num_contacts);
- for(int i = 0; i < num_contacts; ++i)
- contacts[i] = pairs[i];
- }
-
- return num_contacts;
+ return result.numContacts() - num_contacts_old;
}
template<typename NarrowPhaseSolver>
@@ -119,34 +88,18 @@ int OcTreeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
+ if(request.num_max_contacts <= result.numContacts()) return 0;
+ size_t num_contacts_old = result.numContacts();
+
OcTreeCollisionTraversalNode<NarrowPhaseSolver> node;
const OcTree* obj1 = static_cast<const OcTree*>(o1);
const OcTree* obj2 = static_cast<const OcTree*>(o2);
OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
- CollisionResult local_result;
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, local_result);
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
collide(&node);
- int num_contacts = OcTreeContactCollection(local_result.contacts, obj1, obj2, request, result);
- return num_contacts;
-}
-
-template<typename T_BVH>
-static inline int OcTreeBVHContactCollection(const std::vector<Contact>& pairs, const OcTree* obj1, const BVHModel<T_BVH>* obj2,
- const CollisionRequest& request, CollisionResult& result)
-{
- int num_contacts = pairs.size();
- if(num_contacts > 0)
- {
- if((!request.exhaustive) && (num_contacts > request.num_max_contacts)) num_contacts = request.num_max_contacts;
- std::vector<Contact>& contacts = result.contacts;
- contacts.resize(num_contacts);
- for(int i = 0; i < num_contacts; ++i)
- contacts[i] = pairs[i];
- }
-
- return num_contacts;
+ return result.numContacts() - num_contacts_old;
}
template<typename T_BVH, typename NarrowPhaseSolver>
@@ -154,16 +107,18 @@ int OcTreeBVHCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, co
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
+ if(request.num_max_contacts <= result.numContacts()) return 0;
+ size_t num_contacts_old = result.numContacts();
+
OcTreeMeshCollisionTraversalNode<T_BVH, NarrowPhaseSolver> node;
const OcTree* obj1 = static_cast<const OcTree*>(o1);
const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>*>(o2);
OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
- CollisionResult local_result;
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, local_result);
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
collide(&node);
- int num_contacts = OcTreeBVHContactCollection(local_result.contacts, obj1, obj2, request, result);
- return num_contacts;
+
+ return result.numContacts() - num_contacts_old;
}
template<typename T_BVH, typename NarrowPhaseSolver>
@@ -171,16 +126,18 @@ int BVHOcTreeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, co
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
+ if(request.num_max_contacts <= result.numContacts()) return 0;
+ size_t num_contacts_old = result.numContacts();
+
MeshOcTreeCollisionTraversalNode<T_BVH, NarrowPhaseSolver> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
const OcTree* obj2 = static_cast<const OcTree*>(o2);
OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
- CollisionResult local_result;
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, local_result);
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
collide(&node);
- int num_contacts = OcTreeBVHContactCollection(local_result.contacts, obj2, obj1, request, result);
- return num_contacts;
+
+ return result.numContacts() - num_contacts_old;
}
@@ -189,39 +146,19 @@ int ShapeShapeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, c
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
+ if(request.num_max_contacts <= result.numContacts()) return 0;
+ size_t num_contacts_old = result.numContacts();
+
ShapeCollisionTraversalNode<T_SH1, T_SH2, NarrowPhaseSolver> node;
const T_SH1* obj1 = static_cast<const T_SH1*>(o1);
const T_SH2* obj2 = static_cast<const T_SH2*>(o2);
- CollisionResult local_result;
- initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, local_result);
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
collide(&node);
- if(local_result.contacts.size() == 0) return 0;
- result.contacts.resize(1);
- result.contacts[0] = local_result.contacts[0];
- return 1;
-}
-
-
-
-template<typename T_BVH, typename T_SH>
-static inline int BVHShapeContactCollection(const std::vector<Contact>& pairs, const BVHModel<T_BVH>* obj1, const T_SH* obj2,
- const CollisionRequest& request, CollisionResult& result)
-{
- int num_contacts = pairs.size();
- if(num_contacts > 0)
- {
- if((!request.exhaustive) && (num_contacts > request.num_max_contacts)) num_contacts = request.num_max_contacts;
- std::vector<Contact>& contacts = result.contacts;
- contacts.resize(num_contacts);
- for(int i = 0; i < num_contacts; ++i)
- contacts[i] = pairs[i];
- }
- return num_contacts;
+ return result.numContacts() - num_contacts_old;
}
-
template<typename T_BVH, typename T_SH, typename NarrowPhaseSolver>
struct BVHShapeCollider
{
@@ -229,20 +166,20 @@ struct BVHShapeCollider
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
+ if(request.num_max_contacts <= result.numContacts()) return 0;
+ size_t num_contacts_old = result.numContacts();
+
MeshShapeCollisionTraversalNode<T_BVH, T_SH, NarrowPhaseSolver> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
SimpleTransform tf1_tmp = tf1;
const T_SH* obj2 = static_cast<const T_SH*>(o2);
- CollisionResult local_result;
- initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, local_result);
+ initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, result);
fcl::collide(&node);
- int num_contacts = BVHShapeContactCollection(local_result.contacts, obj1, obj2, request, result);
-
delete obj1_tmp;
- return num_contacts;
+ return result.numContacts() - num_contacts_old;
}
};
@@ -254,15 +191,17 @@ struct BVHShapeCollider<OBB, T_SH, NarrowPhaseSolver>
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
+ if(request.num_max_contacts <= result.numContacts()) return 0;
+ size_t num_contacts_old = result.numContacts();
+
MeshShapeCollisionTraversalNodeOBB<T_SH, NarrowPhaseSolver> node;
const BVHModel<OBB>* obj1 = static_cast<const BVHModel<OBB>* >(o1);
const T_SH* obj2 = static_cast<const T_SH*>(o2);
- CollisionResult local_result;
- initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, local_result);
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
fcl::collide(&node);
- return BVHShapeContactCollection(local_result.contacts, obj1, obj2, request, result);
+ return result.numContacts() - num_contacts_old;
}
};
@@ -274,15 +213,17 @@ struct BVHShapeCollider<RSS, T_SH, NarrowPhaseSolver>
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
+ if(request.num_max_contacts <= result.numContacts()) return 0;
+ size_t num_contacts_old = result.numContacts();
+
MeshShapeCollisionTraversalNodeRSS<T_SH, NarrowPhaseSolver> node;
const BVHModel<RSS>* obj1 = static_cast<const BVHModel<RSS>* >(o1);
const T_SH* obj2 = static_cast<const T_SH*>(o2);
- CollisionResult local_result;
- initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, local_result);
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
fcl::collide(&node);
- return BVHShapeContactCollection(local_result.contacts, obj1, obj2, request, result);
+ return result.numContacts() - num_contacts_old;
}
};
@@ -294,15 +235,17 @@ struct BVHShapeCollider<kIOS, T_SH, NarrowPhaseSolver>
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
+ if(request.num_max_contacts <= result.numContacts()) return 0;
+ size_t num_contacts_old = result.numContacts();
+
MeshShapeCollisionTraversalNodekIOS<T_SH, NarrowPhaseSolver> node;
const BVHModel<kIOS>* obj1 = static_cast<const BVHModel<kIOS>* >(o1);
const T_SH* obj2 = static_cast<const T_SH*>(o2);
- CollisionResult local_result;
- initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, local_result);
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
fcl::collide(&node);
- return BVHShapeContactCollection(local_result.contacts, obj1, obj2, request, result);
+ return result.numContacts() - num_contacts_old;
}
};
@@ -314,39 +257,27 @@ struct BVHShapeCollider<OBBRSS, T_SH, NarrowPhaseSolver>
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
+ if(request.num_max_contacts <= result.numContacts()) return 0;
+ size_t num_contacts_old = result.numContacts();
+
MeshShapeCollisionTraversalNodeOBBRSS<T_SH, NarrowPhaseSolver> node;
const BVHModel<OBBRSS>* obj1 = static_cast<const BVHModel<OBBRSS>* >(o1);
const T_SH* obj2 = static_cast<const T_SH*>(o2);
- CollisionResult local_result;
- initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, local_result);
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
fcl::collide(&node);
- return BVHShapeContactCollection(local_result.contacts, obj1, obj2, request, result);
+ return result.numContacts() - num_contacts_old;
}
};
-template<typename T_BVH>
-static inline int BVHContactCollection(const std::vector<Contact>& pairs, const BVHModel<T_BVH>* obj1, const BVHModel<T_BVH>* obj2, const CollisionRequest& request, CollisionResult& result)
-{
- int num_contacts = pairs.size();
- if(num_contacts > 0)
- {
- if((!request.exhaustive) && (num_contacts > request.num_max_contacts)) num_contacts = request.num_max_contacts;
- std::vector<Contact>& contacts = result.contacts;
- contacts.resize(num_contacts);
- for(int i = 0; i < num_contacts; ++i)
- contacts[i] = pairs[i];
- }
-
- return num_contacts;
-}
-
-
template<typename T_BVH>
int BVHCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const CollisionRequest& request, CollisionResult& result)
{
+ if(request.num_max_contacts <= result.numContacts()) return 0;
+ size_t num_contacts_old = result.numContacts();
+
MeshCollisionTraversalNode<T_BVH> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>* >(o2);
@@ -355,57 +286,62 @@ int BVHCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const Co
BVHModel<T_BVH>* obj2_tmp = new BVHModel<T_BVH>(*obj2);
SimpleTransform tf2_tmp = tf2;
- CollisionResult local_result;
- initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request, local_result);
+ initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request, result);
collide(&node);
- int num_contacts = BVHContactCollection(local_result.contacts, obj1, obj2, request, result);
delete obj1_tmp;
delete obj2_tmp;
- return num_contacts;
+
+ return result.numContacts() - num_contacts_old;
}
template<>
int BVHCollide<OBB>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const CollisionRequest& request, CollisionResult& result)
{
+ if(request.num_max_contacts <= result.numContacts()) return 0;
+ size_t num_contacts_old = result.numContacts();
+
MeshCollisionTraversalNodeOBB node;
const BVHModel<OBB>* obj1 = static_cast<const BVHModel<OBB>* >(o1);
const BVHModel<OBB>* obj2 = static_cast<const BVHModel<OBB>* >(o2);
- CollisionResult local_result;
- initialize(node, *obj1, tf1, *obj2, tf2, request, local_result);
+ initialize(node, *obj1, tf1, *obj2, tf2, request, result);
collide(&node);
- return BVHContactCollection(local_result.contacts, obj1, obj2, request, result);
+ return result.numContacts() - num_contacts_old;
}
template<>
int BVHCollide<OBBRSS>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const CollisionRequest& request, CollisionResult& result)
{
+ if(request.num_max_contacts <= result.numContacts()) return 0;
+ size_t num_contacts_old = result.numContacts();
+
MeshCollisionTraversalNodeOBBRSS node;
const BVHModel<OBBRSS>* obj1 = static_cast<const BVHModel<OBBRSS>* >(o1);
const BVHModel<OBBRSS>* obj2 = static_cast<const BVHModel<OBBRSS>* >(o2);
- CollisionResult local_result;
- initialize(node, *obj1, tf1, *obj2, tf2, request, local_result);
+ initialize(node, *obj1, tf1, *obj2, tf2, request, result);
collide(&node);
- return BVHContactCollection(local_result.contacts, obj1, obj2, request, result);
+ return result.numContacts() - num_contacts_old;
}
template<>
int BVHCollide<kIOS>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const CollisionRequest& request, CollisionResult& result)
{
+ if(request.num_max_contacts <= result.numContacts()) return 0;
+ size_t num_contacts_old = result.numContacts();
+
MeshCollisionTraversalNodekIOS node;
const BVHModel<kIOS>* obj1 = static_cast<const BVHModel<kIOS>* >(o1);
const BVHModel<kIOS>* obj2 = static_cast<const BVHModel<kIOS>* >(o2);
- CollisionResult local_result;
- initialize(node, *obj1, tf1, *obj2, tf2, request, local_result);
+ initialize(node, *obj1, tf1, *obj2, tf2, request, result);
collide(&node);
- return BVHContactCollection(local_result.contacts, obj1, obj2, request, result);
+ return result.numContacts() - num_contacts_old;
}
diff --git a/trunk/fcl/src/conservative_advancement.cpp b/trunk/fcl/src/conservative_advancement.cpp
index d4bd095196770ff09cf89abd3e2d591b9c597ba5..dcad4af3dacc05f8321a6d07a3fcab3c6c258ffa 100644
--- a/trunk/fcl/src/conservative_advancement.cpp
+++ b/trunk/fcl/src/conservative_advancement.cpp
@@ -93,7 +93,7 @@ int conservativeAdvancement(const CollisionGeometry* o1,
collide(&cnode);
- int b = result.contacts.size();
+ int b = result.numContacts();
if(b > 0)
{
diff --git a/trunk/fcl/src/distance_func_matrix.cpp b/trunk/fcl/src/distance_func_matrix.cpp
index 4a4429fc5280d125c278cc2e78d565efb74e3078..66c363ae819b59df20e9e513c36b1906ebd6d344 100644
--- a/trunk/fcl/src/distance_func_matrix.cpp
+++ b/trunk/fcl/src/distance_func_matrix.cpp
@@ -51,11 +51,13 @@ FCL_REAL ShapeOcTreeDistance(const CollisionGeometry* o1, const SimpleTransform&
const T_SH* obj1 = static_cast<const T_SH*>(o1);
const OcTree* obj2 = static_cast<const OcTree*>(o2);
OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request);
+
+ DistanceResult local_result;
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, local_result);
distance(&node);
- result.min_distance = node.min_distance;
- return node.min_distance;
+ result.update(local_result);
+ return local_result.min_distance;
}
template<typename T_SH, typename NarrowPhaseSolver>
@@ -66,11 +68,13 @@ FCL_REAL OcTreeShapeDistance(const CollisionGeometry* o1, const SimpleTransform&
const OcTree* obj1 = static_cast<const OcTree*>(o1);
const T_SH* obj2 = static_cast<const T_SH*>(o2);
OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request);
+
+ DistanceResult local_result;
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, local_result);
distance(&node);
- result.min_distance = node.min_distance;
- return node.min_distance;
+ result.update(local_result);
+ return local_result.min_distance;
}
template<typename NarrowPhaseSolver>
@@ -81,11 +85,13 @@ FCL_REAL OcTreeDistance(const CollisionGeometry* o1, const SimpleTransform& tf1,
const OcTree* obj1 = static_cast<const OcTree*>(o1);
const OcTree* obj2 = static_cast<const OcTree*>(o2);
OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request);
+
+ DistanceResult local_result;
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, local_result);
distance(&node);
- result.min_distance = node.min_distance;
- return node.min_distance;
+ result.update(local_result);
+ return local_result.min_distance;
}
template<typename T_BVH, typename NarrowPhaseSolver>
@@ -96,11 +102,13 @@ FCL_REAL BVHOcTreeDistance(const CollisionGeometry* o1, const SimpleTransform& t
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
const OcTree* obj2 = static_cast<const OcTree*>(o2);
OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request);
+
+ DistanceResult local_result;
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, local_result);
distance(&node);
- result.min_distance = node.min_distance;
- return node.min_distance;
+ result.update(local_result);
+ return local_result.min_distance;
}
template<typename T_BVH, typename NarrowPhaseSolver>
@@ -111,11 +119,13 @@ FCL_REAL OcTreeBVHDistance(const CollisionGeometry* o1, const SimpleTransform& t
const OcTree* obj1 = static_cast<const OcTree*>(o1);
const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>*>(o2);
OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request);
+
+ DistanceResult local_result;
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, local_result);
distance(&node);
- result.min_distance = node.min_distance;
- return node.min_distance;
+ result.update(local_result);
+ return local_result.min_distance;
}
@@ -127,11 +137,13 @@ FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const SimpleTransform&
ShapeDistanceTraversalNode<T_SH1, T_SH2, NarrowPhaseSolver> node;
const T_SH1* obj1 = static_cast<const T_SH1*>(o1);
const T_SH2* obj2 = static_cast<const T_SH2*>(o2);
- initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request);
+
+ DistanceResult local_result;
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, local_result);
distance(&node);
- result.min_distance = node.min_distance;
- return node.min_distance;
+ result.update(local_result);
+ return local_result.min_distance;
}
template<typename T_BVH, typename T_SH, typename NarrowPhaseSolver>
@@ -146,12 +158,13 @@ struct BVHShapeDistancer
SimpleTransform tf1_tmp = tf1;
const T_SH* obj2 = static_cast<const T_SH*>(o2);
- initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request);
+ DistanceResult local_result;
+ initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, local_result);
fcl::distance(&node);
delete obj1_tmp;
- result.min_distance = node.min_distance;
- return node.min_distance;
+ result.update(local_result);
+ return local_result.min_distance;
}
};
@@ -165,11 +178,12 @@ struct BVHShapeDistancer<RSS, T_SH, NarrowPhaseSolver>
const BVHModel<RSS>* obj1 = static_cast<const BVHModel<RSS>* >(o1);
const T_SH* obj2 = static_cast<const T_SH*>(o2);
- initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request);
+ DistanceResult local_result;
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, local_result);
fcl::distance(&node);
- result.min_distance = node.min_distance;
- return node.min_distance;
+ result.update(local_result);
+ return local_result.min_distance;
}
};
@@ -184,11 +198,12 @@ struct BVHShapeDistancer<kIOS, T_SH, NarrowPhaseSolver>
const BVHModel<kIOS>* obj1 = static_cast<const BVHModel<kIOS>* >(o1);
const T_SH* obj2 = static_cast<const T_SH*>(o2);
- initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request);
+ DistanceResult local_result;
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, local_result);
fcl::distance(&node);
- result.min_distance = node.min_distance;
- return node.min_distance;
+ result.update(local_result);
+ return local_result.min_distance;
}
};
@@ -202,11 +217,12 @@ struct BVHShapeDistancer<OBBRSS, T_SH, NarrowPhaseSolver>
const BVHModel<OBBRSS>* obj1 = static_cast<const BVHModel<OBBRSS>* >(o1);
const T_SH* obj2 = static_cast<const T_SH*>(o2);
- initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request);
+ DistanceResult local_result;
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, local_result);
fcl::distance(&node);
- result.min_distance = node.min_distance;
- return node.min_distance;
+ result.update(local_result);
+ return local_result.min_distance;
}
};
@@ -223,11 +239,12 @@ FCL_REAL BVHDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, co
BVHModel<T_BVH>* obj2_tmp = new BVHModel<T_BVH>(*obj2);
SimpleTransform tf2_tmp = tf2;
- initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request);
+ DistanceResult local_result;
+ initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request, local_result);
distance(&node);
- result.min_distance = node.min_distance;
- return node.min_distance;
+ result.update(local_result);
+ return local_result.min_distance;
}
template<>
@@ -238,11 +255,12 @@ FCL_REAL BVHDistance<RSS>(const CollisionGeometry* o1, const SimpleTransform& tf
const BVHModel<RSS>* obj1 = static_cast<const BVHModel<RSS>* >(o1);
const BVHModel<RSS>* obj2 = static_cast<const BVHModel<RSS>* >(o2);
- initialize(node, *obj1, tf1, *obj2, tf2, request);
+ DistanceResult local_result;
+ initialize(node, *obj1, tf1, *obj2, tf2, request, local_result);
distance(&node);
- result.min_distance = node.min_distance;
- return node.min_distance;
+ result.update(local_result);
+ return local_result.min_distance;
}
template<>
@@ -253,11 +271,12 @@ FCL_REAL BVHDistance<kIOS>(const CollisionGeometry* o1, const SimpleTransform& t
const BVHModel<kIOS>* obj1 = static_cast<const BVHModel<kIOS>* >(o1);
const BVHModel<kIOS>* obj2 = static_cast<const BVHModel<kIOS>* >(o2);
- initialize(node, *obj1, tf1, *obj2, tf2, request);
+ DistanceResult local_result;
+ initialize(node, *obj1, tf1, *obj2, tf2, request, local_result);
distance(&node);
- result.min_distance = node.min_distance;
- return node.min_distance;
+ result.update(local_result);
+ return local_result.min_distance;
}
@@ -269,11 +288,12 @@ FCL_REAL BVHDistance<OBBRSS>(const CollisionGeometry* o1, const SimpleTransform&
const BVHModel<OBBRSS>* obj1 = static_cast<const BVHModel<OBBRSS>* >(o1);
const BVHModel<OBBRSS>* obj2 = static_cast<const BVHModel<OBBRSS>* >(o2);
- initialize(node, *obj1, tf1, *obj2, tf2, request);
+ DistanceResult local_result;
+ initialize(node, *obj1, tf1, *obj2, tf2, request, local_result);
distance(&node);
- result.min_distance = node.min_distance;
- return node.min_distance;
+ result.update(local_result);
+ return local_result.min_distance;
}
diff --git a/trunk/fcl/src/simple_setup.cpp b/trunk/fcl/src/simple_setup.cpp
index a9d9c21188bc7de8af2b64f33c8fe9c93785b253..53d472a685d0c6eb32ea4001363e6cd645265056 100644
--- a/trunk/fcl/src/simple_setup.cpp
+++ b/trunk/fcl/src/simple_setup.cpp
@@ -255,12 +255,14 @@ template<typename BV, typename OrientedNode>
static inline bool setupMeshDistanceOrientedNode(OrientedNode& node,
const BVHModel<BV>& model1, const SimpleTransform& tf1,
const BVHModel<BV>& model2, const SimpleTransform& tf2,
- const DistanceRequest& request)
+ const DistanceRequest& request,
+ DistanceResult& result)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
return false;
node.request = request;
+ node.result = &result;
node.model1 = &model1;
node.tf1 = tf1;
@@ -284,26 +286,29 @@ static inline bool setupMeshDistanceOrientedNode(OrientedNode& 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)
{
- return details::setupMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, request);
+ return details::setupMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, request, 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)
{
- return details::setupMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, request);
+ return details::setupMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, request, 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)
{
- return details::setupMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, request);
+ return details::setupMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, request, result);
}
diff --git a/trunk/fcl/src/traversal_node_bvhs.cpp b/trunk/fcl/src/traversal_node_bvhs.cpp
index e9a667fabedcf7d5b62d1b0a29986c7f039ebb16..5c487612230b4b56f8c1565080e445a560fb6b88 100644
--- a/trunk/fcl/src/traversal_node_bvhs.cpp
+++ b/trunk/fcl/src/traversal_node_bvhs.cpp
@@ -85,7 +85,7 @@ static inline void meshCollisionOrientedNodeLeafTesting(int b1, int b2,
if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, R, T))
{
is_intersect = true;
- result.contacts.push_back(Contact(model1, model2, primitive_id1, primitive_id2));
+ result.addContact(Contact(model1, model2, primitive_id1, primitive_id2));
}
}
else // need compute the contact information
@@ -100,22 +100,22 @@ static inline void meshCollisionOrientedNodeLeafTesting(int b1, int b2,
is_intersect = true;
if(!request.exhaustive)
{
- if(request.num_max_contacts < result.contacts.size() + n_contacts)
- n_contacts = (request.num_max_contacts > result.contacts.size()) ? (request.num_max_contacts - result.contacts.size()) : 0;
+ if(request.num_max_contacts < result.numContacts() + n_contacts)
+ n_contacts = (request.num_max_contacts > result.numContacts()) ? (request.num_max_contacts - result.numContacts()) : 0;
}
for(unsigned int i = 0; i < n_contacts; ++i)
{
- result.contacts.push_back(Contact(model1, model2, primitive_id1, primitive_id2, tf1.transform(contacts[i]), tf1.getQuatRotation().transform(normal), penetration));
+ result.addContact(Contact(model1, model2, primitive_id1, primitive_id2, tf1.transform(contacts[i]), tf1.getQuatRotation().transform(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(tf1.transform(p1), tf1.transform(p2), tf1.transform(p3)).overlap(AABB(tf2.transform(q1), tf2.transform(q2), tf2.transform(q3)), 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));
}
}
@@ -130,11 +130,8 @@ static inline void meshDistanceOrientedNodeLeafTesting(int b1, int b2,
const Matrix3f& R, const Vec3f& T,
bool enable_statistics,
int& num_leaf_tests,
- Vec3f& p1,
- Vec3f& p2,
- int& last_tri_id1,
- int& last_tri_id2,
- FCL_REAL& min_distance)
+ const DistanceRequest& request,
+ DistanceResult& result)
{
if(enable_statistics) num_leaf_tests++;
@@ -162,16 +159,10 @@ static inline void meshDistanceOrientedNodeLeafTesting(int b1, int b2,
R, T,
P1, P2);
- if(d < min_distance)
- {
- min_distance = d;
-
- p1 = P1;
- p2 = P2;
-
- last_tri_id1 = primitive_id1;
- last_tri_id2 = primitive_id2;
- }
+ if(request.enable_nearest_points)
+ result.update(d, model1, model2, primitive_id1, primitive_id2, P1, P2);
+ else
+ result.update(d, model1, model2, primitive_id1, primitive_id2);
}
}
@@ -472,63 +463,70 @@ void PointCloudMeshCollisionTraversalNodeRSS::leafTesting(int b1, int b2) const
#endif
-
-
-
-static inline void distance_preprocess(Vec3f* vertices1, Vec3f* vertices2,
- Triangle* tri_indices1, Triangle* tri_indices2,
- int last_tri_id1, int last_tri_id2,
- const Matrix3f& R, const Vec3f& T,
- FCL_REAL& min_distance,
- Vec3f& p1,
- Vec3f& p2)
+namespace details
{
- const Triangle& last_tri1 = tri_indices1[last_tri_id1];
- const Triangle& last_tri2 = tri_indices2[last_tri_id2];
- Vec3f last_tri1_points[3];
- Vec3f last_tri2_points[3];
-
- last_tri1_points[0] = vertices1[last_tri1[0]];
- last_tri1_points[1] = vertices1[last_tri1[1]];
- last_tri1_points[2] = vertices1[last_tri1[2]];
-
- last_tri2_points[0] = vertices2[last_tri2[0]];
- last_tri2_points[1] = vertices2[last_tri2[1]];
- last_tri2_points[2] = vertices2[last_tri2[2]];
-
- Vec3f last_tri_P, last_tri_Q;
-
- FCL_REAL init_dist = TriangleDistance::triDistance(last_tri1_points[0], last_tri1_points[1], last_tri1_points[2],
- last_tri2_points[0], last_tri2_points[1], last_tri2_points[2],
- R, T, last_tri_P, last_tri_Q);
- p1 = last_tri_P;
- p2 = R.transposeTimes(last_tri_Q - T);
-
- // if(min_distance > init_dist) min_distance = init_dist;
+template<typename BV>
+static inline void distancePreprocessOrientedNode(const BVHModel<BV>* model1, const BVHModel<BV>* model2,
+ const Vec3f* vertices1, Vec3f* vertices2,
+ Triangle* tri_indices1, Triangle* tri_indices2,
+ int init_tri_id1, int init_tri_id2,
+ const Matrix3f& R, const Vec3f& T,
+ const DistanceRequest& request,
+ DistanceResult& result)
+{
+ const Triangle& init_tri1 = tri_indices1[init_tri_id1];
+ const Triangle& init_tri2 = tri_indices2[init_tri_id2];
+
+ Vec3f init_tri1_points[3];
+ Vec3f init_tri2_points[3];
+
+ init_tri1_points[0] = vertices1[init_tri1[0]];
+ init_tri1_points[1] = vertices1[init_tri1[1]];
+ init_tri1_points[2] = vertices1[init_tri1[2]];
+
+ init_tri2_points[0] = vertices2[init_tri2[0]];
+ init_tri2_points[1] = vertices2[init_tri2[1]];
+ init_tri2_points[2] = vertices2[init_tri2[2]];
+
+ Vec3f p1, p2;
+ FCL_REAL distance = TriangleDistance::triDistance(init_tri1_points[0], init_tri1_points[1], init_tri1_points[2],
+ init_tri2_points[0], init_tri2_points[1], init_tri2_points[2],
+ R, T, p1, p2);
+
+ if(request.enable_nearest_points)
+ result.update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2);
+ else
+ result.update(distance, model1, model2, init_tri_id1, init_tri_id2);
}
-static inline void distance_postprocess(const Matrix3f& R, const Vec3f& T, Vec3f& p2)
+template<typename BV>
+static inline void distancePostprocessOrientedNode(const BVHModel<BV>* model1, const BVHModel<BV>* model2,
+ const SimpleTransform& tf1, const DistanceRequest& request, DistanceResult& result)
{
- Vec3f u = p2 - T;
- p2 = R.transposeTimes(u);
+ /// the points obtained by triDistance are not in world space: both are in object1's local coordinate system, so we need to convert them into the world space.
+ if(request.enable_nearest_points && (result.o1 == model1) && (result.o2 == model2))
+ {
+ result.nearest_points[0] = tf1.transform(result.nearest_points[0]);
+ result.nearest_points[1] = tf1.transform(result.nearest_points[1]);
+ }
}
+}
MeshDistanceTraversalNodeRSS::MeshDistanceTraversalNodeRSS() : MeshDistanceTraversalNode<RSS>()
{
R.setIdentity();
- // default T is 0
}
void MeshDistanceTraversalNodeRSS::preprocess()
{
- distance_preprocess(vertices1, vertices2, tri_indices1, tri_indices2, last_tri_id1, last_tri_id2, R, T, min_distance, p1, p2);
+ details::distancePreprocessOrientedNode(model1, model2, vertices1, vertices2, tri_indices1, tri_indices2, 0, 0, R, T, request, *result);
}
void MeshDistanceTraversalNodeRSS::postprocess()
{
- distance_postprocess(R, T, p2);
+ details::distancePostprocessOrientedNode(model1, model2, tf1, request, *result);
}
FCL_REAL MeshDistanceTraversalNodeRSS::BVTesting(int b1, int b2) const
@@ -541,23 +539,22 @@ void MeshDistanceTraversalNodeRSS::leafTesting(int b1, int b2) const
{
details::meshDistanceOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
R, T, enable_statistics, num_leaf_tests,
- p1, p2, last_tri_id1, last_tri_id2, min_distance);
+ request, *result);
}
MeshDistanceTraversalNodekIOS::MeshDistanceTraversalNodekIOS() : MeshDistanceTraversalNode<kIOS>()
{
R.setIdentity();
- // default T is 0
}
void MeshDistanceTraversalNodekIOS::preprocess()
{
- distance_preprocess(vertices1, vertices2, tri_indices1, tri_indices2, last_tri_id1, last_tri_id2, R, T, min_distance, p1, p2);
+ details::distancePreprocessOrientedNode(model1, model2, vertices1, vertices2, tri_indices1, tri_indices2, 0, 0, R, T, request, *result);
}
void MeshDistanceTraversalNodekIOS::postprocess()
{
- distance_postprocess(R, T, p2);
+ details::distancePostprocessOrientedNode(model1, model2, tf1, request, *result);
}
FCL_REAL MeshDistanceTraversalNodekIOS::BVTesting(int b1, int b2) const
@@ -570,23 +567,22 @@ void MeshDistanceTraversalNodekIOS::leafTesting(int b1, int b2) const
{
details::meshDistanceOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
R, T, enable_statistics, num_leaf_tests,
- p1, p2, last_tri_id1, last_tri_id2, min_distance);
+ request, *result);
}
MeshDistanceTraversalNodeOBBRSS::MeshDistanceTraversalNodeOBBRSS() : MeshDistanceTraversalNode<OBBRSS>()
{
R.setIdentity();
- // default T is 0
}
void MeshDistanceTraversalNodeOBBRSS::preprocess()
{
- distance_preprocess(vertices1, vertices2, tri_indices1, tri_indices2, last_tri_id1, last_tri_id2, R, T, min_distance, p1, p2);
+ details::distancePreprocessOrientedNode(model1, model2, vertices1, vertices2, tri_indices1, tri_indices2, 0, 0, R, T, request, *result);
}
void MeshDistanceTraversalNodeOBBRSS::postprocess()
{
- distance_postprocess(R, T, p2);
+ details::distancePostprocessOrientedNode(model1, model2, tf1, request, *result);
}
FCL_REAL MeshDistanceTraversalNodeOBBRSS::BVTesting(int b1, int b2) const
@@ -599,7 +595,7 @@ void MeshDistanceTraversalNodeOBBRSS::leafTesting(int b1, int b2) const
{
details::meshDistanceOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
R, T, enable_statistics, num_leaf_tests,
- p1, p2, last_tri_id1, last_tri_id2, min_distance);
+ request, *result);
}