diff --git a/trunk/fcl/include/fcl/collision.h b/trunk/fcl/include/fcl/collision.h
index 985d7fe01e33c28bfdea762e6d7c3c408bb56f97..d9f59f8d9b44209d4ec8ab66c03834a08d29d302 100644
--- a/trunk/fcl/include/fcl/collision.h
+++ b/trunk/fcl/include/fcl/collision.h
@@ -55,25 +55,24 @@ namespace fcl
template<typename NarrowPhaseSolver>
int collide(const CollisionObject* o1, const CollisionObject* o2,
const NarrowPhaseSolver* nsolver,
- int num_max_contacts, bool exhaustive, bool enable_contact,
- std::vector<Contact>& contacts);
+ const CollisionRequest& request,
+ CollisionResult& result);
template<typename NarrowPhaseSolver>
int collide(const CollisionGeometry* o1, const SimpleTransform& tf1,
const CollisionGeometry* o2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
- int num_max_contacts, bool exhaustive, bool enable_contact,
- std::vector<Contact>& contacts);
-
+ const CollisionRequest& request,
+ CollisionResult& result);
int collide(const CollisionObject* o1, const CollisionObject* o2,
- int num_max_contacts, bool exhaustive, bool enable_contact,
- std::vector<Contact>& contacts);
+ const CollisionRequest& request,
+ CollisionResult& result);
int collide(const CollisionGeometry* o1, const SimpleTransform& tf1,
const CollisionGeometry* o2, const SimpleTransform& tf2,
- int num_max_contacts, bool exhaustive, bool enable_contact,
- std::vector<Contact>& contacts);
+ const CollisionRequest& request,
+ CollisionResult& result);
}
#endif
diff --git a/trunk/fcl/include/fcl/collision_data.h b/trunk/fcl/include/fcl/collision_data.h
index c22eec13e0f159798ea551699f1dde440f9293bf..05db4dfecd0ee6e803ff0049edb802308b15a8de 100644
--- a/trunk/fcl/include/fcl/collision_data.h
+++ b/trunk/fcl/include/fcl/collision_data.h
@@ -49,36 +49,104 @@ struct Contact
}
};
+struct CostSource
+{
+ Vec3f aabb_min;
+ Vec3f aabb_max;
+ FCL_REAL cost; // density
+};
+
+struct CollisionRequest
+{
+ bool exhaustive;
+ int num_max_contacts;
+ bool enable_contact;
+ int num_max_cost_sources;
+ bool enable_cost;
+
+ CollisionRequest(bool exhaustive_ = false,
+ unsigned int num_max_contacts_ = 1,
+ bool enable_contact_ = false,
+ unsigned int num_max_cost_sources_ = 1,
+ bool enable_cost_ = false) : exhaustive(exhaustive_),
+ num_max_contacts(num_max_contacts_),
+ enable_contact(enable_contact_),
+ num_max_cost_sources(num_max_cost_sources_),
+ enable_cost(enable_cost_)
+ {
+ }
+
+};
+
+
+struct CollisionResult
+{
+ std::vector<Contact> contacts;
+ std::vector<CostSource> cost_sources;
+
+ bool is_collision;
+
+ CollisionResult() : is_collision(false)
+ {
+ }
+
+ void clear()
+ {
+ contacts.clear();
+ cost_sources.clear();
+ is_collision = false;
+ }
+};
+
+
struct CollisionData
{
CollisionData()
{
done = false;
- is_collision = false;
- num_max_contacts = 1;
- enable_contact = false;
- exhaustive = false;
}
+ CollisionRequest request;
+ CollisionResult result;
bool done;
- bool is_collision;
- bool exhaustive;
- unsigned int num_max_contacts;
- bool enable_contact;
+};
- std::vector<Contact> contacts;
+
+struct DistanceRequest
+{
+ bool enable_nearest_points;
+ DistanceRequest() : enable_nearest_points(false)
+ {
+ }
+};
+
+struct DistanceResult
+{
+ FCL_REAL min_distance;
+
+ Vec3f nearest_points[2];
+
+ DistanceResult() : min_distance(std::numeric_limits<FCL_REAL>::max())
+ {
+ }
+
+ void clear()
+ {
+ min_distance = std::numeric_limits<FCL_REAL>::max();
+ }
};
struct DistanceData
{
DistanceData()
{
- min_distance = std::numeric_limits<FCL_REAL>::max();
done = false;
}
- FCL_REAL min_distance;
bool done;
+
+ DistanceRequest request;
+ DistanceResult result;
};
diff --git a/trunk/fcl/include/fcl/collision_func_matrix.h b/trunk/fcl/include/fcl/collision_func_matrix.h
index ab18a84745ed795671a90711479ad22401c9942f..7cdb13823af7af2f4fd6e970c23228a92e85f205 100644
--- a/trunk/fcl/include/fcl/collision_func_matrix.h
+++ b/trunk/fcl/include/fcl/collision_func_matrix.h
@@ -48,7 +48,7 @@ namespace fcl
template<typename NarrowPhaseSolver>
struct CollisionFunctionMatrix
{
- typedef int (*CollisionFunc)(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts);
+ typedef int (*CollisionFunc)(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result);
CollisionFunc collision_matrix[NODE_COUNT][NODE_COUNT];
diff --git a/trunk/fcl/include/fcl/conservative_advancement.h b/trunk/fcl/include/fcl/conservative_advancement.h
index df2fc0fd37266ae6c14a2f2f7286477af547f1a6..5ee477c389664c9b7b7ff14dfae212fe5f89cc35 100644
--- a/trunk/fcl/include/fcl/conservative_advancement.h
+++ b/trunk/fcl/include/fcl/conservative_advancement.h
@@ -52,8 +52,8 @@ int conservativeAdvancement(const CollisionGeometry* o1,
MotionBase<BV>* motion1,
const CollisionGeometry* o2,
MotionBase<BV>* motion2,
- int num_max_contacts, bool exhaustive, bool enable_contact,
- std::vector<Contact>& contacts,
+ const CollisionRequest& request,
+ CollisionResult& result,
FCL_REAL& toc);
}
diff --git a/trunk/fcl/include/fcl/distance.h b/trunk/fcl/include/fcl/distance.h
index 3a05c814659d5c1aa76207918add92f94a7bc349..2497fcbbc8a064caf9229832d7708c3f8413b69f 100644
--- a/trunk/fcl/include/fcl/distance.h
+++ b/trunk/fcl/include/fcl/distance.h
@@ -38,24 +38,29 @@
#define FCL_DISTANCE_H
#include "fcl/collision_object.h"
+#include "fcl/collision_data.h"
namespace fcl
{
template<typename NarrowPhaseSolver>
-FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const NarrowPhaseSolver* nsolver);
+FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const NarrowPhaseSolver* nsolver,
+ const DistanceRequest& request, DistanceResult& result);
template<typename NarrowPhaseSolver>
FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
const CollisionGeometry* o2, const SimpleTransform& tf2,
- const NarrowPhaseSolver* nsolver);
+ const NarrowPhaseSolver* nsolver,
+ const DistanceRequest& request, DistanceResult& result);
-
-FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2);
+FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2,
+ const DistanceRequest& request, DistanceResult& result);
FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
- const CollisionGeometry* o2, const SimpleTransform& tf2);
+ const CollisionGeometry* o2, const SimpleTransform& tf2,
+ const DistanceRequest& request, DistanceResult& result);
+
}
#endif
diff --git a/trunk/fcl/include/fcl/distance_func_matrix.h b/trunk/fcl/include/fcl/distance_func_matrix.h
index 622f5a90585c3f7032211e07d550996dd6d32b25..3f8c1830c35b65a16c7a1c4a0beaa8221bfacce9 100644
--- a/trunk/fcl/include/fcl/distance_func_matrix.h
+++ b/trunk/fcl/include/fcl/distance_func_matrix.h
@@ -46,7 +46,8 @@ namespace fcl
template<typename NarrowPhaseSolver>
struct DistanceFunctionMatrix
{
- typedef FCL_REAL (*DistanceFunc)(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver);
+ typedef FCL_REAL (*DistanceFunc)(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver,
+ const DistanceRequest& request, DistanceResult& result);
DistanceFunc distance_matrix[NODE_COUNT][NODE_COUNT];
diff --git a/trunk/fcl/include/fcl/simple_setup.h b/trunk/fcl/include/fcl/simple_setup.h
index ba3ed19d047882df2cce48ec6c403156a7783975..acceed504d9f55bb5731430627721dd066bf1ea5 100644
--- a/trunk/fcl/include/fcl/simple_setup.h
+++ b/trunk/fcl/include/fcl/simple_setup.h
@@ -53,11 +53,9 @@ bool initialize(OcTreeCollisionTraversalNode<NarrowPhaseSolver>& node,
const OcTree& model1, const SimpleTransform& tf1,
const OcTree& model2, const SimpleTransform& tf2,
const OcTreeSolver<NarrowPhaseSolver>* otsolver,
- int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
+ const CollisionRequest& request)
{
- node.enable_contact = enable_contact;
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
+ node.request = request;
node.model1 = &model1;
node.model2 = &model2;
@@ -74,8 +72,11 @@ template<typename NarrowPhaseSolver>
bool initialize(OcTreeDistanceTraversalNode<NarrowPhaseSolver>& node,
const OcTree& model1, const SimpleTransform& tf1,
const OcTree& model2, const SimpleTransform& tf2,
- const OcTreeSolver<NarrowPhaseSolver>* otsolver)
+ const OcTreeSolver<NarrowPhaseSolver>* otsolver,
+ const DistanceRequest& request)
{
+ node.request = request;
+
node.model1 = &model1;
node.model2 = &model2;
@@ -92,11 +93,9 @@ bool initialize(ShapeOcTreeCollisionTraversalNode<S, NarrowPhaseSolver>& node,
const S& model1, const SimpleTransform& tf1,
const OcTree& model2, const SimpleTransform& tf2,
const OcTreeSolver<NarrowPhaseSolver>* otsolver,
- int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
+ const CollisionRequest& request)
{
- node.enable_contact = enable_contact;
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
+ node.request = request;
node.model1 = &model1;
node.model2 = &model2;
@@ -114,11 +113,9 @@ bool initialize(OcTreeShapeCollisionTraversalNode<S, NarrowPhaseSolver>& node,
const OcTree& model1, const SimpleTransform& tf1,
const S& model2, const SimpleTransform& tf2,
const OcTreeSolver<NarrowPhaseSolver>* otsolver,
- int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
+ const CollisionRequest& request)
{
- node.enable_contact = enable_contact;
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
+ node.request = request;
node.model1 = &model1;
node.model2 = &model2;
@@ -135,8 +132,11 @@ template<typename S, typename NarrowPhaseSolver>
bool initialize(ShapeOcTreeDistanceTraversalNode<S, NarrowPhaseSolver>& node,
const S& model1, const SimpleTransform& tf1,
const OcTree& model2, const SimpleTransform& tf2,
- const OcTreeSolver<NarrowPhaseSolver>* otsolver)
+ const OcTreeSolver<NarrowPhaseSolver>* otsolver,
+ const DistanceRequest& request)
{
+ node.request = request;
+
node.model1 = &model1;
node.model2 = &model2;
@@ -153,8 +153,11 @@ template<typename S, typename NarrowPhaseSolver>
bool initialize(OcTreeShapeDistanceTraversalNode<S, NarrowPhaseSolver>& node,
const OcTree& model1, const SimpleTransform& tf1,
const S& model2, const SimpleTransform& tf2,
- const OcTreeSolver<NarrowPhaseSolver>* otsolver)
+ const OcTreeSolver<NarrowPhaseSolver>* otsolver,
+ const DistanceRequest& request)
{
+ node.request = request;
+
node.model1 = &model1;
node.model2 = &model2;
@@ -171,11 +174,9 @@ bool initialize(MeshOcTreeCollisionTraversalNode<BV, NarrowPhaseSolver>& node,
const BVHModel<BV>& model1, const SimpleTransform& tf1,
const OcTree& model2, const SimpleTransform& tf2,
const OcTreeSolver<NarrowPhaseSolver>* otsolver,
- int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
+ const CollisionRequest& request)
{
- node.enable_contact = enable_contact;
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
+ node.request = request;
node.model1 = &model1;
node.model2 = &model2;
@@ -193,11 +194,9 @@ bool initialize(OcTreeMeshCollisionTraversalNode<BV, NarrowPhaseSolver>& node,
const OcTree& model1, const SimpleTransform& tf1,
const BVHModel<BV>& model2, const SimpleTransform& tf2,
const OcTreeSolver<NarrowPhaseSolver>* otsolver,
- int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
+ const CollisionRequest& request)
{
- node.enable_contact = enable_contact;
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
+ node.request = request;
node.model1 = &model1;
node.model2 = &model2;
@@ -214,8 +213,11 @@ template<typename BV, typename NarrowPhaseSolver>
bool initialize(MeshOcTreeDistanceTraversalNode<BV, NarrowPhaseSolver>& node,
const BVHModel<BV>& model1, const SimpleTransform& tf1,
const OcTree& model2, const SimpleTransform& tf2,
- const OcTreeSolver<NarrowPhaseSolver>* otsolver)
+ const OcTreeSolver<NarrowPhaseSolver>* otsolver,
+ const DistanceRequest& request)
{
+ node.request = request;
+
node.model1 = &model1;
node.model2 = &model2;
@@ -232,8 +234,11 @@ template<typename BV, typename NarrowPhaseSolver>
bool initialize(OcTreeMeshDistanceTraversalNode<BV, NarrowPhaseSolver>& node,
const OcTree& model1, const SimpleTransform& tf1,
const BVHModel<BV>& model2, const SimpleTransform& tf2,
- const OcTreeSolver<NarrowPhaseSolver>* otsolver)
+ const OcTreeSolver<NarrowPhaseSolver>* otsolver,
+ const DistanceRequest& request)
{
+ node.request = request;
+
node.model1 = &model1;
node.model2 = &model2;
@@ -253,14 +258,14 @@ bool initialize(ShapeCollisionTraversalNode<S1, S2, NarrowPhaseSolver>& node,
const S1& shape1, const SimpleTransform& tf1,
const S2& shape2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
- bool enable_contact = false)
+ const CollisionRequest& request)
{
- node.enable_contact = enable_contact;
node.model1 = &shape1;
node.tf1 = tf1;
node.model2 = &shape2;
node.tf2 = tf2;
node.nsolver = nsolver;
+ node.request = request;
return true;
}
@@ -270,7 +275,7 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S, NarrowPhaseSolver>& node,
BVHModel<BV>& model1, SimpleTransform& tf1,
const S& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
- int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false,
+ const CollisionRequest& request,
bool use_refit = false, bool refit_bottomup = false)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES)
@@ -303,9 +308,7 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S, NarrowPhaseSolver>& node,
node.vertices = model1.vertices;
node.tri_indices = model1.tri_indices;
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
+ node.request = request;
return true;
}
@@ -317,7 +320,7 @@ bool initialize(ShapeMeshCollisionTraversalNode<S, BV, NarrowPhaseSolver>& node,
const S& model1, const SimpleTransform& tf1,
BVHModel<BV>& model2, SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
- int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false,
+ const CollisionRequest& request,
bool use_refit = false, bool refit_bottomup = false)
{
if(model2.getModelType() != BVH_MODEL_TRIANGLES)
@@ -350,9 +353,7 @@ bool initialize(ShapeMeshCollisionTraversalNode<S, BV, NarrowPhaseSolver>& node,
node.vertices = model2.vertices;
node.tri_indices = model2.tri_indices;
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
+ node.request = request;
return true;
}
@@ -366,7 +367,7 @@ static inline bool setupMeshShapeCollisionOrientedNode(OrientedNode<S, NarrowPha
const BVHModel<BV>& model1, const SimpleTransform& tf1,
const S& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
- int num_max_contacts, bool exhaustive, bool enable_contact)
+ const CollisionRequest& request)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES)
return false;
@@ -381,9 +382,7 @@ static inline bool setupMeshShapeCollisionOrientedNode(OrientedNode<S, NarrowPha
node.vertices = model1.vertices;
node.tri_indices = model1.tri_indices;
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
+ node.request = request;
node.R = tf1.getRotation();
node.T = tf1.getTranslation();
@@ -401,9 +400,9 @@ bool initialize(MeshShapeCollisionTraversalNodeOBB<S, NarrowPhaseSolver>& node,
const BVHModel<OBB>& model1, const SimpleTransform& tf1,
const S& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
- int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
+ const CollisionRequest& request)
{
- return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, num_max_contacts, exhaustive, enable_contact);
+ return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
}
/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for RSS type */
@@ -412,9 +411,9 @@ bool initialize(MeshShapeCollisionTraversalNodeRSS<S, NarrowPhaseSolver>& node,
const BVHModel<RSS>& model1, const SimpleTransform& tf1,
const S& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
- int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
+ const CollisionRequest& request)
{
- return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, num_max_contacts, exhaustive, enable_contact);
+ return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
}
/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for kIOS type */
@@ -423,9 +422,9 @@ bool initialize(MeshShapeCollisionTraversalNodekIOS<S, NarrowPhaseSolver>& node,
const BVHModel<kIOS>& model1, const SimpleTransform& tf1,
const S& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
- int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
+ const CollisionRequest& request)
{
- return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, num_max_contacts, exhaustive, enable_contact);
+ return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
}
/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBBRSS type */
@@ -434,9 +433,9 @@ bool initialize(MeshShapeCollisionTraversalNodeOBBRSS<S, NarrowPhaseSolver>& nod
const BVHModel<OBBRSS>& model1, const SimpleTransform& tf1,
const S& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
- int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
+ const CollisionRequest& request)
{
- return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, num_max_contacts, exhaustive, enable_contact);
+ return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
}
@@ -448,7 +447,7 @@ static inline bool setupShapeMeshCollisionOrientedNode(OrientedNode<S, NarrowPha
const S& model1, const SimpleTransform& tf1,
const BVHModel<BV>& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
- int num_max_contacts, bool exhaustive, bool enable_contact)
+ const CollisionRequest& request)
{
if(model2.getModelType() != BVH_MODEL_TRIANGLES)
return false;
@@ -463,9 +462,7 @@ static inline bool setupShapeMeshCollisionOrientedNode(OrientedNode<S, NarrowPha
node.vertices = model2.vertices;
node.tri_indices = model2.tri_indices;
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
+ node.request = request;
node.R = tf2.getRotation();
node.T = tf2.getTranslation();
@@ -481,9 +478,9 @@ bool initialize(ShapeMeshCollisionTraversalNodeOBB<S, NarrowPhaseSolver>& node,
const S& model1, const SimpleTransform& tf1,
const BVHModel<OBB>& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
- int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
+ const CollisionRequest& request)
{
- return setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, num_max_contacts, exhaustive, enable_contact);
+ return setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
}
/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for RSS type */
@@ -492,9 +489,9 @@ bool initialize(ShapeMeshCollisionTraversalNodeRSS<S, NarrowPhaseSolver>& node,
const S& model1, const SimpleTransform& tf1,
const BVHModel<RSS>& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
- int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
+ const CollisionRequest& request)
{
- return setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, num_max_contacts, exhaustive, enable_contact);
+ return setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
}
/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for kIOS type */
@@ -503,9 +500,9 @@ bool initialize(ShapeMeshCollisionTraversalNodekIOS<S, NarrowPhaseSolver>& node,
const S& model1, const SimpleTransform& tf1,
const BVHModel<kIOS>& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
- int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
+ const CollisionRequest& request)
{
- return setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, num_max_contacts, exhaustive, enable_contact);
+ return setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
}
/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBBRSS type */
@@ -514,9 +511,9 @@ bool initialize(ShapeMeshCollisionTraversalNodeOBBRSS<S, NarrowPhaseSolver>& nod
const S& model1, const SimpleTransform& tf1,
const BVHModel<OBBRSS>& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
- int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
+ const CollisionRequest& request)
{
- return setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, num_max_contacts, exhaustive, enable_contact);
+ return setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
}
@@ -527,7 +524,7 @@ template<typename BV>
bool initialize(MeshCollisionTraversalNode<BV>& node,
BVHModel<BV>& model1, SimpleTransform& tf1,
BVHModel<BV>& model2, SimpleTransform& tf2,
- int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false,
+ const CollisionRequest& request,
bool use_refit = false, bool refit_bottomup = false)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
@@ -578,9 +575,7 @@ bool initialize(MeshCollisionTraversalNode<BV>& node,
node.tri_indices1 = model1.tri_indices;
node.tri_indices2 = model2.tri_indices;
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
+ node.request = request;
return true;
}
@@ -590,22 +585,22 @@ bool initialize(MeshCollisionTraversalNode<BV>& node,
bool initialize(MeshCollisionTraversalNodeOBB& node,
const BVHModel<OBB>& model1, const SimpleTransform& tf1,
const BVHModel<OBB>& model2, const SimpleTransform& tf2,
- int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false);
+ const CollisionRequest& request);
bool initialize(MeshCollisionTraversalNodeRSS& node,
const BVHModel<RSS>& model1, const SimpleTransform& tf1,
const BVHModel<RSS>& model2, const SimpleTransform& tf2,
- int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false);
+ const CollisionRequest& request);
bool initialize(MeshCollisionTraversalNodeOBBRSS& node,
const BVHModel<OBBRSS>& model1, const SimpleTransform& tf1,
const BVHModel<OBBRSS>& model2, const SimpleTransform& tf2,
- int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false);
+ const CollisionRequest& request);
bool initialize(MeshCollisionTraversalNodekIOS& node,
const BVHModel<kIOS>& model1, const SimpleTransform& tf1,
const BVHModel<kIOS>& model2, const SimpleTransform& tf2,
- int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false);
+ const CollisionRequest& request);
#if USE_SVMLIGHT
@@ -614,11 +609,9 @@ template<typename BV, bool use_refit, bool refit_bottomup>
bool initialize(PointCloudCollisionTraversalNode<BV>& node,
BVHModel<BV>& model1, SimpleTransform& tf1,
BVHModel<BV>& model2, SimpleTransform& tf2,
+ const CollisionRequest& request,
FCL_REAL collision_prob_threshold = 0.5,
int leaf_size_threshold = 1,
- int num_max_contacts = 1,
- bool exhaustive = false,
- bool enable_contact = false,
FCL_REAL expand_r = 1)
{
if(!(model1.getModelType() == BVH_MODEL_TRIANGLES || model1.getModelType() == BVH_MODEL_POINTCLOUD)
@@ -676,9 +669,7 @@ bool initialize(PointCloudCollisionTraversalNode<BV>& node,
BVHExpand(model1, node.uc1.get(), expand_r);
BVHExpand(model2, node.uc2.get(), expand_r);
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
+ node.request = request;
node.collision_prob_threshold = collision_prob_threshold;
node.leaf_size_threshold = leaf_size_threshold;
@@ -689,22 +680,18 @@ bool initialize(PointCloudCollisionTraversalNode<BV>& node,
bool initialize(PointCloudCollisionTraversalNodeOBB& node,
BVHModel<OBB>& model1, const SimpleTransform& tf1,
BVHModel<OBB>& model2, const SimpleTransform& tf2,
+ const CollisionRequest& request,
FCL_REAL collision_prob_threshold = 0.5,
int leaf_size_threshold = 1,
- int num_max_contacts = 1,
- bool exhaustive = false,
- bool enable_contact = false,
FCL_REAL expand_r = 1);
/** \brief Initialize traversal node for collision between two point clouds, given current transforms, specialized for RSS type */
bool initialize(PointCloudCollisionTraversalNodeRSS& node,
BVHModel<RSS>& model1, const SimpleTransform& tf1,
BVHModel<RSS>& model2, const SimpleTransform& tf2,
+ const CollisionRequest& request,
FCL_REAL collision_prob_threshold = 0.5,
int leaf_size_threshold = 1,
- int num_max_contacts = 1,
- bool exhaustive = false,
- bool enable_contact = false,
FCL_REAL expand_r = 1);
/** \brief Initialize traversal node for collision between one point cloud and one mesh, given current transforms */
@@ -712,11 +699,9 @@ template<typename BV, bool use_refit, bool refit_bottomup>
bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node,
BVHModel<BV>& model1, SimpleTransform& tf1,
BVHModel<BV>& model2, SimpleTransform& tf2,
+ const CollisionRequest& request,
FCL_REAL collision_prob_threshold = 0.5,
int leaf_size_threshold = 1,
- int num_max_contacts = 1,
- bool exhaustive = false,
- bool enable_contact = false,
FCL_REAL expand_r = 1)
{
if(!(model1.getModelType() == BVH_MODEL_TRIANGLES || model1.getModelType() == BVH_MODEL_POINTCLOUD) || model2.getModelType() != BVH_MODEL_TRIANGLES)
@@ -771,9 +756,7 @@ bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node,
BVHExpand(model1, node.uc1.get(), expand_r);
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
+ node.request = request;
node.collision_prob_threshold = collision_prob_threshold;
node.leaf_size_threshold = leaf_size_threshold;
@@ -785,22 +768,18 @@ bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node,
bool initialize(PointCloudMeshCollisionTraversalNodeOBB& node,
BVHModel<OBB>& model1, const SimpleTransform& tf1,
const BVHModel<OBB>& model2, const SimpleTransform& tf2,
+ const CollisionRequest& request,
FCL_REAL collision_prob_threshold = 0.5,
int leaf_size_threshold = 1,
- int num_max_contacts = 1,
- bool exhaustive = false,
- bool enable_contact = false,
FCL_REAL expand_r = 1);
/** \brief Initialize traversal node for collision between one point cloud and one mesh, given current transforms, specialized for RSS type */
bool initialize(PointCloudMeshCollisionTraversalNodeRSS& node,
BVHModel<RSS>& model1, const SimpleTransform& tf1,
const BVHModel<RSS>& model2, const SimpleTransform& tf2,
+ const CollisionRequest& request,
FCL_REAL collision_prob_threshold = 0.5,
int leaf_size_threshold = 1,
- int num_max_contacts = 1,
- bool exhaustive = false,
- bool enable_contact = false,
FCL_REAL expand_r = 1);
#endif
@@ -811,8 +790,11 @@ template<typename S1, typename S2, typename NarrowPhaseSolver>
bool initialize(ShapeDistanceTraversalNode<S1, S2, NarrowPhaseSolver>& node,
const S1& shape1, const SimpleTransform& tf1,
const S2& shape2, const SimpleTransform& tf2,
- const NarrowPhaseSolver* nsolver)
+ const NarrowPhaseSolver* nsolver,
+ const DistanceRequest& request)
{
+ node.request = request;
+
node.model1 = &shape1;
node.tf1 = tf1;
node.model2 = &shape2;
@@ -827,6 +809,7 @@ template<typename BV>
bool initialize(MeshDistanceTraversalNode<BV>& node,
BVHModel<BV>& model1, SimpleTransform& tf1,
BVHModel<BV>& model2, SimpleTransform& tf2,
+ const DistanceRequest& request,
bool use_refit = false, bool refit_bottomup = false)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
@@ -866,6 +849,8 @@ bool initialize(MeshDistanceTraversalNode<BV>& node,
tf2.setIdentity();
}
+ node.request = request;
+
node.model1 = &model1;
node.tf1 = tf1;
node.model2 = &model2;
@@ -884,16 +869,19 @@ bool initialize(MeshDistanceTraversalNode<BV>& node,
/** \brief Initialize traversal node for distance computation between two meshes, given the current transforms */
bool initialize(MeshDistanceTraversalNodeRSS& node,
const BVHModel<RSS>& model1, const SimpleTransform& tf1,
- const BVHModel<RSS>& model2, const SimpleTransform& tf2);
+ const BVHModel<RSS>& model2, const SimpleTransform& tf2,
+ const DistanceRequest& request);
bool initialize(MeshDistanceTraversalNodekIOS& node,
const BVHModel<kIOS>& model1, const SimpleTransform& tf1,
- const BVHModel<kIOS>& model2, const SimpleTransform& tf2);
+ const BVHModel<kIOS>& model2, const SimpleTransform& tf2,
+ const DistanceRequest& request);
bool initialize(MeshDistanceTraversalNodeOBBRSS& node,
const BVHModel<OBBRSS>& model1, const SimpleTransform& tf1,
- const BVHModel<OBBRSS>& model2, const SimpleTransform& tf2);
+ const BVHModel<OBBRSS>& model2, const SimpleTransform& tf2,
+ const DistanceRequest& request);
template<typename BV, typename S, typename NarrowPhaseSolver>
@@ -901,6 +889,7 @@ bool initialize(MeshShapeDistanceTraversalNode<BV, S, NarrowPhaseSolver>& node,
BVHModel<BV>& model1, SimpleTransform& tf1,
const S& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
+ const DistanceRequest& request,
bool use_refit = false, bool refit_bottomup = false)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES)
@@ -923,6 +912,8 @@ bool initialize(MeshShapeDistanceTraversalNode<BV, S, NarrowPhaseSolver>& node,
tf1.setIdentity();
}
+ node.request = request;
+
node.model1 = &model1;
node.tf1 = tf1;
node.model2 = &model2;
@@ -943,6 +934,7 @@ bool initialize(ShapeMeshDistanceTraversalNode<S, BV, NarrowPhaseSolver>& node,
const S& model1, const SimpleTransform& tf1,
BVHModel<BV>& model2, SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
+ const DistanceRequest& request,
bool use_refit = false, bool refit_bottomup = false)
{
if(model2.getModelType() != BVH_MODEL_TRIANGLES)
@@ -965,6 +957,8 @@ bool initialize(ShapeMeshDistanceTraversalNode<S, BV, NarrowPhaseSolver>& node,
tf2.setIdentity();
}
+ node.request = request;
+
node.model1 = &model1;
node.tf1 = tf1;
node.model2 = &model2;
@@ -987,11 +981,14 @@ template<typename BV, typename S, typename NarrowPhaseSolver, template<typename,
static inline bool setupMeshShapeDistanceOrientedNode(OrientedNode<S, NarrowPhaseSolver>& node,
const BVHModel<BV>& model1, const SimpleTransform& tf1,
const S& model2, const SimpleTransform& tf2,
- const NarrowPhaseSolver* nsolver)
+ const NarrowPhaseSolver* nsolver,
+ const DistanceRequest& request)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES)
return false;
+ node.request = request;
+
node.model1 = &model1;
node.tf1 = tf1;
node.model2 = &model2;
@@ -1014,27 +1011,30 @@ template<typename S, typename NarrowPhaseSolver>
bool initialize(MeshShapeDistanceTraversalNodeRSS<S, NarrowPhaseSolver>& node,
const BVHModel<RSS>& model1, const SimpleTransform& tf1,
const S& model2, const SimpleTransform& tf2,
- const NarrowPhaseSolver* nsolver)
+ const NarrowPhaseSolver* nsolver,
+ const DistanceRequest& request)
{
- return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver);
+ return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
}
template<typename S, typename NarrowPhaseSolver>
bool initialize(MeshShapeDistanceTraversalNodekIOS<S, NarrowPhaseSolver>& node,
const BVHModel<kIOS>& model1, const SimpleTransform& tf1,
const S& model2, const SimpleTransform& tf2,
- const NarrowPhaseSolver* nsolver)
+ const NarrowPhaseSolver* nsolver,
+ const DistanceRequest& request)
{
- return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver);
+ return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
}
template<typename S, typename NarrowPhaseSolver>
bool initialize(MeshShapeDistanceTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node,
const BVHModel<OBBRSS>& model1, const SimpleTransform& tf1,
const S& model2, const SimpleTransform& tf2,
- const NarrowPhaseSolver* nsolver)
+ const NarrowPhaseSolver* nsolver,
+ const DistanceRequest& request)
{
- return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver);
+ return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
}
@@ -1044,11 +1044,14 @@ template<typename S, typename BV, typename NarrowPhaseSolver, template<typename,
static inline bool setupShapeMeshDistanceOrientedNode(OrientedNode<S, NarrowPhaseSolver>& node,
const S& model1, const SimpleTransform& tf1,
const BVHModel<BV>& model2, const SimpleTransform& tf2,
- const NarrowPhaseSolver* nsolver)
+ const NarrowPhaseSolver* nsolver,
+ const DistanceRequest& request)
{
if(model2.getModelType() != BVH_MODEL_TRIANGLES)
return false;
+ node.request = request;
+
node.model1 = &model1;
node.tf1 = tf1;
node.model2 = &model2;
@@ -1072,27 +1075,30 @@ template<typename S, typename NarrowPhaseSolver>
bool initialize(ShapeMeshDistanceTraversalNodeRSS<S, NarrowPhaseSolver>& node,
const S& model1, const SimpleTransform& tf1,
const BVHModel<RSS>& model2, const SimpleTransform& tf2,
- const NarrowPhaseSolver* nsolver)
+ const NarrowPhaseSolver* nsolver,
+ const DistanceRequest& request)
{
- return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver);
+ return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
}
template<typename S, typename NarrowPhaseSolver>
bool initialize(ShapeMeshDistanceTraversalNodekIOS<S, NarrowPhaseSolver>& node,
const S& model1, const SimpleTransform& tf1,
const BVHModel<kIOS>& model2, const SimpleTransform& tf2,
- const NarrowPhaseSolver* nsolver)
+ const NarrowPhaseSolver* nsolver,
+ const DistanceRequest& request)
{
- return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver);
+ return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
}
template<typename S, typename NarrowPhaseSolver>
bool initialize(ShapeMeshDistanceTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node,
const S& model1, const SimpleTransform& tf1,
const BVHModel<OBBRSS>& model2, const SimpleTransform& tf2,
- const NarrowPhaseSolver* nsolver)
+ const NarrowPhaseSolver* nsolver,
+ const DistanceRequest& request)
{
- return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver);
+ return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
}
@@ -1103,7 +1109,7 @@ template<typename BV>
bool initialize(MeshContinuousCollisionTraversalNode<BV>& node,
const BVHModel<BV>& model1, const SimpleTransform& tf1,
const BVHModel<BV>& model2, const SimpleTransform& tf2,
- int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
+ const CollisionRequest& request)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
return false;
@@ -1122,9 +1128,7 @@ bool initialize(MeshContinuousCollisionTraversalNode<BV>& node,
node.prev_vertices1 = model1.prev_vertices;
node.prev_vertices2 = model2.prev_vertices;
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
+ node.request = request;
return true;
}
@@ -1134,7 +1138,7 @@ template<typename BV>
bool initialize(MeshPointCloudContinuousCollisionTraversalNode<BV>& node,
const BVHModel<BV>& model1, const SimpleTransform& tf1,
const BVHModel<BV>& model2, const SimpleTransform& tf2,
- int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
+ const CollisionRequest& request)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES || !(model2.getModelType() == BVH_MODEL_TRIANGLES || model2.getModelType() == BVH_MODEL_POINTCLOUD))
return false;
@@ -1151,9 +1155,7 @@ bool initialize(MeshPointCloudContinuousCollisionTraversalNode<BV>& node,
node.prev_vertices1 = model1.prev_vertices;
node.prev_vertices2 = model2.prev_vertices;
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
+ node.request = request;
return true;
}
@@ -1163,7 +1165,7 @@ template<typename BV>
bool initialize(PointCloudMeshContinuousCollisionTraversalNode<BV>& node,
const BVHModel<BV>& model1, const SimpleTransform& tf1,
const BVHModel<BV>& model2, const SimpleTransform& tf2,
- int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
+ const CollisionRequest& request)
{
if(!(model1.getModelType() == BVH_MODEL_TRIANGLES || model1.getModelType() == BVH_MODEL_POINTCLOUD) || model2.getModelType() != BVH_MODEL_TRIANGLES)
return false;
@@ -1180,9 +1182,7 @@ bool initialize(PointCloudMeshContinuousCollisionTraversalNode<BV>& node,
node.prev_vertices1 = model1.prev_vertices;
node.prev_vertices2 = model2.prev_vertices;
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
+ node.request = request;
return true;
}
diff --git a/trunk/fcl/include/fcl/traversal_node_base.h b/trunk/fcl/include/fcl/traversal_node_base.h
index 6557ef168d5fda12daadea7a91d31486300af188..3881cc9f4fa901acc09e81a036bdab30c84560b8 100644
--- a/trunk/fcl/include/fcl/traversal_node_base.h
+++ b/trunk/fcl/include/fcl/traversal_node_base.h
@@ -39,6 +39,7 @@
#include "fcl/primitive.h"
#include "fcl/transform.h"
+#include "fcl/collision_data.h"
/** \brief Main namespace */
namespace fcl
@@ -100,6 +101,8 @@ public:
/** \brief Check whether the traversal can stop */
virtual bool canStop() const;
+
+ CollisionRequest request;
};
class DistanceTraversalNodeBase : public TraversalNodeBase
@@ -114,6 +117,8 @@ public:
virtual void leafTesting(int b1, int b2) const;
virtual bool canStop(FCL_REAL c) const;
+
+ DistanceRequest request;
};
}
diff --git a/trunk/fcl/include/fcl/traversal_node_bvh_shape.h b/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
index da72cc0ded989d989163eb5207eed8f065c6bfa2..848258f0c25d86cbcd8d50a21e45c595fd6b0e0c 100644
--- a/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
+++ b/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
@@ -38,6 +38,7 @@
#ifndef FCL_TRAVERSAL_NODE_MESH_SHAPE_H
#define FCL_TRAVERSAL_NODE_MESH_SHAPE_H
+#include "fcl/collision_data.h"
#include "fcl/geometric_shapes.h"
#include "fcl/traversal_node_base.h"
#include "fcl/BVH_model.h"
@@ -183,10 +184,6 @@ public:
vertices = NULL;
tri_indices = NULL;
- num_max_contacts = 1;
- exhaustive = false;
- enable_contact = false;
-
nsolver = NULL;
}
@@ -207,7 +204,7 @@ public:
Vec3f normal;
Vec3f contactp;
- if(!enable_contact) // only interested in collision or not
+ if(!request.enable_contact) // only interested in collision or not
{
if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, NULL, NULL, NULL))
{
@@ -225,15 +222,13 @@ public:
bool canStop() const
{
- return (pairs.size() > 0) && (!exhaustive) && (num_max_contacts <= (int)pairs.size());
+ return (pairs.size() > 0) && (!request.exhaustive) && (request.num_max_contacts <= (int)pairs.size());
}
Vec3f* vertices;
Triangle* tri_indices;
- int num_max_contacts;
- bool exhaustive;
- bool enable_contact;
+ CollisionRequest request;
mutable std::vector<BVHShapeCollisionPair> pairs;
@@ -251,9 +246,7 @@ static inline void meshShapeCollisionOrientedNodeLeafTesting(int b1, int b2,
const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
bool enable_statistics,
- bool enable_contact,
- bool exhaustive,
- int num_max_contacts,
+ const CollisionRequest& request,
int& num_leaf_tests,
std::vector<BVHShapeCollisionPair>& pairs)
@@ -273,7 +266,7 @@ static inline void meshShapeCollisionOrientedNodeLeafTesting(int b1, int b2,
Vec3f normal;
Vec3f contactp;
- if(!enable_contact) // only interested in collision or not
+ if(!request.enable_contact) // only interested in collision or not
{
if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, R, T, NULL, NULL, NULL))
{
@@ -309,8 +302,7 @@ public:
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
- R, T, this->tf2, this->nsolver, this->enable_statistics, this->enable_contact,
- this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
+ R, T, this->tf2, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
}
// R, T is the transform of bvh model
@@ -336,8 +328,7 @@ public:
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
- R, T, this->tf2, this->nsolver, this->enable_statistics, this->enable_contact,
- this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
+ R, T, this->tf2, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
}
// R, T is the transform of bvh model
@@ -363,8 +354,7 @@ public:
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
- R, T, this->tf2, this->nsolver, this->enable_statistics, this->enable_contact,
- this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
+ R, T, this->tf2, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
}
// R, T is the transform of bvh model
@@ -390,8 +380,7 @@ public:
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
- R, T, this->tf2, this->nsolver, this->enable_statistics, this->enable_contact,
- this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
+ R, T, this->tf2, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
}
// R, T is the transform of bvh model
@@ -408,10 +397,6 @@ public:
vertices = NULL;
tri_indices = NULL;
- num_max_contacts = 1;
- exhaustive = false;
- enable_contact = false;
-
nsolver = NULL;
}
@@ -432,7 +417,7 @@ public:
Vec3f normal;
Vec3f contactp;
- if(!enable_contact) // only interested in collision or not
+ if(!request.enable_contact) // only interested in collision or not
{
if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, NULL, NULL, NULL))
{
@@ -450,15 +435,13 @@ public:
bool canStop() const
{
- return (pairs.size() > 0) && (!exhaustive) && (num_max_contacts <= (int)pairs.size());
+ return (pairs.size() > 0) && (!request.exhaustive) && (request.num_max_contacts <= (int)pairs.size());
}
Vec3f* vertices;
Triangle* tri_indices;
- int num_max_contacts;
- bool exhaustive;
- bool enable_contact;
+ CollisionRequest request;
mutable std::vector<BVHShapeCollisionPair> pairs;
@@ -483,8 +466,7 @@ public:
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
- R, T, this->tf1, this->nsolver, this->enable_statistics, this->enable_contact,
- this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
+ R, T, this->tf1, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
// may need to change the order in pairs
}
@@ -513,8 +495,7 @@ public:
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
- R, T, this->tf1, this->nsolver, this->enable_statistics, this->enable_contact,
- this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
+ R, T, this->tf1, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
// may need to change the order in pairs
}
@@ -543,8 +524,7 @@ public:
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
- R, T, this->tf1, this->nsolver, this->enable_statistics, this->enable_contact,
- this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
+ R, T, this->tf1, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
// may need to change the order in pairs
}
@@ -573,8 +553,7 @@ public:
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
- R, T, this->tf1, this->nsolver, this->enable_statistics, this->enable_contact,
- this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
+ R, T, this->tf1, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
// may need to change the order in pairs
}
diff --git a/trunk/fcl/include/fcl/traversal_node_bvhs.h b/trunk/fcl/include/fcl/traversal_node_bvhs.h
index f9df09ffb78d6dd7d3d6bc670e13b6e0af9ed5ad..a169a8e387db7df94d8cbb9a8c7219658e0a21b7 100644
--- a/trunk/fcl/include/fcl/traversal_node_bvhs.h
+++ b/trunk/fcl/include/fcl/traversal_node_bvhs.h
@@ -38,6 +38,7 @@
#ifndef FCL_TRAVERSAL_NODE_MESHES_H
#define FCL_TRAVERSAL_NODE_MESHES_H
+#include "fcl/collision_data.h"
#include "fcl/traversal_node_base.h"
#include "fcl/BV_node.h"
#include "fcl/BV.h"
@@ -188,10 +189,6 @@ public:
vertices2 = NULL;
tri_indices1 = NULL;
tri_indices2 = NULL;
-
- num_max_contacts = 1;
- exhaustive = false;
- enable_contact = false;
}
/** \brief Intersection testing between leaves (two triangles) */
@@ -221,7 +218,7 @@ public:
Vec3f contacts[2];
- if(!enable_contact) // only interested in collision or not
+ if(!this->request.enable_contact) // only interested in collision or not
{
if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3))
{
@@ -238,7 +235,7 @@ public:
{
for(int i = 0; i < n_contacts; ++i)
{
- if((!exhaustive) && (num_max_contacts <= (int)pairs.size())) break;
+ if((!this->request.exhaustive) && (this->request.num_max_contacts <= (int)pairs.size())) break;
pairs.push_back(BVHCollisionPair(primitive_id1, primitive_id2, contacts[i], normal, penetration));
}
}
@@ -248,7 +245,7 @@ public:
/** \brief Whether the traversal process can stop early */
bool canStop() const
{
- return (pairs.size() > 0) && (!exhaustive) && (num_max_contacts <= (int)pairs.size());
+ return (pairs.size() > 0) && (!this->request.exhaustive) && (this->request.num_max_contacts <= (int)pairs.size());
}
Vec3f* vertices1;
@@ -257,10 +254,6 @@ public:
Triangle* tri_indices1;
Triangle* tri_indices2;
- int num_max_contacts;
- bool exhaustive;
- bool enable_contact;
-
mutable std::vector<BVHCollisionPair> pairs;
};
@@ -365,10 +358,6 @@ public:
vertices1 = NULL;
vertices2 = NULL;
- num_max_contacts = 1;
- exhaustive = false;
- enable_contact = false;
-
collision_prob_threshold = 0.5;
max_collision_prob = 0;
leaf_size_threshold = 1;
@@ -409,7 +398,7 @@ public:
bool canStop() const
{
- return (pairs.size() > 0) && (!exhaustive) && (num_max_contacts <= (int)pairs.size());
+ return (pairs.size() > 0) && (!this->request.exhaustive) && (this->request.num_max_contacts <= (int)pairs.size());
}
Vec3f* vertices1;
@@ -418,10 +407,6 @@ public:
boost::shared_array<Uncertainty> uc1;
boost::shared_array<Uncertainty> uc2;
- int num_max_contacts;
- bool exhaustive;
- bool enable_contact;
-
mutable std::vector<BVHPointCollisionPair> pairs;
int leaf_size_threshold;
@@ -474,10 +459,6 @@ public:
vertices2 = NULL;
tri_indices2 = NULL;
- num_max_contacts = 1;
- exhaustive = false;
- enable_contact = false;
-
collision_prob_threshold = 0.5;
max_collision_prob = 0;
leaf_size_threshold = 1;
@@ -515,7 +496,7 @@ public:
bool canStop() const
{
- return (pairs.size() > 0) && (!exhaustive) && (num_max_contacts <= (int)pairs.size());
+ return (pairs.size() > 0) && (!this->request.exhaustive) && (this->request.num_max_contacts <= (int)pairs.size());
}
Vec3f* vertices1;
@@ -524,10 +505,6 @@ public:
boost::shared_array<Uncertainty> uc1;
Triangle* tri_indices2;
- int num_max_contacts;
- bool exhaustive;
- bool enable_contact;
-
mutable std::vector<BVHPointCollisionPair> pairs;
int leaf_size_threshold;
@@ -596,10 +573,6 @@ public:
prev_vertices1 = NULL;
prev_vertices2 = NULL;
- num_max_contacts = 1;
- exhaustive = false;
- enable_contact = false;
-
num_vf_tests = 0;
num_ee_tests = 0;
}
@@ -688,7 +661,7 @@ public:
bool canStop() const
{
- return (pairs.size() > 0) && (!exhaustive) && (num_max_contacts <= (int)pairs.size());
+ return (pairs.size() > 0) && (!this->request.exhaustive) && (this->request.num_max_contacts <= (int)pairs.size());
}
Vec3f* vertices1;
@@ -700,10 +673,6 @@ public:
Vec3f* prev_vertices1;
Vec3f* prev_vertices2;
- int num_max_contacts;
- bool exhaustive;
- bool enable_contact;
-
mutable int num_vf_tests;
mutable int num_ee_tests;
@@ -723,10 +692,6 @@ public:
prev_vertices1 = NULL;
prev_vertices2 = NULL;
- num_max_contacts = 1;
- exhaustive = false;
- enable_contact = false;
-
num_vf_tests = 0;
}
@@ -781,7 +746,7 @@ public:
bool canStop() const
{
- return (pairs.size() > 0) && (!exhaustive) && (num_max_contacts <= (int)pairs.size());
+ return (pairs.size() > 0) && (!this->request.exhaustive) && (this->request.num_max_contacts <= (int)pairs.size());
}
Vec3f* vertices1;
@@ -792,10 +757,6 @@ public:
Vec3f* prev_vertices1;
Vec3f* prev_vertices2;
- int num_max_contacts;
- bool exhaustive;
- bool enable_contact;
-
mutable int num_vf_tests;
mutable std::vector<BVHContinuousCollisionPair> pairs;
@@ -814,10 +775,6 @@ public:
prev_vertices1 = NULL;
prev_vertices2 = NULL;
- num_max_contacts = 1;
- exhaustive = false;
- enable_contact = false;
-
num_vf_tests = 0;
}
@@ -872,7 +829,7 @@ public:
bool canStop() const
{
- return (pairs.size() > 0) && (!exhaustive) && (num_max_contacts <= (int)pairs.size());
+ return (pairs.size() > 0) && (!this->request.exhaustive) && (this->request.num_max_contacts <= (int)pairs.size());
}
Vec3f* vertices1;
@@ -883,10 +840,6 @@ public:
Vec3f* prev_vertices1;
Vec3f* prev_vertices2;
- int num_max_contacts;
- bool exhaustive;
- bool enable_contact;
-
mutable int num_vf_tests;
mutable std::vector<BVHContinuousCollisionPair> pairs;
diff --git a/trunk/fcl/include/fcl/traversal_node_octree.h b/trunk/fcl/include/fcl/traversal_node_octree.h
index a6dc192235242f987fcf2b4f851ae9b7d5016cd2..83391489eb7e914bab6de252224e87450e16f13a 100644
--- a/trunk/fcl/include/fcl/traversal_node_octree.h
+++ b/trunk/fcl/include/fcl/traversal_node_octree.h
@@ -37,6 +37,7 @@
#ifndef FCL_TRAVERSAL_NODE_OCTREE_H
#define FCL_TRAVERSAL_NODE_OCTREE_H
+#include "fcl/collision_data.h"
#include "fcl/traversal_node_base.h"
#include "fcl/narrowphase/narrowphase.h"
#include "fcl/geometric_shapes_utility.h"
@@ -122,9 +123,7 @@ class OcTreeSolver
private:
const NarrowPhaseSolver* solver;
- mutable int num_max_contacts;
- mutable bool enable_contact;
- mutable bool exhaustive;
+ mutable CollisionRequest request;
public:
OcTreeSolver(const NarrowPhaseSolver* solver_) : solver(solver_) {}
@@ -132,11 +131,9 @@ public:
void OcTreeIntersect(const OcTree* tree1, const OcTree* tree2,
const SimpleTransform& tf1, const SimpleTransform& tf2,
std::vector<OcTreeCollisionPair>& pairs,
- int num_max_contacts_, bool enable_contact_, bool exhaustive_) const
+ const CollisionRequest& request_) const
{
- num_max_contacts = num_max_contacts_;
- enable_contact = enable_contact_;
- exhaustive = exhaustive_;
+ request = request_;
OcTreeIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
tree2, tree2->getRoot(), tree2->getRootBV(),
@@ -160,11 +157,9 @@ public:
void OcTreeMeshIntersect(const OcTree* tree1, const BVHModel<BV>* tree2,
const SimpleTransform& tf1, const SimpleTransform& tf2,
std::vector<OcTreeMeshCollisionPair>& pairs,
- int num_max_contacts_, bool enable_contact_, bool exhaustive_) const
+ const CollisionRequest& request_) const
{
- num_max_contacts = num_max_contacts_;
- enable_contact = enable_contact_;
- exhaustive = exhaustive_;
+ request = request_;
OcTreeMeshIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
tree2, 0,
@@ -189,12 +184,10 @@ public:
void MeshOcTreeIntersect(const BVHModel<BV>* tree1, const OcTree* tree2,
const SimpleTransform& tf1, const SimpleTransform& tf2,
std::vector<OcTreeMeshCollisionPair>& pairs,
- int num_max_contacts_, bool enable_contact_, bool exhaustive_) const
+ const CollisionRequest& request_) const
{
- num_max_contacts = num_max_contacts_;
- enable_contact = enable_contact_;
- exhaustive = exhaustive_;
+ request = request_;
OcTreeMeshIntersectRecurse(tree2, tree2->getRoot(), tree2->getRootBV(),
tree1, 0,
@@ -219,11 +212,9 @@ public:
void OcTreeShapeIntersect(const OcTree* tree, const S& s,
const SimpleTransform& tf1, const SimpleTransform& tf2,
std::vector<OcTreeShapeCollisionPair>& pairs,
- int num_max_contacts_, bool enable_contact_, bool exhaustive_) const
+ const CollisionRequest& request_) const
{
- num_max_contacts = num_max_contacts_;
- enable_contact = enable_contact_;
- exhaustive = exhaustive_;
+ request = request_;
AABB bv2;
computeBV<AABB>(s, SimpleTransform(), bv2);
@@ -239,11 +230,9 @@ public:
void ShapeOcTreeIntersect(const S& s, const OcTree* tree,
const SimpleTransform& tf1, const SimpleTransform& tf2,
std::vector<OcTreeShapeCollisionPair>& pairs,
- int num_max_contacts_, bool enable_contact_, bool exhaustive_) const
+ const CollisionRequest& request_) const
{
- num_max_contacts = num_max_contacts_;
- enable_contact = enable_contact_;
- exhaustive = exhaustive_;
+ request = request_;
AABB bv1;
computeBV<AABB>(s, SimpleTransform(), bv1);
@@ -351,7 +340,7 @@ private:
SimpleTransform box_tf;
constructBox(bv1, tf1, box, box_tf);
- if(!enable_contact)
+ if(!request.enable_contact)
{
if(solver->shapeIntersect(box, box_tf, s, tf2, NULL, NULL, NULL))
pairs.push_back(OcTreeShapeCollisionPair(root1));
@@ -366,7 +355,7 @@ private:
pairs.push_back(OcTreeShapeCollisionPair(root1, contact, normal, depth));
}
- return ((pairs.size() >= num_max_contacts) && !exhaustive);
+ return ((pairs.size() >= request.num_max_contacts) && !request.exhaustive);
}
else return false;
}
@@ -503,7 +492,7 @@ private:
const Vec3f& p2 = tree2->vertices[tri_id[1]];
const Vec3f& p3 = tree2->vertices[tri_id[2]];
- if(!enable_contact)
+ if(!request.enable_contact)
{
if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2.getRotation(), tf2.getTranslation(), NULL, NULL, NULL))
pairs.push_back(OcTreeMeshCollisionPair(root1, root2));
@@ -518,7 +507,7 @@ private:
pairs.push_back(OcTreeMeshCollisionPair(root1, root2, contact, normal, depth));
}
- return ((pairs.size() >= num_max_contacts) && !exhaustive);
+ return ((pairs.size() >= request.num_max_contacts) && !request.exhaustive);
}
else
return false;
@@ -650,7 +639,7 @@ private:
{
if(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2))
{
- if(!enable_contact)
+ if(!request.enable_contact)
{
OBB obb1, obb2;
convertBV(bv1, tf1, obb1);
@@ -673,7 +662,7 @@ private:
pairs.push_back(OcTreeCollisionPair(root1, root2, contact, normal, depth));
}
- return ((pairs.size() >= num_max_contacts) && !exhaustive);
+ return ((pairs.size() >= request.num_max_contacts) && !request.exhaustive);
}
else
return false;
@@ -738,10 +727,6 @@ public:
model1 = NULL;
model2 = NULL;
- num_max_contacts = 1;
- enable_contact = false;
- exhaustive = false;
-
otsolver = NULL;
}
@@ -752,16 +737,12 @@ public:
void leafTesting(int, int) const
{
- otsolver->OcTreeIntersect(model1, model2, tf1, tf2, pairs, num_max_contacts, enable_contact, exhaustive);
+ otsolver->OcTreeIntersect(model1, model2, tf1, tf2, pairs, request);
}
const OcTree* model1;
const OcTree* model2;
- int num_max_contacts;
- bool exhaustive;
- bool enable_contact;
-
SimpleTransform tf1, tf2;
mutable std::vector<OcTreeCollisionPair> pairs;
@@ -810,10 +791,6 @@ public:
model1 = NULL;
model2 = NULL;
- num_max_contacts = 1;
- enable_contact = false;
- exhaustive = false;
-
otsolver = NULL;
}
@@ -824,16 +801,12 @@ public:
void leafTesting(int, int) const
{
- otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, pairs, num_max_contacts, enable_contact, exhaustive);
+ otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, pairs, request);
}
const S* model1;
const OcTree* model2;
- int num_max_contacts;
- bool exhaustive;
- bool enable_contact;
-
SimpleTransform tf1, tf2;
mutable std::vector<OcTreeShapeCollisionPair> pairs;
@@ -850,10 +823,6 @@ public:
model1 = NULL;
model2 = NULL;
- num_max_contacts = 1;
- enable_contact = false;
- exhaustive = false;
-
otsolver = NULL;
}
@@ -864,16 +833,12 @@ public:
void leafTesting(int, int) const
{
- otsolver->OcTreeShapeIntersect(model1, *model2, tf1, tf2, pairs, num_max_contacts, enable_contact, exhaustive);
+ otsolver->OcTreeShapeIntersect(model1, *model2, tf1, tf2, pairs, request);
}
const OcTree* model1;
const S* model2;
- int num_max_contacts;
- bool exhaustive;
- bool enable_contact;
-
SimpleTransform tf1, tf2;
mutable std::vector<OcTreeShapeCollisionPair> pairs;
@@ -951,10 +916,6 @@ public:
model1 = NULL;
model2 = NULL;
- num_max_contacts = 1;
- enable_contact = false;
- exhaustive = false;
-
otsolver = NULL;
}
@@ -965,16 +926,12 @@ public:
void leafTesting(int, int) const
{
- otsolver->OcTreeMeshIntersect(model2, model1, tf2, tf1, pairs, num_max_contacts, enable_contact, exhaustive);
+ otsolver->OcTreeMeshIntersect(model2, model1, tf2, tf1, pairs, request);
}
const BVHModel<BV>* model1;
const OcTree* model2;
- int num_max_contacts;
- bool exhaustive;
- bool enable_contact;
-
SimpleTransform tf1, tf2;
mutable std::vector<OcTreeMeshCollisionPair> pairs;
@@ -991,10 +948,6 @@ public:
model1 = NULL;
model2 = NULL;
- num_max_contacts = 1;
- enable_contact = false;
- exhaustive = false;
-
otsolver = NULL;
}
@@ -1005,16 +958,12 @@ public:
void leafTesting(int, int) const
{
- otsolver->OcTreeMeshIntersect(model1, model2, tf1, tf2, pairs, num_max_contacts, enable_contact, exhaustive);
+ otsolver->OcTreeMeshIntersect(model1, model2, tf1, tf2, pairs, request);
}
const OcTree* model1;
const BVHModel<BV>* model2;
- int num_max_contacts;
- bool exhaustive;
- bool enable_contact;
-
SimpleTransform tf1, tf2;
mutable std::vector<OcTreeMeshCollisionPair> pairs;
diff --git a/trunk/fcl/include/fcl/traversal_node_shapes.h b/trunk/fcl/include/fcl/traversal_node_shapes.h
index e413ecc999be330daa674b33bcb13f075eff30f1..e545996e52dd0a71900fe55f05c5db755c0812a1 100644
--- a/trunk/fcl/include/fcl/traversal_node_shapes.h
+++ b/trunk/fcl/include/fcl/traversal_node_shapes.h
@@ -38,6 +38,7 @@
#ifndef FCL_TRAVERSAL_NODE_SHAPES_H
#define FCL_TRAVERSAL_NODE_SHAPES_H
+#include "fcl/collision_data.h"
#include "fcl/traversal_node_base.h"
#include "fcl/narrowphase/narrowphase.h"
@@ -53,7 +54,6 @@ public:
model1 = NULL;
model2 = NULL;
- enable_contact = false;
is_collision = false;
nsolver = NULL;
@@ -66,7 +66,7 @@ public:
void leafTesting(int, int) const
{
- if(enable_contact)
+ if(request.enable_contact)
is_collision = nsolver->shapeIntersect(*model1, tf1, *model2, tf2, &contact_point, &penetration_depth, &normal);
else
is_collision = nsolver->shapeIntersect(*model1, tf1, *model2, tf2, NULL, NULL, NULL);
@@ -82,7 +82,7 @@ public:
mutable FCL_REAL penetration_depth;
- bool enable_contact;
+ CollisionRequest request;
mutable bool is_collision;
diff --git a/trunk/fcl/src/broad_phase_collision.cpp b/trunk/fcl/src/broad_phase_collision.cpp
index 1d01ea4dcab5c9b226b6ed0f38417711006c8e58..3fb5808a20ee572c20b5a55d4aa9a8106e98b9cc 100644
--- a/trunk/fcl/src/broad_phase_collision.cpp
+++ b/trunk/fcl/src/broad_phase_collision.cpp
@@ -52,21 +52,22 @@ namespace fcl
bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cdata_)
{
CollisionData* cdata = static_cast<CollisionData*>(cdata_);
+ const CollisionRequest& request = cdata->request;
+ CollisionResult& result = cdata->result;
if(cdata->done) return true;
- std::vector<Contact> contacts;
- // int num_contacts = collide(o1, o2, 1, false, false, contacts);
- int num_contacts = collide(o1, o2, cdata->num_max_contacts, cdata->exhaustive, cdata->enable_contact, contacts);
+ CollisionResult local_result;
+ int num_contacts = collide(o1, o2, request, local_result);
- cdata->is_collision = (num_contacts > 0);
+ result.is_collision = (num_contacts > 0);
for(int i = 0; i < num_contacts; ++i)
{
- cdata->contacts.push_back(contacts[i]);
+ result.contacts.push_back(local_result.contacts[i]);
}
// set done flag
- if( (!cdata->exhaustive) && (cdata->is_collision) && (cdata->contacts.size() >= cdata->num_max_contacts))
+ if( (!request.exhaustive) && (result.is_collision) && (result.contacts.size() >= request.num_max_contacts))
cdata->done = true;
return cdata->done;
@@ -75,13 +76,17 @@ bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cd
bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cdata_, FCL_REAL& dist)
{
DistanceData* cdata = static_cast<DistanceData*>(cdata_);
-
- if(cdata->done) { dist = cdata->min_distance; return true; }
+ const DistanceRequest& request = cdata->request;
+ DistanceResult& result = cdata->result;
+
+ if(cdata->done) { dist = result.min_distance; return true; }
- FCL_REAL d = distance(o1, o2);
- if(cdata->min_distance > d) cdata->min_distance = d;
+ DistanceResult local_result;
+ distance(o1, o2, request, local_result);
+ FCL_REAL d = local_result.min_distance;
+ if(result.min_distance > d) result.min_distance = d;
- dist = cdata->min_distance;
+ dist = result.min_distance;
if(dist <= 0) return true; // in collision or in touch
diff --git a/trunk/fcl/src/collision.cpp b/trunk/fcl/src/collision.cpp
index 64a2b194db9a67ebb5b70970aadfad40de4ff7c0..90836d50bc3e12865101635c5cd94d7c0d7c9f85 100644
--- a/trunk/fcl/src/collision.cpp
+++ b/trunk/fcl/src/collision.cpp
@@ -54,20 +54,19 @@ CollisionFunctionMatrix<GJKSolver>& getCollisionFunctionLookTable()
template<typename NarrowPhaseSolver>
int collide(const CollisionObject* o1, const CollisionObject* o2,
const NarrowPhaseSolver* nsolver,
- int num_max_contacts, bool exhaustive, bool enable_contact,
- std::vector<Contact>& contacts)
+ const CollisionRequest& request,
+ CollisionResult& result)
{
return collide(o1->getCollisionGeometry(), o1->getTransform(), o2->getCollisionGeometry(), o2->getTransform(),
- nsolver,
- num_max_contacts, exhaustive, enable_contact, contacts);
+ nsolver, request, result);
}
template<typename NarrowPhaseSolver>
int collide(const CollisionGeometry* o1, const SimpleTransform& tf1,
const CollisionGeometry* o2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver_,
- int num_max_contacts, bool exhaustive, bool enable_contact,
- std::vector<Contact>& contacts)
+ const CollisionRequest& request,
+ CollisionResult& result)
{
const NarrowPhaseSolver* nsolver = nsolver_;
if(!nsolver_)
@@ -76,9 +75,9 @@ int collide(const CollisionGeometry* o1, const SimpleTransform& tf1,
const CollisionFunctionMatrix<NarrowPhaseSolver>& looktable = getCollisionFunctionLookTable<NarrowPhaseSolver>();
int res;
- if(num_max_contacts <= 0 && !exhaustive)
+ if(request.num_max_contacts <= 0 && !request.exhaustive)
{
- std::cerr << "Warning: should stop early as num_max_contact is " << num_max_contacts << " !" << std::endl;
+ std::cerr << "Warning: should stop early as num_max_contact is " << request.num_max_contacts << " !" << std::endl;
res = 0;
}
else
@@ -96,7 +95,7 @@ int collide(const CollisionGeometry* o1, const SimpleTransform& tf1,
res = 0;
}
else
- res = looktable.collision_matrix[node_type2][node_type1](o2, tf2, o1, tf1, nsolver, num_max_contacts, exhaustive, enable_contact, contacts);
+ res = looktable.collision_matrix[node_type2][node_type1](o2, tf2, o1, tf1, nsolver, request, result);
}
else
{
@@ -106,7 +105,7 @@ int collide(const CollisionGeometry* o1, const SimpleTransform& tf1,
res = 0;
}
else
- res = looktable.collision_matrix[node_type1][node_type2](o1, tf1, o2, tf2, nsolver, num_max_contacts, exhaustive, enable_contact, contacts);
+ res = looktable.collision_matrix[node_type1][node_type2](o1, tf1, o2, tf2, nsolver, request, result);
}
}
@@ -116,29 +115,27 @@ int collide(const CollisionGeometry* o1, const SimpleTransform& tf1,
return res;
}
-template int collide(const CollisionObject* o1, const CollisionObject* o2, const GJKSolver_libccd* nsolver, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts);
-template int collide(const CollisionObject* o1, const CollisionObject* o2, const GJKSolver_indep* nsolver, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts);
-template int collide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const GJKSolver_libccd* nsolver, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts);
-template int collide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const GJKSolver_indep* nsolver, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts);
+template int collide(const CollisionObject* o1, const CollisionObject* o2, const GJKSolver_libccd* nsolver, const CollisionRequest& request, CollisionResult& result);
+template int collide(const CollisionObject* o1, const CollisionObject* o2, const GJKSolver_indep* nsolver, const CollisionRequest& request, CollisionResult& result);
+template int collide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const GJKSolver_libccd* nsolver, const CollisionRequest& request, CollisionResult& result);
+template int collide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const GJKSolver_indep* nsolver, const CollisionRequest& request, CollisionResult& result);
int collide(const CollisionObject* o1, const CollisionObject* o2,
- int num_max_contacts, bool exhaustive, bool enable_contact,
- std::vector<Contact>& contacts)
+ const CollisionRequest& request, CollisionResult& result)
{
GJKSolver_libccd solver;
- return collide<GJKSolver_libccd>(o1, o2, &solver, num_max_contacts, exhaustive, enable_contact, contacts);
+ return collide<GJKSolver_libccd>(o1, o2, &solver, request, result);
}
int collide(const CollisionGeometry* o1, const SimpleTransform& tf1,
const CollisionGeometry* o2, const SimpleTransform& tf2,
- int num_max_contacts, bool exhaustive, bool enable_contact,
- std::vector<Contact>& contacts)
+ const CollisionRequest& request, CollisionResult& result)
{
GJKSolver_libccd solver;
- return collide<GJKSolver_libccd>(o1, tf1, o2, tf2, &solver, num_max_contacts, exhaustive, enable_contact, contacts);
+ return collide<GJKSolver_libccd>(o1, tf1, o2, tf2, &solver, request, result);
// GJKSolver_indep solver;
- // return collide<GJKSolver_indep>(o1, tf1, o2, tf2, &solver, num_max_contacts, exhaustive, enable_contact, contacts);
+ // return collide<GJKSolver_indep>(o1, tf1, o2, tf2, &solver, request, result);
}
}
diff --git a/trunk/fcl/src/collision_func_matrix.cpp b/trunk/fcl/src/collision_func_matrix.cpp
index 09ffe9c46ec7b653cf957ecfb1474b83093b697a..9e2b8989b8ecea0c216e1984f908881a143b84ec 100644
--- a/trunk/fcl/src/collision_func_matrix.cpp
+++ b/trunk/fcl/src/collision_func_matrix.cpp
@@ -47,22 +47,22 @@ namespace fcl
template<typename T_SH>
static inline int OcTreeShapeContactCollection(const std::vector<OcTreeShapeCollisionPair>& pairs, const OcTree* obj1, const T_SH* obj2,
- int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+ const CollisionRequest& request, CollisionResult& result)
{
int num_contacts = pairs.size();
if(num_contacts > 0)
{
- if((!exhaustive) && (num_contacts > num_max_contacts)) num_contacts = num_max_contacts;
- contacts.resize(num_contacts);
- if(!enable_contact)
+ if((!request.exhaustive) && (num_contacts > request.num_max_contacts)) num_contacts = request.num_max_contacts;
+ result.contacts.resize(num_contacts);
+ if(!request.enable_contact)
{
for(int i = 0; i < num_contacts; ++i)
- contacts[i] = Contact(obj1, obj2, pairs[i].node - obj1->getRoot(), 0);
+ result.contacts[i] = Contact(obj1, obj2, pairs[i].node - obj1->getRoot(), 0);
}
else
{
for(int i = 0; i < num_contacts; ++i)
- contacts[i] = Contact(obj1, obj2, pairs[i].node - obj1->getRoot(), 0, pairs[i].contact_point, pairs[i].normal, pairs[i].penetration_depth);
+ result.contacts[i] = Contact(obj1, obj2, pairs[i].node - obj1->getRoot(), 0, pairs[i].contact_point, pairs[i].normal, pairs[i].penetration_depth);
}
}
@@ -72,15 +72,15 @@ static inline int OcTreeShapeContactCollection(const std::vector<OcTreeShapeColl
template<typename T_SH, typename NarrowPhaseSolver>
int ShapeOcTreeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
- int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+ const CollisionRequest& request, CollisionResult& result)
{
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);
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, num_max_contacts, exhaustive, enable_contact);
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request);
collide(&node);
- int num_contacts = OcTreeShapeContactCollection(node.pairs, obj2, obj1, num_max_contacts, exhaustive, enable_contact, contacts);
+ int num_contacts = OcTreeShapeContactCollection(node.pairs, obj2, obj1, request, result);
return num_contacts;
}
@@ -88,37 +88,37 @@ int ShapeOcTreeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1,
template<typename T_SH, typename NarrowPhaseSolver>
int OcTreeShapeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
- int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+ const CollisionRequest& request, CollisionResult& result)
{
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);
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, num_max_contacts, exhaustive, enable_contact);
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request);
collide(&node);
- int num_contacts = OcTreeShapeContactCollection(node.pairs, obj1, obj2, num_max_contacts, exhaustive, enable_contact, contacts);
+ int num_contacts = OcTreeShapeContactCollection(node.pairs, obj1, obj2, request, result);
return num_contacts;
}
static inline int OcTreeContactCollection(const std::vector<OcTreeCollisionPair>& pairs, const OcTree* obj1, const OcTree* obj2,
- int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+ const CollisionRequest& request, CollisionResult& result)
{
int num_contacts = pairs.size();
if(num_contacts > 0)
{
- if((!exhaustive) && (num_contacts > num_max_contacts)) num_contacts = num_max_contacts;
- contacts.resize(num_contacts);
- if(!enable_contact)
+ if((!request.exhaustive) && (num_contacts > request.num_max_contacts)) num_contacts = request.num_max_contacts;
+ result.contacts.resize(num_contacts);
+ if(!request.enable_contact)
{
for(int i = 0; i < num_contacts; ++i)
- contacts[i] = Contact(obj1, obj2, pairs[i].node1 - obj1->getRoot(), pairs[i].node2 - obj2->getRoot());
+ result.contacts[i] = Contact(obj1, obj2, pairs[i].node1 - obj1->getRoot(), pairs[i].node2 - obj2->getRoot());
}
else
{
for(int i = 0; i < num_contacts; ++i)
- contacts[i] = Contact(obj1, obj2, pairs[i].node1 - obj1->getRoot(), pairs[i].node2 - obj2->getRoot(), pairs[i].contact_point, pairs[i].normal, pairs[i].penetration_depth);
+ result.contacts[i] = Contact(obj1, obj2, pairs[i].node1 - obj1->getRoot(), pairs[i].node2 - obj2->getRoot(), pairs[i].contact_point, pairs[i].normal, pairs[i].penetration_depth);
}
}
@@ -128,38 +128,38 @@ static inline int OcTreeContactCollection(const std::vector<OcTreeCollisionPair>
template<typename NarrowPhaseSolver>
int OcTreeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
- int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+ const CollisionRequest& request, CollisionResult& result)
{
OcTreeCollisionTraversalNode<NarrowPhaseSolver> node;
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, num_max_contacts, exhaustive, enable_contact);
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request);
collide(&node);
- int num_contacts = OcTreeContactCollection(node.pairs, obj1, obj2, num_max_contacts, exhaustive, enable_contact, contacts);
+ int num_contacts = OcTreeContactCollection(node.pairs, obj1, obj2, request, result);
return num_contacts;
}
template<typename T_BVH>
static inline int OcTreeBVHContactCollection(const std::vector<OcTreeMeshCollisionPair>& pairs, const OcTree* obj1, const BVHModel<T_BVH>* obj2,
- int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+ const CollisionRequest& request, CollisionResult& result)
{
int num_contacts = pairs.size();
if(num_contacts > 0)
{
- if((!exhaustive) && (num_contacts > num_max_contacts)) num_contacts = num_max_contacts;
- contacts.resize(num_contacts);
- if(!enable_contact)
+ if((!request.exhaustive) && (num_contacts > request.num_max_contacts)) num_contacts = request.num_max_contacts;
+ result.contacts.resize(num_contacts);
+ if(!request.enable_contact)
{
for(int i = 0; i < num_contacts; ++i)
- contacts[i] = Contact(obj1, obj2, pairs[i].node - obj1->getRoot(), pairs[i].id);
+ result.contacts[i] = Contact(obj1, obj2, pairs[i].node - obj1->getRoot(), pairs[i].id);
}
else
{
for(int i = 0; i < num_contacts; ++i)
- contacts[i] = Contact(obj1, obj2, pairs[i].node - obj1->getRoot(), pairs[i].id, pairs[i].contact_point, pairs[i].normal, pairs[i].penetration_depth);
+ result.contacts[i] = Contact(obj1, obj2, pairs[i].node - obj1->getRoot(), pairs[i].id, pairs[i].contact_point, pairs[i].normal, pairs[i].penetration_depth);
}
}
@@ -169,32 +169,32 @@ static inline int OcTreeBVHContactCollection(const std::vector<OcTreeMeshCollisi
template<typename T_BVH, typename NarrowPhaseSolver>
int OcTreeBVHCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
- int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+ const CollisionRequest& request, CollisionResult& result)
{
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);
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, num_max_contacts, exhaustive, enable_contact);
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request);
collide(&node);
- int num_contacts = OcTreeBVHContactCollection(node.pairs, obj1, obj2, num_max_contacts, exhaustive, enable_contact, contacts);
+ int num_contacts = OcTreeBVHContactCollection(node.pairs, obj1, obj2, request, result);
return num_contacts;
}
template<typename T_BVH, typename NarrowPhaseSolver>
int BVHOcTreeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
- int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+ const CollisionRequest& request, CollisionResult& result)
{
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);
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, num_max_contacts, exhaustive, enable_contact);
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request);
collide(&node);
- int num_contacts = OcTreeBVHContactCollection(node.pairs, obj2, obj1, num_max_contacts, exhaustive, enable_contact, contacts);
+ int num_contacts = OcTreeBVHContactCollection(node.pairs, obj2, obj1, request, result);
return num_contacts;
}
@@ -202,39 +202,40 @@ int BVHOcTreeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, co
template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>
int ShapeShapeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
- int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+ const CollisionRequest& request, CollisionResult& result)
{
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);
- initialize(node, *obj1, tf1, *obj2, tf2, nsolver, enable_contact);
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request);
collide(&node);
if(!node.is_collision) return 0;
- contacts.resize(1);
- if(!enable_contact) contacts[0] = Contact(o1, o2, 0, 0);
- else contacts[0] = Contact(o1, o2, 0, 0, node.contact_point, node.normal, node.penetration_depth);
+ result.contacts.resize(1);
+ if(!request.enable_contact) result.contacts[0] = Contact(o1, o2, 0, 0);
+ else result.contacts[0] = Contact(o1, o2, 0, 0, node.contact_point, node.normal, node.penetration_depth);
return 1;
}
template<typename T_BVH, typename T_SH>
-static inline int BVHShapeContactCollection(const std::vector<BVHShapeCollisionPair>& pairs, const BVHModel<T_BVH>* obj1, const T_SH* obj2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+static inline int BVHShapeContactCollection(const std::vector<BVHShapeCollisionPair>& 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((!exhaustive) && (num_contacts > num_max_contacts)) num_contacts = num_max_contacts;
- contacts.resize(num_contacts);
- if(!enable_contact)
+ if((!request.exhaustive) && (num_contacts > request.num_max_contacts)) num_contacts = request.num_max_contacts;
+ result.contacts.resize(num_contacts);
+ if(!request.enable_contact)
{
for(int i = 0; i < num_contacts; ++i)
- contacts[i] = Contact(obj1, obj2, pairs[i].id, 0);
+ result.contacts[i] = Contact(obj1, obj2, pairs[i].id, 0);
}
else
{
for(int i = 0; i < num_contacts; ++i)
- contacts[i] = Contact(obj1, obj2, pairs[i].id, 0, pairs[i].contact_point, pairs[i].normal, pairs[i].penetration_depth);
+ result.contacts[i] = Contact(obj1, obj2, pairs[i].id, 0, pairs[i].contact_point, pairs[i].normal, pairs[i].penetration_depth);
}
}
@@ -247,7 +248,7 @@ struct BVHShapeCollider
{
static int collide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
- int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+ const CollisionRequest& request, CollisionResult& result)
{
MeshShapeCollisionTraversalNode<T_BVH, T_SH, NarrowPhaseSolver> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
@@ -255,10 +256,10 @@ struct BVHShapeCollider
SimpleTransform tf1_tmp = tf1;
const T_SH* obj2 = static_cast<const T_SH*>(o2);
- initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, num_max_contacts, exhaustive, enable_contact);
+ initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request);
fcl::collide(&node);
- int num_contacts = BVHShapeContactCollection(node.pairs, obj1, obj2, num_max_contacts, exhaustive, enable_contact, contacts);
+ int num_contacts = BVHShapeContactCollection(node.pairs, obj1, obj2, request, result);
delete obj1_tmp;
return num_contacts;
@@ -271,16 +272,16 @@ struct BVHShapeCollider<OBB, T_SH, NarrowPhaseSolver>
{
static int collide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
- int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+ const CollisionRequest& request, CollisionResult& result)
{
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);
- initialize(node, *obj1, tf1, *obj2, tf2, nsolver, num_max_contacts, exhaustive, enable_contact);
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request);
fcl::collide(&node);
- return BVHShapeContactCollection(node.pairs, obj1, obj2, num_max_contacts, exhaustive, enable_contact, contacts);
+ return BVHShapeContactCollection(node.pairs, obj1, obj2, request, result);
}
};
@@ -290,16 +291,16 @@ struct BVHShapeCollider<RSS, T_SH, NarrowPhaseSolver>
{
static int collide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
- int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+ const CollisionRequest& request, CollisionResult& result)
{
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);
- initialize(node, *obj1, tf1, *obj2, tf2, nsolver, num_max_contacts, exhaustive, enable_contact);
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request);
fcl::collide(&node);
- return BVHShapeContactCollection(node.pairs, obj1, obj2, num_max_contacts, exhaustive, enable_contact, contacts);
+ return BVHShapeContactCollection(node.pairs, obj1, obj2, request, result);
}
};
@@ -309,16 +310,16 @@ struct BVHShapeCollider<kIOS, T_SH, NarrowPhaseSolver>
{
static int collide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
- int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+ const CollisionRequest& request, CollisionResult& result)
{
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);
- initialize(node, *obj1, tf1, *obj2, tf2, nsolver, num_max_contacts, exhaustive, enable_contact);
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request);
fcl::collide(&node);
- return BVHShapeContactCollection(node.pairs, obj1, obj2, num_max_contacts, exhaustive, enable_contact, contacts);
+ return BVHShapeContactCollection(node.pairs, obj1, obj2, request, result);
}
};
@@ -328,37 +329,37 @@ struct BVHShapeCollider<OBBRSS, T_SH, NarrowPhaseSolver>
{
static int collide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
- int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+ const CollisionRequest& request, CollisionResult& result)
{
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);
- initialize(node, *obj1, tf1, *obj2, tf2, nsolver, num_max_contacts, exhaustive, enable_contact);
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request);
fcl::collide(&node);
- return BVHShapeContactCollection(node.pairs, obj1, obj2, num_max_contacts, exhaustive, enable_contact, contacts);
+ return BVHShapeContactCollection(node.pairs, obj1, obj2, request, result);
}
};
template<typename T_BVH>
-static inline int BVHContactCollection(const std::vector<BVHCollisionPair>& pairs, const BVHModel<T_BVH>* obj1, const BVHModel<T_BVH>* obj2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+static inline int BVHContactCollection(const std::vector<BVHCollisionPair>& 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((!exhaustive) && (num_contacts > num_max_contacts)) num_contacts = num_max_contacts;
- contacts.resize(num_contacts);
- if(!enable_contact)
+ if((!request.exhaustive) && (num_contacts > request.num_max_contacts)) num_contacts = request.num_max_contacts;
+ result.contacts.resize(num_contacts);
+ if(!request.enable_contact)
{
for(int i = 0; i < num_contacts; ++i)
- contacts[i] = Contact(obj1, obj2, pairs[i].id1, pairs[i].id2);
+ result.contacts[i] = Contact(obj1, obj2, pairs[i].id1, pairs[i].id2);
}
else
{
for(int i = 0; i < num_contacts; ++i)
- contacts[i] = Contact(obj1, obj2, pairs[i].id1, pairs[i].id2, pairs[i].contact_point, pairs[i].normal, pairs[i].penetration_depth);
+ result.contacts[i] = Contact(obj1, obj2, pairs[i].id1, pairs[i].id2, pairs[i].contact_point, pairs[i].normal, pairs[i].penetration_depth);
}
}
@@ -367,17 +368,17 @@ static inline int BVHContactCollection(const std::vector<BVHCollisionPair>& pair
// for OBB-alike contact
template<typename T_BVH>
-static inline int BVHContactCollection2(const std::vector<BVHCollisionPair>& pairs, const SimpleTransform& tf1, const BVHModel<T_BVH>* obj1, const BVHModel<T_BVH>* obj2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+static inline int BVHContactCollection2(const std::vector<BVHCollisionPair>& pairs, const SimpleTransform& tf1, 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((!exhaustive) && (num_contacts > num_max_contacts)) num_contacts = num_max_contacts;
- contacts.resize(num_contacts);
- if(!enable_contact)
+ if((!request.exhaustive) && (num_contacts > request.num_max_contacts)) num_contacts = request.num_max_contacts;
+ result.contacts.resize(num_contacts);
+ if(!request.enable_contact)
{
for(int i = 0; i < num_contacts; ++i)
- contacts[i] = Contact(obj1, obj2, pairs[i].id1, pairs[i].id2);
+ result.contacts[i] = Contact(obj1, obj2, pairs[i].id1, pairs[i].id2);
}
else
{
@@ -385,7 +386,7 @@ static inline int BVHContactCollection2(const std::vector<BVHCollisionPair>& pai
{
Vec3f normal = tf1.getRotation() * pairs[i].normal;
Vec3f contact_point = tf1.transform(pairs[i].contact_point);
- contacts[i] = Contact(obj1, obj2, pairs[i].id1, pairs[i].id2, contact_point, normal, pairs[i].penetration_depth);
+ result.contacts[i] = Contact(obj1, obj2, pairs[i].id1, pairs[i].id2, contact_point, normal, pairs[i].penetration_depth);
}
}
}
@@ -396,7 +397,7 @@ static inline int BVHContactCollection2(const std::vector<BVHCollisionPair>& pai
template<typename T_BVH>
-int BVHCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int BVHCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const CollisionRequest& request, CollisionResult& result)
{
MeshCollisionTraversalNode<T_BVH> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
@@ -406,9 +407,9 @@ int BVHCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const 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, num_max_contacts, exhaustive, enable_contact);
+ initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request);
collide(&node);
- int num_contacts = BVHContactCollection(node.pairs, obj1, obj2, num_max_contacts, exhaustive, enable_contact, contacts);
+ int num_contacts = BVHContactCollection(node.pairs, obj1, obj2, request, result);
delete obj1_tmp;
delete obj2_tmp;
@@ -416,52 +417,52 @@ int BVHCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const Co
}
template<>
-int BVHCollide<OBB>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int BVHCollide<OBB>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const CollisionRequest& request, CollisionResult& result)
{
MeshCollisionTraversalNodeOBB node;
const BVHModel<OBB>* obj1 = static_cast<const BVHModel<OBB>* >(o1);
const BVHModel<OBB>* obj2 = static_cast<const BVHModel<OBB>* >(o2);
- initialize(node, *obj1, tf1, *obj2, tf2, num_max_contacts, exhaustive, enable_contact);
+ initialize(node, *obj1, tf1, *obj2, tf2, request);
collide(&node);
- return BVHContactCollection2(node.pairs, tf1, obj1, obj2, num_max_contacts, exhaustive, enable_contact, contacts);
+ return BVHContactCollection2(node.pairs, tf1, obj1, obj2, request, result);
}
template<>
-int BVHCollide<OBBRSS>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int BVHCollide<OBBRSS>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const CollisionRequest& request, CollisionResult& result)
{
MeshCollisionTraversalNodeOBBRSS node;
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, num_max_contacts, exhaustive, enable_contact);
+ initialize(node, *obj1, tf1, *obj2, tf2, request);
collide(&node);
- return BVHContactCollection2(node.pairs, tf1, obj1, obj2, num_max_contacts, exhaustive, enable_contact, contacts);
+ return BVHContactCollection2(node.pairs, tf1, obj1, obj2, request, result);
}
template<>
-int BVHCollide<kIOS>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+int BVHCollide<kIOS>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const CollisionRequest& request, CollisionResult& result)
{
MeshCollisionTraversalNodekIOS node;
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, num_max_contacts, exhaustive, enable_contact);
+ initialize(node, *obj1, tf1, *obj2, tf2, request);
collide(&node);
- return BVHContactCollection2(node.pairs, tf1, obj1, obj2, num_max_contacts, exhaustive, enable_contact, contacts);
+ return BVHContactCollection2(node.pairs, tf1, obj1, obj2, request, result);
}
template<typename T_BVH, typename NarrowPhaseSolver>
int BVHCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
- int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+ const CollisionRequest& request, CollisionResult& result)
{
- return BVHCollide<T_BVH>(o1, tf1, o2, tf2, num_max_contacts, exhaustive, enable_contact, contacts);
+ return BVHCollide<T_BVH>(o1, tf1, o2, tf2, request, result);
}
diff --git a/trunk/fcl/src/conservative_advancement.cpp b/trunk/fcl/src/conservative_advancement.cpp
index 31dc75f1a8cd1b159c8749cef57712fd55755a80..0fc159ee818e27f61fbf42bfec6fe50a6e313214 100644
--- a/trunk/fcl/src/conservative_advancement.cpp
+++ b/trunk/fcl/src/conservative_advancement.cpp
@@ -51,13 +51,13 @@ int conservativeAdvancement(const CollisionGeometry* o1,
MotionBase<BV>* motion1,
const CollisionGeometry* o2,
MotionBase<BV>* motion2,
- int num_max_contacts, bool exhaustive, bool enable_contact,
- std::vector<Contact>& contacts,
+ const CollisionRequest& request,
+ CollisionResult& result,
FCL_REAL& toc)
{
- if(num_max_contacts <= 0 && !exhaustive)
+ if(request.num_max_contacts <= 0 && !request.exhaustive)
{
- std::cerr << "Warning: should stop early as num_max_contact is " << num_max_contacts << " !" << std::endl;
+ std::cerr << "Warning: should stop early as num_max_contact is " << request.num_max_contacts << " !" << std::endl;
return 0;
}
@@ -83,15 +83,13 @@ int conservativeAdvancement(const CollisionGeometry* o1,
// whether the first start configuration is in collision
MeshCollisionTraversalNodeRSS cnode;
- if(!initialize(cnode, *model1, tf1, *model2, tf2))
+ if(!initialize(cnode, *model1, tf1, *model2, tf2, request))
std::cout << "initialize error" << std::endl;
relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), cnode.R, cnode.T);
cnode.enable_statistics = false;
- cnode.num_max_contacts = num_max_contacts;
- cnode.exhaustive = exhaustive;
- cnode.enable_contact = enable_contact;
+ cnode.request = request;
collide(&cnode);
@@ -160,8 +158,8 @@ template int conservativeAdvancement<RSS>(const CollisionGeometry* o1,
MotionBase<RSS>* motion1,
const CollisionGeometry* o2,
MotionBase<RSS>* motion2,
- int num_max_contacts, bool exhaustive, bool enable_contact,
- std::vector<Contact>& contacts,
+ const CollisionRequest& request,
+ CollisionResult& result,
FCL_REAL& toc);
diff --git a/trunk/fcl/src/distance.cpp b/trunk/fcl/src/distance.cpp
index 2a4f650203e92e97d345740e2c3bd7e1b12570de..4ff8070954f820f26f72df1e7fe4f1c972decde1 100644
--- a/trunk/fcl/src/distance.cpp
+++ b/trunk/fcl/src/distance.cpp
@@ -51,15 +51,18 @@ DistanceFunctionMatrix<GJKSolver>& getDistanceFunctionLookTable()
}
template<typename NarrowPhaseSolver>
-FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const NarrowPhaseSolver* nsolver)
+FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const NarrowPhaseSolver* nsolver,
+ const DistanceRequest& request, DistanceResult& result)
{
- return distance<NarrowPhaseSolver>(o1->getCollisionGeometry(), o1->getTransform(), o2->getCollisionGeometry(), o2->getTransform(), nsolver);
+ return distance<NarrowPhaseSolver>(o1->getCollisionGeometry(), o1->getTransform(), o2->getCollisionGeometry(), o2->getTransform(), nsolver,
+ request, result);
}
template<typename NarrowPhaseSolver>
FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
const CollisionGeometry* o2, const SimpleTransform& tf2,
- const NarrowPhaseSolver* nsolver_)
+ const NarrowPhaseSolver* nsolver_,
+ const DistanceRequest& request, DistanceResult& result)
{
const NarrowPhaseSolver* nsolver = nsolver_;
if(!nsolver_)
@@ -72,18 +75,17 @@ FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
OBJECT_TYPE object_type2 = o2->getObjectType();
NODE_TYPE node_type2 = o2->getNodeType();
- FCL_REAL res;
+ FCL_REAL res = std::numeric_limits<FCL_REAL>::max();
if(object_type1 == OT_GEOM && object_type2 == OT_BVH)
{
if(!looktable.distance_matrix[node_type2][node_type1])
{
std::cerr << "Warning: distance function between node type " << node_type1 << " and node type " << node_type2 << " is not supported" << std::endl;
- res = 0;
}
else
{
- res = looktable.distance_matrix[node_type2][node_type1](o2, tf2, o1, tf1, nsolver);
+ res = looktable.distance_matrix[node_type2][node_type1](o2, tf2, o1, tf1, nsolver, request, result);
}
}
else
@@ -91,11 +93,10 @@ FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
if(!looktable.distance_matrix[node_type1][node_type2])
{
std::cerr << "Warning: distance function between node type " << node_type1 << " and node type " << node_type2 << " is not supported" << std::endl;
- res = 0;
}
else
{
- res = looktable.distance_matrix[node_type1][node_type2](o1, tf1, o2, tf2, nsolver);
+ res = looktable.distance_matrix[node_type1][node_type2](o1, tf1, o2, tf2, nsolver, request, result);
}
}
@@ -105,24 +106,25 @@ FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
return res;
}
-template FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const GJKSolver_libccd* nsolver);
-template FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const GJKSolver_indep* nsolver);
-template FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const GJKSolver_libccd* nsolver);
-template FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const GJKSolver_indep* nsolver);
+template FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const GJKSolver_libccd* nsolver, const DistanceRequest& request, DistanceResult& result);
+template FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const GJKSolver_indep* nsolver, const DistanceRequest& request, DistanceResult& result);
+template FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const GJKSolver_libccd* nsolver, const DistanceRequest& request, DistanceResult& result);
+template FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const GJKSolver_indep* nsolver, const DistanceRequest& request, DistanceResult& result);
-FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2)
+FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const DistanceRequest& request, DistanceResult& result)
{
GJKSolver_libccd solver;
- return distance<GJKSolver_libccd>(o1, o2, &solver);
+ return distance<GJKSolver_libccd>(o1, o2, &solver, request, result);
}
FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
- const CollisionGeometry* o2, const SimpleTransform& tf2)
+ const CollisionGeometry* o2, const SimpleTransform& tf2,
+ const DistanceRequest& request, DistanceResult& result)
{
GJKSolver_libccd solver;
- return distance<GJKSolver_libccd>(o1, tf1, o2, tf2, &solver);
+ return distance<GJKSolver_libccd>(o1, tf1, o2, tf2, &solver, request, result);
// GJKSolver_indep solver;
- // return distance<GJKSolver_indep>(o1, tf1, o2, tf2, &solver);
+ // return distance<GJKSolver_indep>(o1, tf1, o2, tf2, &solver, request, result);
}
diff --git a/trunk/fcl/src/distance_func_matrix.cpp b/trunk/fcl/src/distance_func_matrix.cpp
index 01a6d493b996194d6c49ca7d497404610a5d2462..4a4429fc5280d125c278cc2e78d565efb74e3078 100644
--- a/trunk/fcl/src/distance_func_matrix.cpp
+++ b/trunk/fcl/src/distance_func_matrix.cpp
@@ -44,88 +44,101 @@ namespace fcl
{
template<typename T_SH, typename NarrowPhaseSolver>
-FCL_REAL ShapeOcTreeDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver)
+FCL_REAL ShapeOcTreeDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver,
+ const DistanceRequest& request, DistanceResult& result)
{
ShapeOcTreeDistanceTraversalNode<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);
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver);
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request);
distance(&node);
+ result.min_distance = node.min_distance;
return node.min_distance;
}
template<typename T_SH, typename NarrowPhaseSolver>
-FCL_REAL OcTreeShapeDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver)
+FCL_REAL OcTreeShapeDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver,
+ const DistanceRequest& request, DistanceResult& result)
{
OcTreeShapeDistanceTraversalNode<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);
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver);
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request);
distance(&node);
+ result.min_distance = node.min_distance;
return node.min_distance;
}
template<typename NarrowPhaseSolver>
-FCL_REAL OcTreeDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver)
+FCL_REAL OcTreeDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver,
+ const DistanceRequest& request, DistanceResult& result)
{
OcTreeDistanceTraversalNode<NarrowPhaseSolver> node;
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);
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request);
distance(&node);
+ result.min_distance = node.min_distance;
return node.min_distance;
}
template<typename T_BVH, typename NarrowPhaseSolver>
-FCL_REAL BVHOcTreeDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver)
+FCL_REAL BVHOcTreeDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver,
+ const DistanceRequest& request, DistanceResult& result)
{
MeshOcTreeDistanceTraversalNode<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);
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver);
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request);
distance(&node);
+ result.min_distance = node.min_distance;
return node.min_distance;
}
template<typename T_BVH, typename NarrowPhaseSolver>
-FCL_REAL OcTreeBVHDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver)
+FCL_REAL OcTreeBVHDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver,
+ const DistanceRequest& request, DistanceResult& result)
{
OcTreeMeshDistanceTraversalNode<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);
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver);
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request);
distance(&node);
+ result.min_distance = node.min_distance;
return node.min_distance;
}
template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>
-FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver)
+FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver,
+ const DistanceRequest& request, DistanceResult& result)
{
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);
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request);
distance(&node);
+ result.min_distance = node.min_distance;
return node.min_distance;
}
template<typename T_BVH, typename T_SH, typename NarrowPhaseSolver>
struct BVHShapeDistancer
{
- static FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver)
+ static FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver,
+ const DistanceRequest& request, DistanceResult& result)
{
MeshShapeDistanceTraversalNode<T_BVH, T_SH, NarrowPhaseSolver> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
@@ -133,10 +146,11 @@ 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);
+ initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request);
fcl::distance(&node);
delete obj1_tmp;
+ result.min_distance = node.min_distance;
return node.min_distance;
}
};
@@ -144,15 +158,17 @@ struct BVHShapeDistancer
template<typename T_SH, typename NarrowPhaseSolver>
struct BVHShapeDistancer<RSS, T_SH, NarrowPhaseSolver>
{
- static FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver)
+ static FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver,
+ const DistanceRequest& request, DistanceResult& result)
{
MeshShapeDistanceTraversalNodeRSS<T_SH, NarrowPhaseSolver> node;
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);
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request);
fcl::distance(&node);
+ result.min_distance = node.min_distance;
return node.min_distance;
}
};
@@ -161,15 +177,17 @@ struct BVHShapeDistancer<RSS, T_SH, NarrowPhaseSolver>
template<typename T_SH, typename NarrowPhaseSolver>
struct BVHShapeDistancer<kIOS, T_SH, NarrowPhaseSolver>
{
- static FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver)
+ static FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver,
+ const DistanceRequest& request, DistanceResult& result)
{
MeshShapeDistanceTraversalNodekIOS<T_SH, NarrowPhaseSolver> node;
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);
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request);
fcl::distance(&node);
+ result.min_distance = node.min_distance;
return node.min_distance;
}
};
@@ -177,22 +195,25 @@ struct BVHShapeDistancer<kIOS, T_SH, NarrowPhaseSolver>
template<typename T_SH, typename NarrowPhaseSolver>
struct BVHShapeDistancer<OBBRSS, T_SH, NarrowPhaseSolver>
{
- static FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver)
+ static FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver,
+ const DistanceRequest& request, DistanceResult& result)
{
MeshShapeDistanceTraversalNodeOBBRSS<T_SH, NarrowPhaseSolver> node;
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);
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request);
fcl::distance(&node);
+ result.min_distance = node.min_distance;
return node.min_distance;
}
};
template<typename T_BVH>
-FCL_REAL BVHDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2)
+FCL_REAL BVHDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
+ const DistanceRequest& request, DistanceResult& result)
{
MeshDistanceTraversalNode<T_BVH> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
@@ -202,58 +223,66 @@ 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);
+ initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request);
distance(&node);
+ result.min_distance = node.min_distance;
return node.min_distance;
}
template<>
-FCL_REAL BVHDistance<RSS>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2)
+FCL_REAL BVHDistance<RSS>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
+ const DistanceRequest& request, DistanceResult& result)
{
MeshDistanceTraversalNodeRSS node;
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);
+ initialize(node, *obj1, tf1, *obj2, tf2, request);
distance(&node);
+ result.min_distance = node.min_distance;
return node.min_distance;
}
template<>
-FCL_REAL BVHDistance<kIOS>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2)
+FCL_REAL BVHDistance<kIOS>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
+ const DistanceRequest& request, DistanceResult& result)
{
MeshDistanceTraversalNodekIOS node;
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);
+ initialize(node, *obj1, tf1, *obj2, tf2, request);
distance(&node);
+ result.min_distance = node.min_distance;
return node.min_distance;
}
template<>
-FCL_REAL BVHDistance<OBBRSS>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2)
+FCL_REAL BVHDistance<OBBRSS>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
+ const DistanceRequest& request, DistanceResult& result)
{
MeshDistanceTraversalNodeOBBRSS node;
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);
+ initialize(node, *obj1, tf1, *obj2, tf2, request);
distance(&node);
+ result.min_distance = node.min_distance;
return node.min_distance;
}
template<typename T_BVH, typename NarrowPhaseSolver>
FCL_REAL BVHDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
- const NarrowPhaseSolver* nsolver)
+ const NarrowPhaseSolver* nsolver,
+ const DistanceRequest& request, DistanceResult& result)
{
- return BVHDistance<T_BVH>(o1, tf1, o2, tf2);
+ return BVHDistance<T_BVH>(o1, tf1, o2, tf2, request, result);
}
template<typename NarrowPhaseSolver>
diff --git a/trunk/fcl/src/simple_setup.cpp b/trunk/fcl/src/simple_setup.cpp
index 613959fb86dd6fd50cff55634925b64c0444c31c..c14c6880c74cc2d33f32c636cc2f972a457bbdb9 100644
--- a/trunk/fcl/src/simple_setup.cpp
+++ b/trunk/fcl/src/simple_setup.cpp
@@ -47,7 +47,7 @@ template<typename BV, typename OrientedNode>
static inline bool setupMeshCollisionOrientedNode(OrientedNode& node,
const BVHModel<BV>& model1, const SimpleTransform& tf1,
const BVHModel<BV>& model2, const SimpleTransform& tf2,
- int num_max_contacts, bool exhaustive, bool enable_contact)
+ const CollisionRequest& request)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
return false;
@@ -63,9 +63,7 @@ static inline bool setupMeshCollisionOrientedNode(OrientedNode& node,
node.model2 = &model2;
node.tf2 = tf2;
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
+ node.request = request;
relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T);
@@ -78,35 +76,35 @@ static inline bool setupMeshCollisionOrientedNode(OrientedNode& node,
bool initialize(MeshCollisionTraversalNodeOBB& node,
const BVHModel<OBB>& model1, const SimpleTransform& tf1,
const BVHModel<OBB>& model2, const SimpleTransform& tf2,
- int num_max_contacts, bool exhaustive, bool enable_contact)
+ const CollisionRequest& request)
{
- return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, num_max_contacts, exhaustive, enable_contact);
+ return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, request);
}
bool initialize(MeshCollisionTraversalNodeRSS& node,
const BVHModel<RSS>& model1, const SimpleTransform& tf1,
const BVHModel<RSS>& model2, const SimpleTransform& tf2,
- int num_max_contacts, bool exhaustive, bool enable_contact)
+ const CollisionRequest& request)
{
- return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, num_max_contacts, exhaustive, enable_contact);
+ return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, request);
}
bool initialize(MeshCollisionTraversalNodekIOS& node,
const BVHModel<kIOS>& model1, const SimpleTransform& tf1,
const BVHModel<kIOS>& model2, const SimpleTransform& tf2,
- int num_max_contacts, bool exhaustive, bool enable_contact)
+ const CollisionRequest& request)
{
- return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, num_max_contacts, exhaustive, enable_contact);
+ return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, request);
}
bool initialize(MeshCollisionTraversalNodeOBBRSS& node,
const BVHModel<OBBRSS>& model1, const SimpleTransform& tf1,
const BVHModel<OBBRSS>& model2, const SimpleTransform& tf2,
- int num_max_contacts, bool exhaustive, bool enable_contact)
+ const CollisionRequest& request)
{
- return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, num_max_contacts, exhaustive, enable_contact);
+ return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, request);
}
@@ -118,11 +116,9 @@ template<typename BV, typename OrientedNode>
static inline bool setupPointCloudCollisionOrientedNode(OrientedNode& node,
BVHModel<BV>& model1, const SimpleTransform& tf1,
BVHModel<BV>& model2, const SimpleTransform& tf2,
+ const CollisionRequest& request,
FCL_REAL collision_prob_threshold,
int leaf_size_threshold,
- int num_max_contacts,
- bool exhaustive,
- bool enable_contact,
FCL_REAL expand_r)
{
if(!(model1.getModelType() == BVH_MODEL_TRIANGLES || model1.getModelType() == BVH_MODEL_POINTCLOUD)
@@ -146,9 +142,7 @@ static inline bool setupPointCloudCollisionOrientedNode(OrientedNode& node,
BVHExpand(model1, node.uc1.get(), expand_r);
BVHExpand(model2, node.uc2.get(), expand_r);
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
+ node.request = request;
node.collision_prob_threshold = collision_prob_threshold;
node.leaf_size_threshold = leaf_size_threshold;
@@ -162,28 +156,24 @@ static inline bool setupPointCloudCollisionOrientedNode(OrientedNode& node,
bool initialize(PointCloudCollisionTraversalNodeOBB& node,
BVHModel<OBB>& model1, const SimpleTransform& tf1,
BVHModel<OBB>& model2, const SimpleTransform& tf2,
+ const CollisionRequest& request,
FCL_REAL collision_prob_threshold,
int leaf_size_threshold,
- int num_max_contacts,
- bool exhaustive,
- bool enable_contact,
FCL_REAL expand_r)
{
- return details::setupPointCloudCollisionOrientedNode(node, model1, tf1, model2, tf2, collision_prob_threshold, leaf_size_threshold, num_max_contacts, exhaustive, enable_contact, expand_r);
+ return details::setupPointCloudCollisionOrientedNode(node, model1, tf1, model2, tf2, request, collision_prob_threshold, leaf_size_threshold, expand_r);
}
bool initialize(PointCloudCollisionTraversalNodeRSS& node,
BVHModel<RSS>& model1, const SimpleTransform& tf1,
BVHModel<RSS>& model2, const SimpleTransform& tf2,
+ const CollisionRequest& request,
FCL_REAL collision_prob_threshold,
int leaf_size_threshold,
- int num_max_contacts,
- bool exhaustive,
- bool enable_contact,
FCL_REAL expand_r)
{
- return details::setupPointCloudCollisionOrientedNode(node, model1, tf1, model2, tf2, collision_prob_threshold, leaf_size_threshold, num_max_contacts, exhaustive, enable_contact, expand_r);
+ return details::setupPointCloudCollisionOrientedNode(node, model1, tf1, model2, tf2, request, collision_prob_threshold, leaf_size_threshold, expand_r);
}
namespace details
@@ -193,11 +183,9 @@ template<typename BV, typename OrientedNode>
static inline bool setupPointCloudMeshCollisionOrientedNode(OrientedNode& node,
BVHModel<BV>& model1, const SimpleTransform& tf1,
const BVHModel<BV>& model2, const SimpleTransform& tf2,
+ const CollisionRequest& request,
FCL_REAL collision_prob_threshold,
int leaf_size_threshold,
- int num_max_contacts,
- bool exhaustive,
- bool enable_contact,
FCL_REAL expand_r)
{
if(!(model1.getModelType() == BVH_MODEL_TRIANGLES || model1.getModelType() == BVH_MODEL_POINTCLOUD) || model2.getModelType() != BVH_MODEL_TRIANGLES)
@@ -218,9 +206,7 @@ static inline bool setupPointCloudMeshCollisionOrientedNode(OrientedNode& node,
BVHExpand(model1, node.uc1.get(), expand_r);
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
+ node.request = request;
node.collision_prob_threshold = collision_prob_threshold;
node.leaf_size_threshold = leaf_size_threshold;
@@ -233,28 +219,24 @@ static inline bool setupPointCloudMeshCollisionOrientedNode(OrientedNode& node,
bool initialize(PointCloudMeshCollisionTraversalNodeOBB& node,
BVHModel<OBB>& model1, const SimpleTransform& tf1,
const BVHModel<OBB>& model2, const SimpleTransform& tf2,
+ const CollisionRequest& request,
FCL_REAL collision_prob_threshold,
int leaf_size_threshold,
- int num_max_contacts,
- bool exhaustive,
- bool enable_contact,
FCL_REAL expand_r)
{
- return details::setupPointCloudMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, collision_prob_threshold, leaf_size_threshold, num_max_contacts, exhaustive, enable_contact, expand_r);
+ return details::setupPointCloudMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, request, collision_prob_threshold, leaf_size_threshold, expand_r);
}
bool initialize(PointCloudMeshCollisionTraversalNodeRSS& node,
BVHModel<RSS>& model1, const SimpleTransform& tf1,
const BVHModel<RSS>& model2, const SimpleTransform& tf2,
+ const CollisionRequest& request,
FCL_REAL collision_prob_threshold,
int leaf_size_threshold,
- int num_max_contacts,
- bool exhaustive,
- bool enable_contact,
FCL_REAL expand_r)
{
- return details::setupPointCloudMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, collision_prob_threshold, leaf_size_threshold, num_max_contacts, exhaustive, enable_contact, expand_r);
+ return details::setupPointCloudMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, request, collision_prob_threshold, leaf_size_threshold, expand_r);
}
#endif
@@ -265,11 +247,14 @@ namespace details
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 BVHModel<BV>& model2, const SimpleTransform& tf2,
+ const DistanceRequest& request)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
return false;
+ node.request = request;
+
node.model1 = &model1;
node.tf1 = tf1;
node.model2 = &model2;
@@ -291,24 +276,27 @@ 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 BVHModel<RSS>& model2, const SimpleTransform& tf2,
+ const DistanceRequest& request)
{
- return details::setupMeshDistanceOrientedNode(node, model1, tf1, model2, tf2);
+ return details::setupMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, request);
}
bool initialize(MeshDistanceTraversalNodekIOS& node,
const BVHModel<kIOS>& model1, const SimpleTransform& tf1,
- const BVHModel<kIOS>& model2, const SimpleTransform& tf2)
+ const BVHModel<kIOS>& model2, const SimpleTransform& tf2,
+ const DistanceRequest& request)
{
- return details::setupMeshDistanceOrientedNode(node, model1, tf1, model2, tf2);
+ return details::setupMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, request);
}
bool initialize(MeshDistanceTraversalNodeOBBRSS& node,
const BVHModel<OBBRSS>& model1, const SimpleTransform& tf1,
- const BVHModel<OBBRSS>& model2, const SimpleTransform& tf2)
+ const BVHModel<OBBRSS>& model2, const SimpleTransform& tf2,
+ const DistanceRequest& request)
{
- return details::setupMeshDistanceOrientedNode(node, model1, tf1, model2, tf2);
+ return details::setupMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, request);
}
diff --git a/trunk/fcl/src/traversal_node_bvhs.cpp b/trunk/fcl/src/traversal_node_bvhs.cpp
index f4d4af3af78eeeccece71173c72b6b7edfbcceca..7f3b409417ebaf616bbf4eb4592b802128c44882 100644
--- a/trunk/fcl/src/traversal_node_bvhs.cpp
+++ b/trunk/fcl/src/traversal_node_bvhs.cpp
@@ -49,9 +49,7 @@ static inline void meshCollisionOrientedNodeLeafTesting(int b1, int b2,
Triangle* tri_indices1, Triangle* tri_indices2,
const Matrix3f& R, const Vec3f& T,
bool enable_statistics,
- bool enable_contact,
- bool exhaustive,
- int num_max_contacts,
+ const CollisionRequest& request,
int& num_leaf_tests,
std::vector<BVHCollisionPair>& pairs)
{
@@ -79,7 +77,7 @@ static inline void meshCollisionOrientedNodeLeafTesting(int b1, int b2,
Vec3f contacts[2];
- if(!enable_contact) // only interested in collision or not
+ if(!request.enable_contact) // only interested in collision or not
{
if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, R, T))
pairs.push_back(BVHCollisionPair(primitive_id1, primitive_id2));
@@ -95,7 +93,7 @@ static inline void meshCollisionOrientedNodeLeafTesting(int b1, int b2,
{
for(int i = 0; i < n_contacts; ++i)
{
- if((!exhaustive) && (num_max_contacts <= (int)pairs.size())) break;
+ if((!request.exhaustive) && (request.num_max_contacts <= (int)pairs.size())) break;
pairs.push_back(BVHCollisionPair(primitive_id1, primitive_id2, contacts[i], normal, penetration));
}
}
@@ -176,8 +174,7 @@ void MeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2) const
details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
tri_indices1, tri_indices2,
R, T,
- enable_statistics, enable_contact, exhaustive,
- num_max_contacts,
+ enable_statistics, request,
num_leaf_tests,
pairs);
}
@@ -194,8 +191,7 @@ void MeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2, const Matrix3f&
details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
tri_indices1, tri_indices2,
R, T,
- enable_statistics, enable_contact, exhaustive,
- num_max_contacts,
+ enable_statistics, request,
num_leaf_tests,
pairs);
}
@@ -219,8 +215,7 @@ void MeshCollisionTraversalNodeRSS::leafTesting(int b1, int b2) const
details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
tri_indices1, tri_indices2,
R, T,
- enable_statistics, enable_contact, exhaustive,
- num_max_contacts,
+ enable_statistics, request,
num_leaf_tests,
pairs);
}
@@ -245,8 +240,7 @@ void MeshCollisionTraversalNodekIOS::leafTesting(int b1, int b2) const
details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
tri_indices1, tri_indices2,
R, T,
- enable_statistics, enable_contact, exhaustive,
- num_max_contacts,
+ enable_statistics, request,
num_leaf_tests,
pairs);
}
@@ -270,8 +264,7 @@ void MeshCollisionTraversalNodeOBBRSS::leafTesting(int b1, int b2) const
details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
tri_indices1, tri_indices2,
R, T,
- enable_statistics, enable_contact, exhaustive,
- num_max_contacts,
+ enable_statistics, request,
num_leaf_tests,
pairs);
}