38 #include <boost/mpl/vector.hpp>
40 #define BOOST_TEST_MODULE FCL_COLLISION
41 #include <boost/test/included/unit_test.hpp>
44 #include <boost/assign/list_of.hpp>
54 #include "../src/collision_node.h"
60 #include "fcl_resources/config.h"
63 namespace utf = boost::unit_test::framework;
68 FCL_REAL r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
69 std::vector<Transform3f> rotate_transform;
77 convertBV(aabb1, rotate_transform[0], obb1);
85 std::vector<Transform3f> transforms;
88 for (std::size_t i = 0; i < transforms.size(); ++i) {
103 bool overlap_obb = obb1.
overlap(obb2);
104 bool overlap_box = solver.
shapeIntersect(box1, box1_tf, box2, box2_tf,
107 BOOST_CHECK(overlap_obb == overlap_box);
112 FCL_REAL r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
113 std::vector<Transform3f> rotate_transform;
121 convertBV(aabb1, rotate_transform[0], obb1);
124 constructBox(aabb1, rotate_transform[0], box1, box1_tf);
127 std::size_t n = 1000;
129 std::vector<Transform3f> transforms;
132 for (std::size_t i = 0; i < transforms.size(); ++i) {
142 bool overlap_obb = obb1.
overlap(obb2);
144 box1, box1_tf,
sphere, transforms[i],
distance,
false, NULL, NULL);
145 BOOST_CHECK(overlap_obb >= overlap_sphere);
152 bool overlap_obb = obb1.
overlap(obb2);
154 box1, box1_tf, capsule, transforms[i],
distance,
false, NULL, NULL);
155 BOOST_CHECK(overlap_obb >= overlap_capsule);
159 Cone cone(len, 2 * len);
162 bool overlap_obb = obb1.
overlap(obb2);
164 box1, box1_tf, cone, transforms[i],
distance,
false, NULL, NULL);
165 BOOST_CHECK(overlap_obb >= overlap_cone);
170 computeBV(cylinder, transforms[i], obb2);
172 bool overlap_obb = obb1.
overlap(obb2);
174 box1, box1_tf, cylinder, transforms[i],
distance,
false, NULL, NULL);
175 BOOST_CHECK(overlap_obb >= overlap_cylinder);
182 std::size_t n = 1000;
184 std::vector<Transform3f> transforms;
194 for (std::size_t i = 0; i < transforms.size(); ++i) {
199 AABB aabb2 =
translate(aabb, transforms[i].getTranslation());
204 bool overlap_aabb = aabb1.
overlap(aabb2);
205 bool overlap_obb = obb1.
overlap(obb2);
206 if (overlap_aabb != overlap_obb) {
207 std::cout << aabb1.
min_ <<
" " << aabb1.
max_ << std::endl;
208 std::cout << aabb2.
min_ <<
" " << aabb2.
max_ << std::endl;
209 std::cout << obb1.
To <<
" " << obb1.
extent <<
" " << obb1.
axes
211 std::cout << obb2.
To <<
" " << obb2.
extent <<
" " << obb2.
axes
215 BOOST_CHECK(overlap_aabb == overlap_obb);
217 std::cout << std::endl;
223 #define BENCHMARK(stream) \
224 if (bench_stream != NULL) { \
225 *bench_stream << (bs_nl ? "" : ", ") << stream; \
228 #define BENCHMARK_HEADER(stream) \
232 #define BENCHMARK_NEXT() \
233 if (bench_stream != NULL && !bs_nl) { \
234 *bench_stream << '\n'; \
245 #define BV_STR_SPECIALIZATION(bv) \
247 const char* str<bv>() { \
250 template <
typename BV>
261 template <
typename T>
265 enum { IS_IMPLEMENTED =
true };
275 template <
typename BV,
bool Oriented,
bool recursive>
278 template <
short N,
bool recursive>
280 enum { IS_IMPLEMENTED =
false };
286 : transforms(_transforms),
288 enable_statistics(false),
304 static const char*
t[] = {
"",
316 template <
typename BV>
317 void query(
const std::vector<Transform3f>& transforms,
319 std::vector<Contacts_t>& contacts) {
323 if (enable_statistics) {
333 typedef shared_ptr<BVH_t> BVHPtr_t;
335 BVHPtr_t model1(
new BVH_t), model2(
new BVH_t);
346 const std::size_t N = transforms.size();
348 contacts.resize(3 * N);
351 BOOST_TEST_MESSAGE(getindent() <<
"BV: " << str<BV>() <<
" oriented");
354 for (std::size_t i = 0; i < transforms.size(); ++i) {
359 MeshCollisionTraversalNode<BV, 0> node(request);
360 node.enable_statistics = enable_statistics;
364 BOOST_REQUIRE(success);
366 collide(&node, request, local_result);
373 if (enable_statistics) {
374 BOOST_TEST_MESSAGE(getindent() <<
"statistics: " << node.num_bv_tests
375 <<
" " << node.num_leaf_tests);
376 BOOST_TEST_MESSAGE(getindent()
388 std::sort(contacts[i].begin(), contacts[i].end());
395 BOOST_TEST_MESSAGE(getindent() <<
"BV: " << str<BV>());
398 for (std::size_t i = 0; i < transforms.size(); ++i) {
403 MeshCollisionTraversalNode<BV, RelativeTransformationIsIdentity> node(
405 node.enable_statistics = enable_statistics;
407 BVH_t* model1_tmp =
new BVH_t(*model1);
409 BVH_t* model2_tmp =
new BVH_t(*model2);
412 bool success =
initialize(node, *model1_tmp, tf1_tmp, *model2_tmp,
413 tf2_tmp, local_result,
true,
true);
414 BOOST_REQUIRE(success);
416 collide(&node, request, local_result);
424 if (enable_statistics) {
425 BOOST_TEST_MESSAGE(getindent() <<
"statistics: " << node.num_bv_tests
426 <<
" " << node.num_leaf_tests);
427 BOOST_TEST_MESSAGE(getindent()
439 std::sort(contacts[i + N].begin(), contacts[i + N].end());
446 BOOST_TEST_MESSAGE(getindent()
447 <<
"BV: " << str<BV>() <<
" oriented non-recursive");
450 for (std::size_t i = 0; i < transforms.size(); ++i) {
455 MeshCollisionTraversalNode<BV, 0> node(request);
456 node.enable_statistics = enable_statistics;
460 BOOST_REQUIRE(success);
462 collide(&node, request, local_result, NULL,
false);
468 if (enable_statistics) {
469 BOOST_TEST_MESSAGE(getindent() <<
"statistics: " << node.num_bv_tests
470 <<
" " << node.num_leaf_tests);
471 BOOST_TEST_MESSAGE(getindent()
483 std::sort(contacts[i + 2 * N].begin(), contacts[i + 2 * N].end());
491 for (std::size_t i = i0; i < i1; ++i) {
494 contacts_ref[i].begin(), contacts_ref[i].end(), contacts[i].begin(),
496 std::inserter(in_ref_but_not_in_i, in_ref_but_not_in_i.begin()));
497 if (!in_ref_but_not_in_i.empty()) {
498 for (std::size_t j = 0; j < in_ref_but_not_in_i.size(); ++j) {
501 false,
"Missed contacts: " << in_ref_but_not_in_i[j].b1 <<
", "
502 << in_ref_but_not_in_i[j].b2);
505 false,
"Missed contacts: " << in_ref_but_not_in_i[j].b1 <<
", "
506 << in_ref_but_not_in_i[j].b2);
513 contacts[i].begin(), contacts[i].end(), contacts_ref[i].begin(),
514 contacts_ref[i].end(),
515 std::inserter(in_i_but_not_in_ref, in_i_but_not_in_ref.begin()));
517 if (!in_i_but_not_in_ref.empty()) {
518 for (std::size_t j = 0; j < in_i_but_not_in_ref.size(); ++j) {
521 false,
"False contacts: " << in_i_but_not_in_ref[j].b1 <<
", "
522 << in_i_but_not_in_ref[j].b2);
525 false,
"False contacts: " << in_i_but_not_in_ref[j].b1 <<
", "
526 << in_i_but_not_in_ref[j].b2);
533 template <
typename BV>
535 if (benchmark)
return;
536 const std::size_t N = transforms.size();
538 BOOST_REQUIRE_EQUAL(contacts.size(), 3 * N);
539 BOOST_REQUIRE_EQUAL(contacts.size(), contacts_ref.size());
542 BOOST_TEST_MESSAGE(getindent() <<
"BV: " << str<BV>() <<
" oriented");
544 check_contacts(0, N,
false);
548 BOOST_TEST_MESSAGE(getindent() <<
"BV: " << str<BV>());
550 check_contacts(N, 2 * N,
true);
554 BOOST_TEST_MESSAGE(getindent()
555 <<
"BV: " << str<BV>() <<
" oriented non-recursive");
557 check_contacts(2 * N, 3 * N,
false);
562 template <
typename BV>
565 BOOST_TEST_MESSAGE(getindent() <<
"splitMethod: " <<
splitMethods[i]);
568 (isInit ? contacts : contacts_ref));
569 if (isInit) check<BV>();
593 std::vector<Transform3f> transforms;
595 #ifndef NDEBUG // if debug mode
600 n =
getNbRun(utf::master_test_suite().argc, utf::master_test_suite().argv, n);
604 Eigen::IOFormat f(Eigen::FullPrecision, 0,
", ",
",",
"",
"",
"(",
")");
605 for (std::size_t i = 0; i < transforms.size(); ++i) {
607 "q" << i <<
"=" << transforms[i].getTranslation().format(f) <<
"+"
608 << transforms[i].getQuatRotation().coeffs().format(f));
615 boost::mpl::for_each<BVs_t, wrap<boost::mpl::placeholders::_1> >(runner);
619 std::vector<Transform3f> transforms;
626 n =
getNbRun(utf::master_test_suite().argc, utf::master_test_suite().argv, n);
630 Eigen::IOFormat f(Eigen::FullPrecision, 0,
", ",
",",
"",
"",
"(",
")");
631 for (std::size_t i = 0; i < transforms.size(); ++i) {
633 "q" << i <<
"=" << transforms[i].getTranslation().format(f) <<
"+"
634 << transforms[i].getQuatRotation().coeffs().format(f));
642 std::ofstream ofs(
"./collision.benchmark.csv", std::ofstream::out);
649 boost::mpl::for_each<BVs_t, wrap<boost::mpl::placeholders::_1> >(runner1);
654 runner2.enable_statistics =
false;
655 runner2.benchmark =
true;
656 boost::mpl::for_each<BVs_t, wrap<boost::mpl::placeholders::_1> >(runner2);