From 68756374120df479f4e322b080a5e8775134f845 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Sun, 2 Dec 2018 07:13:31 +0100
Subject: [PATCH] Give CollisionRequest as input to CollisionTraversalNodeBase
constructor
- store const CollisionRequest& as a member instead of CollisionRequest,
- remove member CollisionRequest::enable_distance_lower_bound,
- modify constructors of all derived classes.
---
.../hpp/fcl/traversal/traversal_node_base.h | 10 +---
.../fcl/traversal/traversal_node_bvh_shape.h | 27 ++++-----
.../hpp/fcl/traversal/traversal_node_bvhs.h | 16 +++---
.../hpp/fcl/traversal/traversal_node_octree.h | 15 +++--
.../hpp/fcl/traversal/traversal_node_setup.h | 55 ++++---------------
.../hpp/fcl/traversal/traversal_node_shapes.h | 3 +-
src/collision_func_matrix.cpp | 40 +++++++-------
src/traversal/traversal_node_bvhs.cpp | 16 +++---
src/traversal/traversal_node_setup.cpp | 14 ++---
src/traversal/traversal_recurse.cpp | 6 +-
test/benchmark.cpp | 4 +-
test/test_fcl_collision.cpp | 37 ++++++-------
test/test_fcl_distance.cpp | 16 +++---
test/test_fcl_frontlist.cpp | 46 +++++++---------
14 files changed, 131 insertions(+), 174 deletions(-)
diff --git a/include/hpp/fcl/traversal/traversal_node_base.h b/include/hpp/fcl/traversal/traversal_node_base.h
index fed49bb3..f1651dad 100644
--- a/include/hpp/fcl/traversal/traversal_node_base.h
+++ b/include/hpp/fcl/traversal/traversal_node_base.h
@@ -90,9 +90,8 @@ public:
class CollisionTraversalNodeBase : public TraversalNodeBase
{
public:
- CollisionTraversalNodeBase(bool enable_distance_lower_bound_ = false) :
- result(NULL), enable_statistics(false),
- enable_distance_lower_bound (enable_distance_lower_bound_){}
+ CollisionTraversalNodeBase (const CollisionRequest& request_) :
+ request (request_), result(NULL), enable_statistics(false) {}
virtual ~CollisionTraversalNodeBase();
@@ -118,16 +117,13 @@ public:
void enableStatistics(bool enable) { enable_statistics = enable; }
/// @brief request setting for collision
- CollisionRequest request;
+ const CollisionRequest& request;
/// @brief collision result kept during the traversal iteration
CollisionResult* result;
/// @brief Whether stores statistics
bool enable_statistics;
-
- /// Whether to compute a lower bound on distance between bounding volumes
- bool enable_distance_lower_bound;
};
/// @brief Node structure encoding the information required for distance traversal.
diff --git a/include/hpp/fcl/traversal/traversal_node_bvh_shape.h b/include/hpp/fcl/traversal/traversal_node_bvh_shape.h
index 0e3171f3..73e6a9d2 100644
--- a/include/hpp/fcl/traversal/traversal_node_bvh_shape.h
+++ b/include/hpp/fcl/traversal/traversal_node_bvh_shape.h
@@ -54,8 +54,8 @@ template<typename BV, typename S>
class BVHShapeCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
- BVHShapeCollisionTraversalNode(bool enable_distance_lower_bound) :
- CollisionTraversalNodeBase (enable_distance_lower_bound)
+ BVHShapeCollisionTraversalNode(const CollisionRequest& request) :
+ CollisionTraversalNodeBase (request)
{
model1 = NULL;
model2 = NULL;
@@ -115,7 +115,8 @@ template<typename S, typename BV>
class ShapeBVHCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
- ShapeBVHCollisionTraversalNode() : CollisionTraversalNodeBase()
+ ShapeBVHCollisionTraversalNode(const CollisionRequest& request) :
+ CollisionTraversalNodeBase(request)
{
model1 = NULL;
model2 = NULL;
@@ -182,8 +183,8 @@ template<typename BV, typename S, typename NarrowPhaseSolver>
class MeshShapeCollisionTraversalNode : public BVHShapeCollisionTraversalNode<BV, S>
{
public:
- MeshShapeCollisionTraversalNode(bool enable_distance_lower_bound = false) :
- BVHShapeCollisionTraversalNode<BV, S> (enable_distance_lower_bound)
+ MeshShapeCollisionTraversalNode(const CollisionRequest& request) :
+ BVHShapeCollisionTraversalNode<BV, S> (request)
{
vertices = NULL;
tri_indices = NULL;
@@ -326,9 +327,9 @@ template<typename S, typename NarrowPhaseSolver>
class MeshShapeCollisionTraversalNodeOBB : public MeshShapeCollisionTraversalNode<OBB, S, NarrowPhaseSolver>
{
public:
- MeshShapeCollisionTraversalNodeOBB(bool enable_distance_lower_bound = false) :
+ MeshShapeCollisionTraversalNodeOBB(const CollisionRequest& request) :
MeshShapeCollisionTraversalNode<OBB, S, NarrowPhaseSolver>
- (enable_distance_lower_bound)
+ (request)
{
}
@@ -353,9 +354,9 @@ template<typename S, typename NarrowPhaseSolver>
class MeshShapeCollisionTraversalNodeRSS : public MeshShapeCollisionTraversalNode<RSS, S, NarrowPhaseSolver>
{
public:
- MeshShapeCollisionTraversalNodeRSS (bool enable_distance_lower_bound = false):
+ MeshShapeCollisionTraversalNodeRSS (const CollisionRequest& request):
MeshShapeCollisionTraversalNode<RSS, S, NarrowPhaseSolver>
- (enable_distance_lower_bound)
+ (request)
{
}
@@ -380,9 +381,9 @@ template<typename S, typename NarrowPhaseSolver>
class MeshShapeCollisionTraversalNodekIOS : public MeshShapeCollisionTraversalNode<kIOS, S, NarrowPhaseSolver>
{
public:
- MeshShapeCollisionTraversalNodekIOS(bool enable_distance_lower_bound = false):
+ MeshShapeCollisionTraversalNodekIOS(const CollisionRequest& request):
MeshShapeCollisionTraversalNode<kIOS, S, NarrowPhaseSolver>
- (enable_distance_lower_bound)
+ (request)
{
}
@@ -408,9 +409,9 @@ class MeshShapeCollisionTraversalNodeOBBRSS : public MeshShapeCollisionTraversal
{
public:
MeshShapeCollisionTraversalNodeOBBRSS
- (bool enable_distance_lower_bound = false) :
+ (const CollisionRequest& request) :
MeshShapeCollisionTraversalNode
- <OBBRSS, S, NarrowPhaseSolver>(enable_distance_lower_bound)
+ <OBBRSS, S, NarrowPhaseSolver>(request)
{
}
diff --git a/include/hpp/fcl/traversal/traversal_node_bvhs.h b/include/hpp/fcl/traversal/traversal_node_bvhs.h
index b401de4d..2aff7688 100644
--- a/include/hpp/fcl/traversal/traversal_node_bvhs.h
+++ b/include/hpp/fcl/traversal/traversal_node_bvhs.h
@@ -61,8 +61,8 @@ template<typename BV>
class BVHCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
- BVHCollisionTraversalNode(bool enable_distance_lower_bound) :
- CollisionTraversalNodeBase (enable_distance_lower_bound)
+ BVHCollisionTraversalNode(const CollisionRequest& request) :
+ CollisionTraversalNodeBase (request)
{
model1 = NULL;
model2 = NULL;
@@ -157,8 +157,8 @@ template<typename BV>
class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV>
{
public:
- MeshCollisionTraversalNode(bool enable_distance_lower_bound) :
- BVHCollisionTraversalNode<BV> (enable_distance_lower_bound)
+ MeshCollisionTraversalNode(const CollisionRequest& request) :
+ BVHCollisionTraversalNode<BV> (request)
{
vertices1 = NULL;
vertices2 = NULL;
@@ -247,7 +247,7 @@ public:
class MeshCollisionTraversalNodeOBB : public MeshCollisionTraversalNode<OBB>
{
public:
- MeshCollisionTraversalNodeOBB (bool enable_distance_lower_bound);
+ MeshCollisionTraversalNodeOBB (const CollisionRequest& request);
bool BVTesting(int b1, int b2) const;
@@ -260,7 +260,7 @@ public:
class MeshCollisionTraversalNodeRSS : public MeshCollisionTraversalNode<RSS>
{
public:
- MeshCollisionTraversalNodeRSS (bool enable_distance_lower_bound);
+ MeshCollisionTraversalNodeRSS (const CollisionRequest& request);
bool BVTesting(int b1, int b2) const;
@@ -273,7 +273,7 @@ public:
class MeshCollisionTraversalNodekIOS : public MeshCollisionTraversalNode<kIOS>
{
public:
- MeshCollisionTraversalNodekIOS (bool enable_distance_lower_bound);
+ MeshCollisionTraversalNodekIOS (const CollisionRequest& request);
bool BVTesting(int b1, int b2) const;
@@ -286,7 +286,7 @@ public:
class MeshCollisionTraversalNodeOBBRSS : public MeshCollisionTraversalNode<OBBRSS>
{
public:
- MeshCollisionTraversalNodeOBBRSS (bool enable_distance_lower_bound);
+ MeshCollisionTraversalNodeOBBRSS (const CollisionRequest& request);
bool BVTesting(int b1, int b2) const;
diff --git a/include/hpp/fcl/traversal/traversal_node_octree.h b/include/hpp/fcl/traversal/traversal_node_octree.h
index a378d682..d16d302a 100644
--- a/include/hpp/fcl/traversal/traversal_node_octree.h
+++ b/include/hpp/fcl/traversal/traversal_node_octree.h
@@ -882,7 +882,8 @@ template<typename NarrowPhaseSolver>
class OcTreeCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
- OcTreeCollisionTraversalNode()
+ OcTreeCollisionTraversalNode(const CollisionRequest& request) :
+ CollisionTraversalNodeBase (request)
{
model1 = NULL;
model2 = NULL;
@@ -953,7 +954,8 @@ template<typename S, typename NarrowPhaseSolver>
class ShapeOcTreeCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
- ShapeOcTreeCollisionTraversalNode()
+ ShapeOcTreeCollisionTraversalNode(const CollisionRequest& request) :
+ CollisionTraversalNodeBase (request)
{
model1 = NULL;
model2 = NULL;
@@ -989,7 +991,8 @@ template<typename S, typename NarrowPhaseSolver>
class OcTreeShapeCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
- OcTreeShapeCollisionTraversalNode()
+ OcTreeShapeCollisionTraversalNode(const CollisionRequest& request) :
+ CollisionTraversalNodeBase (request)
{
model1 = NULL;
model2 = NULL;
@@ -1083,7 +1086,8 @@ template<typename BV, typename NarrowPhaseSolver>
class MeshOcTreeCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
- MeshOcTreeCollisionTraversalNode()
+ MeshOcTreeCollisionTraversalNode(const CollisionRequest& request) :
+ CollisionTraversalNodeBase (request)
{
model1 = NULL;
model2 = NULL;
@@ -1119,7 +1123,8 @@ template<typename BV, typename NarrowPhaseSolver>
class OcTreeMeshCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
- OcTreeMeshCollisionTraversalNode()
+ OcTreeMeshCollisionTraversalNode(const CollisionRequest& request) :
+ CollisionTraversalNodeBase (request)
{
model1 = NULL;
model2 = NULL;
diff --git a/include/hpp/fcl/traversal/traversal_node_setup.h b/include/hpp/fcl/traversal/traversal_node_setup.h
index 5b8a84e2..964782d6 100644
--- a/include/hpp/fcl/traversal/traversal_node_setup.h
+++ b/include/hpp/fcl/traversal/traversal_node_setup.h
@@ -59,10 +59,8 @@ bool initialize(OcTreeCollisionTraversalNode<NarrowPhaseSolver>& node,
const OcTree& model1, const Transform3f& tf1,
const OcTree& model2, const Transform3f& tf2,
const OcTreeSolver<NarrowPhaseSolver>* otsolver,
- const CollisionRequest& request,
CollisionResult& result)
{
- node.request = request;
node.result = &result;
node.model1 = &model1;
@@ -105,10 +103,8 @@ bool initialize(ShapeOcTreeCollisionTraversalNode<S, NarrowPhaseSolver>& node,
const S& model1, const Transform3f& tf1,
const OcTree& model2, const Transform3f& tf2,
const OcTreeSolver<NarrowPhaseSolver>* otsolver,
- const CollisionRequest& request,
CollisionResult& result)
{
- node.request = request;
node.result = &result;
node.model1 = &model1;
@@ -128,10 +124,8 @@ bool initialize(OcTreeShapeCollisionTraversalNode<S, NarrowPhaseSolver>& node,
const OcTree& model1, const Transform3f& tf1,
const S& model2, const Transform3f& tf2,
const OcTreeSolver<NarrowPhaseSolver>* otsolver,
- const CollisionRequest& request,
CollisionResult& result)
{
- node.request = request;
node.result = &result;
node.model1 = &model1;
@@ -197,10 +191,8 @@ bool initialize(MeshOcTreeCollisionTraversalNode<BV, NarrowPhaseSolver>& node,
const BVHModel<BV>& model1, const Transform3f& tf1,
const OcTree& model2, const Transform3f& tf2,
const OcTreeSolver<NarrowPhaseSolver>* otsolver,
- const CollisionRequest& request,
CollisionResult& result)
{
- node.request = request;
node.result = &result;
node.model1 = &model1;
@@ -220,10 +212,8 @@ bool initialize(OcTreeMeshCollisionTraversalNode<BV, NarrowPhaseSolver>& node,
const OcTree& model1, const Transform3f& tf1,
const BVHModel<BV>& model2, const Transform3f& tf2,
const OcTreeSolver<NarrowPhaseSolver>* otsolver,
- const CollisionRequest& request,
CollisionResult& result)
{
- node.request = request;
node.result = &result;
node.model1 = &model1;
@@ -292,16 +282,13 @@ bool initialize(ShapeCollisionTraversalNode<S1, S2, NarrowPhaseSolver>& node,
const S1& shape1, const Transform3f& tf1,
const S2& shape2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
- const CollisionRequest& request,
CollisionResult& result)
{
- node.model1 = &shape1;
node.tf1 = tf1;
node.model2 = &shape2;
node.tf2 = tf2;
node.nsolver = nsolver;
- node.request = request;
node.result = &result;
node.cost_density = shape1.cost_density * shape2.cost_density;
@@ -315,7 +302,6 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S, NarrowPhaseSolver>& node,
BVHModel<BV>& model1, Transform3f& tf1,
const S& model2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
- const CollisionRequest& request,
CollisionResult& result,
bool use_refit = false, bool refit_bottomup = false)
{
@@ -350,7 +336,6 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S, NarrowPhaseSolver>& node,
node.vertices = model1.vertices;
node.tri_indices = model1.tri_indices;
- node.request = request;
node.result = &result;
node.cost_density = model1.cost_density * model2.cost_density;
@@ -365,7 +350,6 @@ bool initialize(ShapeMeshCollisionTraversalNode<S, BV, NarrowPhaseSolver>& node,
const S& model1, const Transform3f& tf1,
BVHModel<BV>& model2, Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
- const CollisionRequest& request,
CollisionResult& result,
bool use_refit = false, bool refit_bottomup = false)
{
@@ -400,7 +384,6 @@ bool initialize(ShapeMeshCollisionTraversalNode<S, BV, NarrowPhaseSolver>& node,
node.vertices = model2.vertices;
node.tri_indices = model2.tri_indices;
- node.request = request;
node.result = &result;
node.cost_density = model1.cost_density * model2.cost_density;
@@ -417,7 +400,6 @@ static inline bool setupMeshShapeCollisionOrientedNode(OrientedNode<S, NarrowPha
const BVHModel<BV>& model1, const Transform3f& tf1,
const S& model2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
- const CollisionRequest& request,
CollisionResult& result)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES)
@@ -434,7 +416,6 @@ static inline bool setupMeshShapeCollisionOrientedNode(OrientedNode<S, NarrowPha
node.vertices = model1.vertices;
node.tri_indices = model1.tri_indices;
- node.request = request;
node.result = &result;
node.cost_density = model1.cost_density * model2.cost_density;
@@ -453,10 +434,9 @@ bool initialize(MeshShapeCollisionTraversalNodeOBB<S, NarrowPhaseSolver>& node,
const BVHModel<OBB>& model1, const Transform3f& tf1,
const S& model2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
- const CollisionRequest& request,
CollisionResult& result)
{
- return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
+ return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, result);
}
/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for RSS type
@@ -465,10 +445,9 @@ bool initialize(MeshShapeCollisionTraversalNodeRSS<S, NarrowPhaseSolver>& node,
const BVHModel<RSS>& model1, const Transform3f& tf1,
const S& model2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
- const CollisionRequest& request,
CollisionResult& result)
{
- return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
+ return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, result);
}
/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for kIOS type
@@ -477,10 +456,9 @@ bool initialize(MeshShapeCollisionTraversalNodekIOS<S, NarrowPhaseSolver>& node,
const BVHModel<kIOS>& model1, const Transform3f& tf1,
const S& model2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
- const CollisionRequest& request,
CollisionResult& result)
{
- return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
+ return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, result);
}
/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBBRSS type
@@ -489,10 +467,9 @@ bool initialize(MeshShapeCollisionTraversalNodeOBBRSS<S, NarrowPhaseSolver>& nod
const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
const S& model2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
- const CollisionRequest& request,
CollisionResult& result)
{
- return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
+ return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, result);
}
@@ -504,7 +481,6 @@ static inline bool setupShapeMeshCollisionOrientedNode(OrientedNode<S, NarrowPha
const S& model1, const Transform3f& tf1,
const BVHModel<BV>& model2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
- const CollisionRequest& request,
CollisionResult& result)
{
if(model2.getModelType() != BVH_MODEL_TRIANGLES)
@@ -521,7 +497,6 @@ static inline bool setupShapeMeshCollisionOrientedNode(OrientedNode<S, NarrowPha
node.vertices = model2.vertices;
node.tri_indices = model2.tri_indices;
- node.request = request;
node.result = &result;
node.cost_density = model1.cost_density * model2.cost_density;
@@ -537,10 +512,9 @@ bool initialize(ShapeMeshCollisionTraversalNodeOBB<S, NarrowPhaseSolver>& node,
const S& model1, const Transform3f& tf1,
const BVHModel<OBB>& model2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
- const CollisionRequest& request,
CollisionResult& result)
{
- return details::setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
+ return details::setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, result);
}
/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for RSS type
@@ -549,10 +523,9 @@ bool initialize(ShapeMeshCollisionTraversalNodeRSS<S, NarrowPhaseSolver>& node,
const S& model1, const Transform3f& tf1,
const BVHModel<RSS>& model2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
- const CollisionRequest& request,
CollisionResult& result)
{
- return details::setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
+ return details::setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, result);
}
/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for kIOS type
@@ -561,10 +534,9 @@ bool initialize(ShapeMeshCollisionTraversalNodekIOS<S, NarrowPhaseSolver>& node,
const S& model1, const Transform3f& tf1,
const BVHModel<kIOS>& model2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
- const CollisionRequest& request,
CollisionResult& result)
{
- return details::setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
+ return details::setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, result);
}
/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBBRSS type
@@ -573,10 +545,9 @@ bool initialize(ShapeMeshCollisionTraversalNodeOBBRSS<S, NarrowPhaseSolver>& nod
const S& model1, const Transform3f& tf1,
const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
- const CollisionRequest& request,
CollisionResult& result)
{
- return details::setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
+ return details::setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, result);
}
@@ -587,7 +558,6 @@ template<typename BV>
bool initialize(MeshCollisionTraversalNode<BV>& node,
BVHModel<BV>& model1, Transform3f& tf1,
BVHModel<BV>& model2, Transform3f& tf2,
- const CollisionRequest& request,
CollisionResult& result,
bool use_refit = false, bool refit_bottomup = false)
{
@@ -639,7 +609,6 @@ bool initialize(MeshCollisionTraversalNode<BV>& node,
node.tri_indices1 = model1.tri_indices;
node.tri_indices2 = model2.tri_indices;
- node.request = request;
node.result = &result;
node.cost_density = model1.cost_density * model2.cost_density;
@@ -652,25 +621,25 @@ bool initialize(MeshCollisionTraversalNode<BV>& node,
bool initialize(MeshCollisionTraversalNodeOBB& node,
const BVHModel<OBB>& model1, const Transform3f& tf1,
const BVHModel<OBB>& model2, const Transform3f& tf2,
- const CollisionRequest& request, CollisionResult& result);
+ CollisionResult& result);
/// @brief Initialize traversal node for collision between two meshes, specialized for RSS type
bool initialize(MeshCollisionTraversalNodeRSS& node,
const BVHModel<RSS>& model1, const Transform3f& tf1,
const BVHModel<RSS>& model2, const Transform3f& tf2,
- const CollisionRequest& request, CollisionResult& result);
+ CollisionResult& result);
/// @brief Initialize traversal node for collision between two meshes, specialized for OBBRSS type
bool initialize(MeshCollisionTraversalNodeOBBRSS& node,
const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
- const CollisionRequest& request, CollisionResult& result);
+ CollisionResult& result);
/// @brief Initialize traversal node for collision between two meshes, specialized for kIOS type
bool initialize(MeshCollisionTraversalNodekIOS& node,
const BVHModel<kIOS>& model1, const Transform3f& tf1,
const BVHModel<kIOS>& model2, const Transform3f& tf2,
- const CollisionRequest& request, CollisionResult& result);
+ CollisionResult& result);
/// @brief Initialize traversal node for distance between two geometric shapes
diff --git a/include/hpp/fcl/traversal/traversal_node_shapes.h b/include/hpp/fcl/traversal/traversal_node_shapes.h
index 659dcc00..c684b556 100644
--- a/include/hpp/fcl/traversal/traversal_node_shapes.h
+++ b/include/hpp/fcl/traversal/traversal_node_shapes.h
@@ -55,7 +55,8 @@ template<typename S1, typename S2, typename NarrowPhaseSolver>
class ShapeCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
- ShapeCollisionTraversalNode() : CollisionTraversalNodeBase()
+ ShapeCollisionTraversalNode(const CollisionRequest& request) :
+ CollisionTraversalNodeBase(request)
{
model1 = NULL;
model2 = NULL;
diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp
index 93be5a92..bd7ee24e 100644
--- a/src/collision_func_matrix.cpp
+++ b/src/collision_func_matrix.cpp
@@ -54,12 +54,12 @@ std::size_t ShapeOcTreeCollide(const CollisionGeometry* o1, const Transform3f& t
{
if(request.isSatisfied(result)) return result.numContacts();
- ShapeOcTreeCollisionTraversalNode<T_SH, NarrowPhaseSolver> node;
+ ShapeOcTreeCollisionTraversalNode<T_SH, NarrowPhaseSolver> node (request);
const T_SH* obj1 = static_cast<const T_SH*>(o1);
const OcTree* obj2 = static_cast<const OcTree*>(o2);
OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, result);
collide(&node, request, result);
return result.numContacts();
@@ -72,12 +72,12 @@ std::size_t OcTreeShapeCollide(const CollisionGeometry* o1, const Transform3f& t
{
if(request.isSatisfied(result)) return result.numContacts();
- OcTreeShapeCollisionTraversalNode<T_SH, NarrowPhaseSolver> node;
+ OcTreeShapeCollisionTraversalNode<T_SH, NarrowPhaseSolver> node (request);
const OcTree* obj1 = static_cast<const OcTree*>(o1);
const T_SH* obj2 = static_cast<const T_SH*>(o2);
OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, result);
collide(&node, request, result);
return result.numContacts();
@@ -90,12 +90,12 @@ std::size_t OcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, c
{
if(request.isSatisfied(result)) return result.numContacts();
- OcTreeCollisionTraversalNode<NarrowPhaseSolver> node;
+ OcTreeCollisionTraversalNode<NarrowPhaseSolver> node (request);
const OcTree* obj1 = static_cast<const OcTree*>(o1);
const OcTree* obj2 = static_cast<const OcTree*>(o2);
OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, result);
collide(&node, request, result);
return result.numContacts();
@@ -108,12 +108,12 @@ std::size_t OcTreeBVHCollide(const CollisionGeometry* o1, const Transform3f& tf1
{
if(request.isSatisfied(result)) return result.numContacts();
- OcTreeMeshCollisionTraversalNode<T_BVH, NarrowPhaseSolver> node;
+ OcTreeMeshCollisionTraversalNode<T_BVH, NarrowPhaseSolver> node (request);
const OcTree* obj1 = static_cast<const OcTree*>(o1);
const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>*>(o2);
OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, result);
collide(&node, request, result);
return result.numContacts();
}
@@ -125,12 +125,12 @@ std::size_t BVHOcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1
{
if(request.isSatisfied(result)) return result.numContacts();
- MeshOcTreeCollisionTraversalNode<T_BVH, NarrowPhaseSolver> node;
+ MeshOcTreeCollisionTraversalNode<T_BVH, NarrowPhaseSolver> node (request);
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
const OcTree* obj2 = static_cast<const OcTree*>(o2);
OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, result);
collide(&node, request, result);
return result.numContacts();
}
@@ -163,7 +163,7 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
return 0;
}
- ShapeCollisionTraversalNode<T_SH1, T_SH2, NarrowPhaseSolver> node;
+ ShapeCollisionTraversalNode<T_SH1, T_SH2, NarrowPhaseSolver> node (request);
const T_SH1* obj1 = static_cast<const T_SH1*>(o1);
const T_SH2* obj2 = static_cast<const T_SH2*>(o2);
@@ -177,7 +177,7 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
nsolver->enableCachedGuess(true);
}
- initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver, result);
collide(&node, request, result);
if(request.enable_cached_gjk_guess)
@@ -195,13 +195,13 @@ struct BVHShapeCollider
{
if(request.isSatisfied(result)) return result.numContacts();
- MeshShapeCollisionTraversalNode<T_BVH, T_SH, NarrowPhaseSolver> node;
+ MeshShapeCollisionTraversalNode<T_BVH, T_SH, NarrowPhaseSolver> node (request);
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
Transform3f tf1_tmp = tf1;
const T_SH* obj2 = static_cast<const T_SH*>(o2);
- initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, result);
+ initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, result);
fcl::collide(&node, request, result);
delete obj1_tmp;
@@ -219,11 +219,11 @@ std::size_t orientedBVHShapeCollide(const CollisionGeometry* o1, const Transform
{
if(request.isSatisfied(result)) return result.numContacts();
- OrientMeshShapeCollisionTraveralNode node;
+ OrientMeshShapeCollisionTraveralNode node (request);
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
const T_SH* obj2 = static_cast<const T_SH*>(o2);
- initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver, result);
fcl::collide(&node, request, result);
return result.numContacts();
}
@@ -284,7 +284,7 @@ std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, cons
{
if(request.isSatisfied(result)) return result.numContacts();
- MeshCollisionTraversalNode<T_BVH> node (request.enable_distance_lower_bound);
+ MeshCollisionTraversalNode<T_BVH> node (request);
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>* >(o2);
BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
@@ -292,7 +292,7 @@ std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, cons
BVHModel<T_BVH>* obj2_tmp = new BVHModel<T_BVH>(*obj2);
Transform3f tf2_tmp = tf2;
- initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request, result);
+ initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, result);
FCL_REAL sqrDistance;
fcl::collide(&node, request, result);
@@ -309,11 +309,11 @@ std::size_t orientedMeshCollide(const CollisionGeometry* o1, const Transform3f&
{
if(request.isSatisfied(result)) return result.numContacts();
- OrientedMeshCollisionTraversalNode node (request.enable_distance_lower_bound);
+ OrientedMeshCollisionTraversalNode node (request);
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>* >(o2);
- initialize(node, *obj1, tf1, *obj2, tf2, request, result);
+ initialize(node, *obj1, tf1, *obj2, tf2, result);
collide(&node, request, result);
return result.numContacts();
diff --git a/src/traversal/traversal_node_bvhs.cpp b/src/traversal/traversal_node_bvhs.cpp
index 05a09240..e17f5392 100644
--- a/src/traversal/traversal_node_bvhs.cpp
+++ b/src/traversal/traversal_node_bvhs.cpp
@@ -171,8 +171,8 @@ static inline void meshDistanceOrientedNodeLeafTesting(int b1, int b2,
}
MeshCollisionTraversalNodeOBB::MeshCollisionTraversalNodeOBB
-(bool enable_distance_lower_bound) :
- MeshCollisionTraversalNode<OBB> (enable_distance_lower_bound)
+(const CollisionRequest& request) :
+ MeshCollisionTraversalNode<OBB> (request)
{
R.setIdentity();
}
@@ -195,8 +195,8 @@ void MeshCollisionTraversalNodeOBB::leafTesting
MeshCollisionTraversalNodeRSS::MeshCollisionTraversalNodeRSS
-(bool enable_distance_lower_bound) :
- MeshCollisionTraversalNode<RSS> (enable_distance_lower_bound)
+(const CollisionRequest& request) :
+ MeshCollisionTraversalNode<RSS> (request)
{
R.setIdentity();
}
@@ -220,8 +220,8 @@ void MeshCollisionTraversalNodeRSS::leafTesting
MeshCollisionTraversalNodekIOS::MeshCollisionTraversalNodekIOS
-(bool enable_distance_lower_bound) :
- MeshCollisionTraversalNode<kIOS>(enable_distance_lower_bound)
+(const CollisionRequest& request) :
+ MeshCollisionTraversalNode<kIOS>(request)
{
R.setIdentity();
}
@@ -244,8 +244,8 @@ void MeshCollisionTraversalNodekIOS::leafTesting
MeshCollisionTraversalNodeOBBRSS::MeshCollisionTraversalNodeOBBRSS
-(bool enable_distance_lower_bound) :
- MeshCollisionTraversalNode<OBBRSS> (enable_distance_lower_bound)
+(const CollisionRequest& request) :
+ MeshCollisionTraversalNode<OBBRSS> (request)
{
R.setIdentity();
}
diff --git a/src/traversal/traversal_node_setup.cpp b/src/traversal/traversal_node_setup.cpp
index 0525d5d6..8e417309 100644
--- a/src/traversal/traversal_node_setup.cpp
+++ b/src/traversal/traversal_node_setup.cpp
@@ -48,7 +48,6 @@ template<typename BV, typename OrientedNode>
static inline bool setupMeshCollisionOrientedNode(OrientedNode& node,
const BVHModel<BV>& model1, const Transform3f& tf1,
const BVHModel<BV>& model2, const Transform3f& tf2,
- const CollisionRequest& request,
CollisionResult& result)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
@@ -65,7 +64,6 @@ static inline bool setupMeshCollisionOrientedNode(OrientedNode& node,
node.model2 = &model2;
node.tf2 = tf2;
- node.request = request;
node.result = &result;
node.cost_density = model1.cost_density * model2.cost_density;
@@ -81,39 +79,35 @@ static inline bool setupMeshCollisionOrientedNode(OrientedNode& node,
bool initialize(MeshCollisionTraversalNodeOBB& node,
const BVHModel<OBB>& model1, const Transform3f& tf1,
const BVHModel<OBB>& model2, const Transform3f& tf2,
- const CollisionRequest& request,
CollisionResult& result)
{
- return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, request, result);
+ return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, result);
}
bool initialize(MeshCollisionTraversalNodeRSS& node,
const BVHModel<RSS>& model1, const Transform3f& tf1,
const BVHModel<RSS>& model2, const Transform3f& tf2,
- const CollisionRequest& request,
CollisionResult& result)
{
- return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, request, result);
+ return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, result);
}
bool initialize(MeshCollisionTraversalNodekIOS& node,
const BVHModel<kIOS>& model1, const Transform3f& tf1,
const BVHModel<kIOS>& model2, const Transform3f& tf2,
- const CollisionRequest& request,
CollisionResult& result)
{
- return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, request, result);
+ return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, result);
}
bool initialize(MeshCollisionTraversalNodeOBBRSS& node,
const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
- const CollisionRequest& request,
CollisionResult& result)
{
- return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, request, result);
+ return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, result);
}
diff --git a/src/traversal/traversal_recurse.cpp b/src/traversal/traversal_recurse.cpp
index eb847321..6a39f010 100644
--- a/src/traversal/traversal_recurse.cpp
+++ b/src/traversal/traversal_recurse.cpp
@@ -50,7 +50,7 @@ void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2,
{
updateFrontList(front_list, b1, b2);
- if (node->enable_distance_lower_bound) {
+ if (node->request.enable_distance_lower_bound) {
if(node->BVTesting(b1, b2, sqrDistLowerBound)) return;
} else {
if(node->BVTesting(b1, b2)) return;
@@ -60,7 +60,7 @@ void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2,
return;
}
- if (node->enable_distance_lower_bound) {
+ if (node->request.enable_distance_lower_bound) {
if(node->BVTesting(b1, b2, sqrDistLowerBound)) {
updateFrontList(front_list, b1, b2);
return;
@@ -332,7 +332,7 @@ void propagateBVHFrontListCollisionRecurse
}
else
{
- if (node->enable_distance_lower_bound) {
+ if (node->request.enable_distance_lower_bound) {
if(!node->BVTesting(b1, b2, sqrDistLowerBound)) {
front_iter->valid = false;
if(node->firstOverSecond(b1, b2)) {
diff --git a/test/benchmark.cpp b/test/benchmark.cpp
index 49e68310..28571253 100644
--- a/test/benchmark.cpp
+++ b/test/benchmark.cpp
@@ -126,8 +126,8 @@ double collide (const std::vector<Transform3f>& tf,
timer.start();
for (std::size_t i = 0; i < tf.size(); ++i) {
- if(!initialize(node, m1, tf[i], m2, pose2, request, local_result))
- std::cout << "initialize error" << std::endl;
+ bool success (initialize(node, m1, tf[i], m2, pose2, local_result));
+ assert (success);
CollisionRequest request (1, true, true);
CollisionResult result;
diff --git a/test/test_fcl_collision.cpp b/test/test_fcl_collision.cpp
index d875137e..18328ea8 100644
--- a/test/test_fcl_collision.cpp
+++ b/test/test_fcl_collision.cpp
@@ -833,17 +833,15 @@ bool collide_Test2(const Transform3f& tf,
Transform3f pose1, pose2;
CollisionResult local_result;
- MeshCollisionTraversalNode<BV> node (false);
+ CollisionRequest request (num_max_contacts, enable_contact);
+ MeshCollisionTraversalNode<BV> node (request);
- if(!initialize<BV>(node, m1, pose1, m2, pose2,
- CollisionRequest(num_max_contacts, enable_contact), local_result))
- std::cout << "initialize error" << std::endl;
+ bool success = initialize <BV> (node, m1, pose1, m2, pose2, local_result);
+ assert (success);
node.enable_statistics = verbose;
- CollisionRequest request (1, true, true);
- CollisionResult result;
- collide(&node, request, result);
+ collide(&node, request, local_result);
if(local_result.numContacts() > 0)
{
@@ -893,17 +891,15 @@ bool collide_Test(const Transform3f& tf,
Transform3f pose1(tf), pose2;
CollisionResult local_result;
- MeshCollisionTraversalNode<BV> node (false);
+ CollisionRequest request (num_max_contacts, enable_contact);
+ MeshCollisionTraversalNode<BV> node (request);
- if(!initialize<BV>(node, m1, pose1, m2, pose2,
- CollisionRequest(num_max_contacts, enable_contact), local_result))
- std::cout << "initialize error" << std::endl;
+ bool success = initialize <BV> (node, m1, pose1, m2, pose2, local_result);
+ assert (success);
node.enable_statistics = verbose;
- CollisionRequest request (1, true, true);
- CollisionResult result;
- collide(&node, request, result);
+ collide(&node, request, local_result);
if(local_result.numContacts() > 0)
{
@@ -952,16 +948,15 @@ bool collide_Test_Oriented(const Transform3f& tf,
Transform3f pose1(tf), pose2;
CollisionResult local_result;
- TraversalNode node (false);
- if(!initialize(node, (const BVHModel<BV>&)m1, pose1, (const BVHModel<BV>&)m2, pose2,
- CollisionRequest(num_max_contacts, enable_contact), local_result))
- std::cout << "initialize error" << std::endl;
+ CollisionRequest request (num_max_contacts, enable_contact);
+ TraversalNode node (request);
+ bool success = initialize (node, (const BVHModel<BV>&)m1, pose1,
+ (const BVHModel<BV>&)m2, pose2, local_result);
+ assert (success);
node.enable_statistics = verbose;
- CollisionRequest request (1, true, true);
- CollisionResult result;
- collide(&node, request, result);
+ collide(&node, request, local_result);
if(local_result.numContacts() > 0)
{
diff --git a/test/test_fcl_distance.cpp b/test/test_fcl_distance.cpp
index 5dee1fbd..ea98c723 100644
--- a/test/test_fcl_distance.cpp
+++ b/test/test_fcl_distance.cpp
@@ -412,17 +412,17 @@ bool collide_Test_OBB(const Transform3f& tf,
m2.addSubModel(vertices2, triangles2);
m2.endModel();
- CollisionResult local_result;
- MeshCollisionTraversalNodeOBB node (false);
- if(!initialize(node, (const BVHModel<OBB>&)m1, tf, (const BVHModel<OBB>&)m2, Transform3f(),
- CollisionRequest(), local_result))
- std::cout << "initialize error" << std::endl;
+ CollisionResult local_result;
+ CollisionRequest request (1, true, true);
+ MeshCollisionTraversalNodeOBB node (request);
+ bool success (initialize(node, (const BVHModel<OBB>&)m1, tf,
+ (const BVHModel<OBB>&)m2, Transform3f(),
+ local_result));
+ assert (success);
node.enable_statistics = verbose;
- CollisionRequest request (1, true, true);
- CollisionResult result;
- collide(&node, request, result);
+ collide(&node, request, local_result);
if(local_result.numContacts() > 0)
return true;
diff --git a/test/test_fcl_frontlist.cpp b/test/test_fcl_frontlist.cpp
index b2ce1e6b..831c26a9 100644
--- a/test/test_fcl_frontlist.cpp
+++ b/test/test_fcl_frontlist.cpp
@@ -228,18 +228,16 @@ bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2,
Transform3f pose1, pose2;
- CollisionResult local_result;
- MeshCollisionTraversalNode<BV> node (false);
+ CollisionResult local_result;
+ CollisionRequest request (std::numeric_limits<int>::max(), false);
+ MeshCollisionTraversalNode<BV> node (request);
- if(!initialize<BV>(node, m1, pose1, m2, pose2,
- CollisionRequest(std::numeric_limits<int>::max(), false), local_result))
- std::cout << "initialize error" << std::endl;
+ bool success = initialize <BV>(node, m1, pose1, m2, pose2, local_result);
+ assert (success);
node.enable_statistics = verbose;
- CollisionRequest request (1, true, true);
- CollisionResult result;
- collide(&node, request, result, &front_list);
+ collide(&node, request, local_result, &front_list);
if(verbose) std::cout << "front list size " << front_list.size() << std::endl;
@@ -294,25 +292,25 @@ bool collide_front_list_Test_Oriented(const Transform3f& tf1, const Transform3f&
Transform3f pose1(tf1), pose2;
CollisionResult local_result;
- TraversalNode node (false);
+ CollisionRequest request (std::numeric_limits<int>::max(), false);
+ TraversalNode node (request);
- if(!initialize(node, (const BVHModel<BV>&)m1, pose1, (const BVHModel<BV>&)m2, pose2,
- CollisionRequest(std::numeric_limits<int>::max(), false), local_result))
- std::cout << "initialize error" << std::endl;
+ bool success = initialize (node, (const BVHModel<BV>&)m1, pose1,
+ (const BVHModel<BV>&)m2, pose2, local_result);
+ assert (success);
node.enable_statistics = verbose;
- CollisionRequest request (1, true, true);
- CollisionResult result;
- collide(&node, request, result, &front_list);
+ collide(&node, request, local_result, &front_list);
if(verbose) std::cout << "front list size " << front_list.size() << std::endl;
// update the mesh
pose1 = tf2;
- if(!initialize(node, (const BVHModel<BV>&)m1, pose1, (const BVHModel<BV>&)m2, pose2, CollisionRequest(), local_result))
- std::cout << "initialize error" << std::endl;
+ success = initialize (node, (const BVHModel<BV>&)m1, pose1,
+ (const BVHModel<BV>&)m2, pose2, local_result);
+ assert (success);
local_result.clear();
collide(&node, request, local_result, &front_list);
@@ -344,18 +342,16 @@ bool collide_Test(const Transform3f& tf,
Transform3f pose1(tf), pose2;
- CollisionResult local_result;
- MeshCollisionTraversalNode<BV> node (false);
+ CollisionResult local_result;
+ CollisionRequest request (std::numeric_limits<int>::max(), false);
+ MeshCollisionTraversalNode<BV> node (request);
- if(!initialize<BV>(node, m1, pose1, m2, pose2,
- CollisionRequest(std::numeric_limits<int>::max(), false), local_result))
- std::cout << "initialize error" << std::endl;
+ bool success = initialize <BV>(node, m1, pose1, m2, pose2, local_result);
+ assert (success);
node.enable_statistics = verbose;
- CollisionRequest request (1, true, true);
- CollisionResult result;
- collide(&node, request, result);
+ collide(&node, request, local_result);
if(local_result.numContacts() > 0)
return true;
--
GitLab