diff --git a/src/collision_node.cpp b/src/collision_node.cpp
index d7a5071fbe8a6557ca6adff1d1106ae5e1629f12..c69457030e0ea3d387bcb42cfea06a2ba9822c72 100644
--- a/src/collision_node.cpp
+++ b/src/collision_node.cpp
@@ -46,7 +46,8 @@ namespace fcl
void collide(CollisionTraversalNodeBase* node,
const CollisionRequest& request, CollisionResult& result,
- BVHFrontList* front_list)
+ BVHFrontList* front_list,
+ bool recursive)
{
if(front_list && front_list->size() > 0)
{
@@ -55,7 +56,10 @@ void collide(CollisionTraversalNodeBase* node,
else
{
FCL_REAL sqrDistLowerBound=0;
- collisionRecurse(node, 0, 0, front_list, sqrDistLowerBound);
+ if (recursive)
+ collisionRecurse(node, 0, 0, front_list, sqrDistLowerBound);
+ else
+ collisionNonRecurse(node, front_list, sqrDistLowerBound);
result.distance_lower_bound = sqrt (sqrDistLowerBound);
}
}
diff --git a/src/collision_node.h b/src/collision_node.h
index 23fd309de6da0d0281da458463727c00e40d367b..2f17fec589163edb1a7e65451d322a511113909e 100644
--- a/src/collision_node.h
+++ b/src/collision_node.h
@@ -62,7 +62,8 @@ namespace fcl
void collide(CollisionTraversalNodeBase* node,
const CollisionRequest& request,
CollisionResult& result,
- BVHFrontList* front_list = NULL);
+ BVHFrontList* front_list = NULL,
+ bool recursive = true);
/// @brief distance computation on distance traversal node; can use front list to accelerate
void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list = NULL, int qsize = 2);
diff --git a/test/test_fcl_collision-bench.py b/test/test_fcl_collision-bench.py
index 8bd9836e35dcd2993efff7517c29ff5edc3fa9ab..41a46cf4210354a9efeaeb1836a4eff3ee144845 100644
--- a/test/test_fcl_collision-bench.py
+++ b/test/test_fcl_collision-bench.py
@@ -26,9 +26,10 @@ while values[0][0:3] == values[Ntransforms][0:3]:
Ntransforms += 1
splitMethods = ['avg', 'med', 'cen']
+type = ["o", "or", "r", ]
BVs = list (set ([ v[0] for v in request1[::Ntransforms] ]))
xvals = [ BVs.index(v[0]) + len(BVs)*v[2] + 3*len(BVs)*v[1] for v in request1[::Ntransforms] ]
-cases = [ v[0] + ("*" if v[1] == 1 else "") + " " + splitMethods[v[2]] for v in request1[::Ntransforms] ]
+cases = [ v[0] + " " + type[v[1]] + " " + splitMethods[v[2]] for v in request1[::Ntransforms] ]
idx_reorder = sorted (list(range(len(xvals))), key=lambda i: xvals[i])
def reorder (l): return [ l[i] for i in idx_reorder ]
@@ -36,10 +37,12 @@ def reorder (l): return [ l[i] for i in idx_reorder ]
xvals_s = reorder (xvals)
cases_s = reorder (cases)
+onlyLB = True
# Time
plt.figure(0)
for i in range(Ntransforms):
- plt.plot(xvals_s, reorder([ v[5] for v in request1[i::Ntransforms] ]) , '-.o', label=str(i))
+ if not onlyLB:
+ plt.plot(xvals_s, reorder([ v[5] for v in request1[i::Ntransforms] ]) , '-.o', label=str(i))
plt.plot(xvals_s, reorder([ v[5] for v in request2[i::Ntransforms] ]) , ':+', label=str(i)+"+lb")
plt.legend()
@@ -47,6 +50,19 @@ plt.xticks(ticks=xvals_s, labels=cases_s, rotation=90)
plt.ylabel('Time (us)')
plt.yscale('log')
+# Time
+plt.figure(2)
+for k in range (0, len(request1), Ntransforms):
+ if not onlyLB:
+ plt.plot([ xvals[int(k/Ntransforms)], ], sum([ v[5] for v in request1[k:k+Ntransforms] ])/Ntransforms)
+ plt.plot([ xvals[int(k/Ntransforms)], ], sum([ v[5] for v in request2[k:k+Ntransforms] ])/Ntransforms)
+
+plt.plot(xvals_s, reorder ([ sum([ v[5] for v in request2[k:k+Ntransforms] ])/Ntransforms for k in range (0, len(request1), Ntransforms) ]))
+
+plt.xticks(ticks=xvals_s, labels=cases_s, rotation=90)
+plt.ylabel('Time (us)')
+plt.yscale('log')
+
# Distance
plt.figure(1)
for i in range(Ntransforms):
diff --git a/test/test_fcl_collision.cpp b/test/test_fcl_collision.cpp
index eb09945343f49809851d221e95624bc8cc687c7a..d0dd6c6f9efe9dcf96a97051c98716733d1fbab3 100644
--- a/test/test_fcl_collision.cpp
+++ b/test/test_fcl_collision.cpp
@@ -244,12 +244,19 @@ struct base_traits
};
};
-template<typename BV, int _Options>
+enum {
+ Oriented = true,
+ NonOriented = false,
+ Recursive = true,
+ NonRecursive = false
+};
+
+template<typename BV, bool Oriented, bool recursive>
struct traits : base_traits
{};
-template<size_t N>
-struct traits<KDOP<N>, 0> : base_traits
+template<size_t N, bool recursive>
+struct traits<KDOP<N>, Oriented, recursive> : base_traits
{
enum { IS_IMPLEMENTED = false
};
@@ -318,21 +325,20 @@ struct mesh_mesh_run_test
clock_type::time_point start, end;
const Transform3f tf2;
const std::size_t N = transforms.size();
- std::size_t s = N, e;
- std::vector<CollisionResult> results (2 * N);
- if (traits<BV, 0>::IS_IMPLEMENTED)
+
+ contacts.resize (3*N);
+
+ if (traits<BV, Oriented, Recursive>::IS_IMPLEMENTED)
{
BOOST_TEST_MESSAGE (getindent() << "BV: " << str<BV>() << " oriented");
++indent;
- s = 0;
- e = N;
- start = clock_type::now();
for(std::size_t i = 0; i < transforms.size(); ++i)
{
+ start = clock_type::now();
const Transform3f& tf1 = transforms[i];
- CollisionResult& local_result (results[i]);
+ CollisionResult local_result;
MeshCollisionTraversalNode<BV, 0> node (request);
node.enable_statistics = enable_statistics;
@@ -358,22 +364,27 @@ struct mesh_mesh_run_test
BENCHMARK(local_result.distance_lower_bound);
BENCHMARK((end - start).count());
BENCHMARK_NEXT();
+
+ if(local_result.numContacts() > 0)
+ {
+ local_result.getContacts(contacts[i]);
+ std::sort(contacts[i].begin(), contacts[i].end());
+ }
}
--indent;
}
- if (traits<BV, RelativeTransformationIsIdentity>::IS_IMPLEMENTED)
+ if (traits<BV, NonOriented, Recursive>::IS_IMPLEMENTED)
{
BOOST_TEST_MESSAGE (getindent() << "BV: " << str<BV>());
++indent;
- e = 2*N;
for(std::size_t i = 0; i < transforms.size(); ++i)
{
start = clock_type::now();
const Transform3f tf1 = transforms[i];
- CollisionResult& local_result (results[i + N]);
+ CollisionResult local_result;
MeshCollisionTraversalNode<BV, RelativeTransformationIsIdentity> node (request);
node.enable_statistics = enable_statistics;
@@ -393,7 +404,7 @@ struct mesh_mesh_run_test
end = clock_type::now();
BENCHMARK(str<BV>());
- BENCHMARK("0");
+ BENCHMARK(2);
BENCHMARK(splitMethod);
if (enable_statistics) {
BOOST_TEST_MESSAGE (getindent() << "statistics: " << node.num_bv_tests << " " << node.num_leaf_tests);
@@ -405,20 +416,59 @@ struct mesh_mesh_run_test
BENCHMARK(local_result.distance_lower_bound);
BENCHMARK((end - start).count());
BENCHMARK_NEXT();
+
+ if(local_result.numContacts() > 0)
+ {
+ local_result.getContacts(contacts[i+N]);
+ std::sort(contacts[i+N].begin(), contacts[i+N].end());
+ }
}
--indent;
}
- contacts.resize (2*N);
- for(std::size_t i = s; i < e; ++i)
+ if (traits<BV, Oriented, NonRecursive>::IS_IMPLEMENTED)
{
- const CollisionResult& local_result (results[i]);
+ BOOST_TEST_MESSAGE (getindent() << "BV: " << str<BV>() << " oriented non-recursive");
+ ++indent;
- if(local_result.numContacts() > 0)
+ for(std::size_t i = 0; i < transforms.size(); ++i)
{
- local_result.getContacts(contacts[i]);
- std::sort(contacts[i].begin(), contacts[i].end());
+ start = clock_type::now();
+ const Transform3f tf1 = transforms[i];
+
+ CollisionResult local_result;
+ MeshCollisionTraversalNode<BV, 0> node (request);
+ node.enable_statistics = enable_statistics;
+
+ bool success = initialize (node,
+ *model1, tf1, *model2, tf2,
+ local_result);
+ BOOST_REQUIRE (success);
+
+ collide(&node, request, local_result, NULL, false);
+
+ end = clock_type::now();
+ BENCHMARK(str<BV>());
+ BENCHMARK(0);
+ BENCHMARK(splitMethod);
+ if (enable_statistics) {
+ BOOST_TEST_MESSAGE (getindent() << "statistics: " << node.num_bv_tests << " " << node.num_leaf_tests);
+ BOOST_TEST_MESSAGE (getindent() << "nb contacts: " << local_result.numContacts());
+ BENCHMARK(node.num_bv_tests);
+ BENCHMARK(node.num_leaf_tests);
+ }
+ BENCHMARK(local_result.numContacts());
+ BENCHMARK(local_result.distance_lower_bound);
+ BENCHMARK((end - start).count());
+ BENCHMARK_NEXT();
+
+ if(local_result.numContacts() > 0)
+ {
+ local_result.getContacts(contacts[i+2*N]);
+ std::sort(contacts[i+2*N].begin(), contacts[i+2*N].end());
+ }
}
+ --indent;
}
}
@@ -428,10 +478,10 @@ struct mesh_mesh_run_test
if (benchmark) return;
const std::size_t N = transforms.size();
- BOOST_REQUIRE_EQUAL(contacts.size(), 2*N);
+ BOOST_REQUIRE_EQUAL(contacts.size(), 3*N);
BOOST_REQUIRE_EQUAL(contacts.size(), contacts_ref.size());
- if (traits<BV, 0>::IS_IMPLEMENTED) {
+ if (traits<BV, Oriented, Recursive>::IS_IMPLEMENTED) {
for(std::size_t i = 0; i < N; ++i) {
BOOST_CHECK_EQUAL(contacts_ref[i].size(), contacts[i].size());
for(std::size_t j = 0; j < contacts[i].size(); ++j) {
@@ -440,7 +490,7 @@ struct mesh_mesh_run_test
}
}
}
- if (traits<BV, RelativeTransformationIsIdentity>::IS_IMPLEMENTED) {
+ if (traits<BV, NonOriented, Recursive>::IS_IMPLEMENTED) {
for(std::size_t i = N; i < 2*N; ++i) {
BOOST_CHECK_EQUAL(contacts_ref[i].size(), contacts[i].size());
for(std::size_t j = 0; j < contacts[i].size(); ++j) {
@@ -449,6 +499,15 @@ struct mesh_mesh_run_test
}
}
}
+ if (traits<BV, Oriented, NonRecursive>::IS_IMPLEMENTED) {
+ for(std::size_t i = 2*N; i < 3*N; ++i) {
+ BOOST_CHECK_EQUAL(contacts_ref[i].size(), contacts[i].size());
+ for(std::size_t j = 0; j < contacts[i].size(); ++j) {
+ BOOST_CHECK_EQUAL(contacts_ref[i][j].b1, contacts[i][j].b1);
+ BOOST_CHECK_EQUAL(contacts_ref[i][j].b2, contacts[i][j].b2);
+ }
+ }
+ }
}
template<typename BV>