test/collision.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2015, Open Source Robotics Foundation
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
38 #include <boost/mpl/vector.hpp>
39 
40 #define BOOST_TEST_MODULE FCL_COLLISION
41 #include <boost/test/included/unit_test.hpp>
42 
43 #include <fstream>
44 #include <boost/assign/list_of.hpp>
45 
46 #include <hpp/fcl/collision.h>
47 #include <hpp/fcl/BV/BV.h>
51 
54 #include "../src/collision_node.h"
56 
57 #include <hpp/fcl/timings.h>
58 
59 #include "utility.h"
60 #include "fcl_resources/config.h"
61 
62 using namespace hpp::fcl;
63 namespace utf = boost::unit_test::framework;
64 
65 int num_max_contacts = (std::numeric_limits<int>::max)();
66 
67 BOOST_AUTO_TEST_CASE(OBB_Box_test) {
68  FCL_REAL r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
69  std::vector<Transform3f> rotate_transform;
70  generateRandomTransforms(r_extents, rotate_transform, 1);
71 
72  AABB aabb1;
73  aabb1.min_ = Vec3f(-600, -600, -600);
74  aabb1.max_ = Vec3f(600, 600, 600);
75 
76  OBB obb1;
77  convertBV(aabb1, rotate_transform[0], obb1);
78  Box box1;
79  Transform3f box1_tf;
80  constructBox(aabb1, rotate_transform[0], box1, box1_tf);
81 
82  FCL_REAL extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
83  std::size_t n = 1000;
84 
85  std::vector<Transform3f> transforms;
86  generateRandomTransforms(extents, transforms, n);
87 
88  for (std::size_t i = 0; i < transforms.size(); ++i) {
89  AABB aabb;
90  aabb.min_ = aabb1.min_ * 0.5;
91  aabb.max_ = aabb1.max_ * 0.5;
92 
93  OBB obb2;
94  convertBV(aabb, transforms[i], obb2);
95 
96  Box box2;
97  Transform3f box2_tf;
98  constructBox(aabb, transforms[i], box2, box2_tf);
99 
100  GJKSolver solver;
101 
103  bool overlap_obb = obb1.overlap(obb2);
104  bool overlap_box = solver.shapeIntersect(box1, box1_tf, box2, box2_tf,
105  distance, false, NULL, NULL);
106 
107  BOOST_CHECK(overlap_obb == overlap_box);
108  }
109 }
110 
111 BOOST_AUTO_TEST_CASE(OBB_shape_test) {
112  FCL_REAL r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
113  std::vector<Transform3f> rotate_transform;
114  generateRandomTransforms(r_extents, rotate_transform, 1);
115 
116  AABB aabb1;
117  aabb1.min_ = Vec3f(-600, -600, -600);
118  aabb1.max_ = Vec3f(600, 600, 600);
119 
120  OBB obb1;
121  convertBV(aabb1, rotate_transform[0], obb1);
122  Box box1;
123  Transform3f box1_tf;
124  constructBox(aabb1, rotate_transform[0], box1, box1_tf);
125 
126  FCL_REAL extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
127  std::size_t n = 1000;
128 
129  std::vector<Transform3f> transforms;
130  generateRandomTransforms(extents, transforms, n);
131 
132  for (std::size_t i = 0; i < transforms.size(); ++i) {
133  FCL_REAL len = (aabb1.max_[0] - aabb1.min_[0]) * 0.5;
134  OBB obb2;
135  GJKSolver solver;
137 
138  {
139  Sphere sphere(len);
140  computeBV(sphere, transforms[i], obb2);
141 
142  bool overlap_obb = obb1.overlap(obb2);
143  bool overlap_sphere = solver.shapeIntersect(
144  box1, box1_tf, sphere, transforms[i], distance, false, NULL, NULL);
145  BOOST_CHECK(overlap_obb >= overlap_sphere);
146  }
147 
148  {
149  Capsule capsule(len, 2 * len);
150  computeBV(capsule, transforms[i], obb2);
151 
152  bool overlap_obb = obb1.overlap(obb2);
153  bool overlap_capsule = solver.shapeIntersect(
154  box1, box1_tf, capsule, transforms[i], distance, false, NULL, NULL);
155  BOOST_CHECK(overlap_obb >= overlap_capsule);
156  }
157 
158  {
159  Cone cone(len, 2 * len);
160  computeBV(cone, transforms[i], obb2);
161 
162  bool overlap_obb = obb1.overlap(obb2);
163  bool overlap_cone = solver.shapeIntersect(
164  box1, box1_tf, cone, transforms[i], distance, false, NULL, NULL);
165  BOOST_CHECK(overlap_obb >= overlap_cone);
166  }
167 
168  {
169  Cylinder cylinder(len, 2 * len);
170  computeBV(cylinder, transforms[i], obb2);
171 
172  bool overlap_obb = obb1.overlap(obb2);
173  bool overlap_cylinder = solver.shapeIntersect(
174  box1, box1_tf, cylinder, transforms[i], distance, false, NULL, NULL);
175  BOOST_CHECK(overlap_obb >= overlap_cylinder);
176  }
177  }
178 }
179 
180 BOOST_AUTO_TEST_CASE(OBB_AABB_test) {
181  FCL_REAL extents[] = {-1000, -1000, -1000, 1000, 1000, 1000};
182  std::size_t n = 1000;
183 
184  std::vector<Transform3f> transforms;
185  generateRandomTransforms(extents, transforms, n);
186 
187  AABB aabb1;
188  aabb1.min_ = Vec3f(-600, -600, -600);
189  aabb1.max_ = Vec3f(600, 600, 600);
190 
191  OBB obb1;
192  convertBV(aabb1, Transform3f(), obb1);
193 
194  for (std::size_t i = 0; i < transforms.size(); ++i) {
195  AABB aabb;
196  aabb.min_ = aabb1.min_ * 0.5;
197  aabb.max_ = aabb1.max_ * 0.5;
198 
199  AABB aabb2 = translate(aabb, transforms[i].getTranslation());
200 
201  OBB obb2;
202  convertBV(aabb, Transform3f(transforms[i].getTranslation()), obb2);
203 
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
210  << std::endl;
211  std::cout << obb2.To << " " << obb2.extent << " " << obb2.axes
212  << std::endl;
213  }
214 
215  BOOST_CHECK(overlap_aabb == overlap_obb);
216  }
217  std::cout << std::endl;
218 }
219 
220 std::ostream* bench_stream = NULL;
221 bool bs_nl = true;
222 bool bs_hp = false;
223 #define BENCHMARK(stream) \
224  if (bench_stream != NULL) { \
225  *bench_stream << (bs_nl ? "" : ", ") << stream; \
226  bs_nl = false; \
227  }
228 #define BENCHMARK_HEADER(stream) \
229  if (!bs_hp) { \
230  BENCHMARK(stream) \
231  }
232 #define BENCHMARK_NEXT() \
233  if (bench_stream != NULL && !bs_nl) { \
234  *bench_stream << '\n'; \
235  bs_nl = true; \
236  bs_hp = true; \
237  }
238 
239 typedef std::vector<Contact> Contacts_t;
240 typedef boost::mpl::vector<OBB, RSS, KDOP<24>, KDOP<18>, KDOP<16>, kIOS, OBBRSS>
242 std::vector<SplitMethodType> splitMethods = boost::assign::list_of(
244 
245 #define BV_STR_SPECIALIZATION(bv) \
246  template <> \
247  const char* str<bv>() { \
248  return #bv; \
249  }
250 template <typename BV>
251 const char* str();
260 
261 template <typename T>
262 struct wrap {};
263 
264 struct base_traits {
265  enum { IS_IMPLEMENTED = true };
266 };
267 
268 enum {
269  Oriented = true,
270  NonOriented = false,
271  Recursive = true,
272  NonRecursive = false
273 };
274 
275 template <typename BV, bool Oriented, bool recursive>
276 struct traits : base_traits {};
277 
278 template <short N, bool recursive>
279 struct traits<KDOP<N>, Oriented, recursive> : base_traits {
280  enum { IS_IMPLEMENTED = false };
281 };
282 
284  mesh_mesh_run_test(const std::vector<Transform3f>& _transforms,
285  const CollisionRequest _request)
286  : transforms(_transforms),
287  request(_request),
288  enable_statistics(false),
289  benchmark(false),
290  isInit(false),
291  indent(0) {}
292 
293  const std::vector<Transform3f>& transforms;
295  bool enable_statistics, benchmark;
296  std::vector<Contacts_t> contacts;
297  std::vector<Contacts_t> contacts_ref;
298  bool isInit;
299 
300  int indent;
301 
302  const char* getindent() {
303  assert(indent < 9);
304  static const char* t[] = {"",
305  "\t",
306  "\t\t",
307  "\t\t\t",
308  "\t\t\t\t",
309  "\t\t\t\t\t",
310  "\t\t\t\t\t\t",
311  "\t\t\t\t\t\t\t",
312  "\t\t\t\t\t\t\t\t"};
313  return t[indent];
314  }
315 
316  template <typename BV>
317  void query(const std::vector<Transform3f>& transforms,
318  SplitMethodType splitMethod, const CollisionRequest request,
319  std::vector<Contacts_t>& contacts) {
320  BENCHMARK_HEADER("BV");
321  BENCHMARK_HEADER("oriented");
322  BENCHMARK_HEADER("Split method");
323  if (enable_statistics) {
324  BENCHMARK_HEADER("num_bv_tests");
325  BENCHMARK_HEADER("num_leaf_tests");
326  }
327  BENCHMARK_HEADER("numContacts");
328  BENCHMARK_HEADER("distance_lower_bound");
329  BENCHMARK_HEADER("time");
330  BENCHMARK_NEXT();
331 
332  typedef BVHModel<BV> BVH_t;
333  typedef shared_ptr<BVH_t> BVHPtr_t;
334 
335  BVHPtr_t model1(new BVH_t), model2(new BVH_t);
336  model1->bv_splitter.reset(new BVSplitter<BV>(splitMethod));
337  model2->bv_splitter.reset(new BVSplitter<BV>(splitMethod));
338 
339  loadPolyhedronFromResource(TEST_RESOURCES_DIR "/env.obj", Vec3f::Ones(),
340  model1);
341  loadPolyhedronFromResource(TEST_RESOURCES_DIR "/rob.obj", Vec3f::Ones(),
342  model2);
343 
344  Timer timer(false);
345  const Transform3f tf2;
346  const std::size_t N = transforms.size();
347 
348  contacts.resize(3 * N);
349 
351  BOOST_TEST_MESSAGE(getindent() << "BV: " << str<BV>() << " oriented");
352  ++indent;
353 
354  for (std::size_t i = 0; i < transforms.size(); ++i) {
355  const Transform3f& tf1 = transforms[i];
356  timer.start();
357 
358  CollisionResult local_result;
359  MeshCollisionTraversalNode<BV, 0> node(request);
360  node.enable_statistics = enable_statistics;
361 
362  bool success =
363  initialize(node, *model1, tf1, *model2, tf2, local_result);
364  BOOST_REQUIRE(success);
365 
366  collide(&node, request, local_result);
367 
368  timer.stop();
369 
370  BENCHMARK(str<BV>());
371  BENCHMARK(1);
372  BENCHMARK(splitMethod);
373  if (enable_statistics) {
374  BOOST_TEST_MESSAGE(getindent() << "statistics: " << node.num_bv_tests
375  << " " << node.num_leaf_tests);
376  BOOST_TEST_MESSAGE(getindent()
377  << "nb contacts: " << local_result.numContacts());
378  BENCHMARK(node.num_bv_tests);
379  BENCHMARK(node.num_leaf_tests);
380  }
381  BENCHMARK(local_result.numContacts());
382  BENCHMARK(local_result.distance_lower_bound);
383  BENCHMARK(timer.duration().count());
384  BENCHMARK_NEXT();
385 
386  if (local_result.numContacts() > 0) {
387  local_result.getContacts(contacts[i]);
388  std::sort(contacts[i].begin(), contacts[i].end());
389  }
390  }
391  --indent;
392  }
393 
395  BOOST_TEST_MESSAGE(getindent() << "BV: " << str<BV>());
396  ++indent;
397 
398  for (std::size_t i = 0; i < transforms.size(); ++i) {
399  const Transform3f tf1 = transforms[i];
400 
401  timer.start();
402  CollisionResult local_result;
403  MeshCollisionTraversalNode<BV, RelativeTransformationIsIdentity> node(
404  request);
405  node.enable_statistics = enable_statistics;
406 
407  BVH_t* model1_tmp = new BVH_t(*model1);
408  Transform3f tf1_tmp = tf1;
409  BVH_t* model2_tmp = new BVH_t(*model2);
410  Transform3f tf2_tmp = tf2;
411 
412  bool success = initialize(node, *model1_tmp, tf1_tmp, *model2_tmp,
413  tf2_tmp, local_result, true, true);
414  BOOST_REQUIRE(success);
415 
416  collide(&node, request, local_result);
417  delete model1_tmp;
418  delete model2_tmp;
419 
420  timer.stop();
421  BENCHMARK(str<BV>());
422  BENCHMARK(2);
423  BENCHMARK(splitMethod);
424  if (enable_statistics) {
425  BOOST_TEST_MESSAGE(getindent() << "statistics: " << node.num_bv_tests
426  << " " << node.num_leaf_tests);
427  BOOST_TEST_MESSAGE(getindent()
428  << "nb contacts: " << local_result.numContacts());
429  BENCHMARK(node.num_bv_tests);
430  BENCHMARK(node.num_leaf_tests);
431  }
432  BENCHMARK(local_result.numContacts());
433  BENCHMARK(local_result.distance_lower_bound);
434  BENCHMARK(timer.duration().count());
435  BENCHMARK_NEXT();
436 
437  if (local_result.numContacts() > 0) {
438  local_result.getContacts(contacts[i + N]);
439  std::sort(contacts[i + N].begin(), contacts[i + N].end());
440  }
441  }
442  --indent;
443  }
444 
446  BOOST_TEST_MESSAGE(getindent()
447  << "BV: " << str<BV>() << " oriented non-recursive");
448  ++indent;
449 
450  for (std::size_t i = 0; i < transforms.size(); ++i) {
451  timer.start();
452  const Transform3f tf1 = transforms[i];
453 
454  CollisionResult local_result;
455  MeshCollisionTraversalNode<BV, 0> node(request);
456  node.enable_statistics = enable_statistics;
457 
458  bool success =
459  initialize(node, *model1, tf1, *model2, tf2, local_result);
460  BOOST_REQUIRE(success);
461 
462  collide(&node, request, local_result, NULL, false);
463 
464  timer.stop();
465  BENCHMARK(str<BV>());
466  BENCHMARK(0);
467  BENCHMARK(splitMethod);
468  if (enable_statistics) {
469  BOOST_TEST_MESSAGE(getindent() << "statistics: " << node.num_bv_tests
470  << " " << node.num_leaf_tests);
471  BOOST_TEST_MESSAGE(getindent()
472  << "nb contacts: " << local_result.numContacts());
473  BENCHMARK(node.num_bv_tests);
474  BENCHMARK(node.num_leaf_tests);
475  }
476  BENCHMARK(local_result.numContacts());
477  BENCHMARK(local_result.distance_lower_bound);
478  BENCHMARK(timer.duration().count());
479  BENCHMARK_NEXT();
480 
481  if (local_result.numContacts() > 0) {
482  local_result.getContacts(contacts[i + 2 * N]);
483  std::sort(contacts[i + 2 * N].begin(), contacts[i + 2 * N].end());
484  }
485  }
486  --indent;
487  }
488  }
489 
490  void check_contacts(std::size_t i0, std::size_t i1, bool warn) {
491  for (std::size_t i = i0; i < i1; ++i) {
492  Contacts_t in_ref_but_not_in_i;
493  std::set_difference(
494  contacts_ref[i].begin(), contacts_ref[i].end(), contacts[i].begin(),
495  contacts[i].end(),
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) {
499  if (warn) {
500  BOOST_WARN_MESSAGE(
501  false, "Missed contacts: " << in_ref_but_not_in_i[j].b1 << ", "
502  << in_ref_but_not_in_i[j].b2);
503  } else {
504  BOOST_CHECK_MESSAGE(
505  false, "Missed contacts: " << in_ref_but_not_in_i[j].b1 << ", "
506  << in_ref_but_not_in_i[j].b2);
507  }
508  }
509  }
510 
511  Contacts_t in_i_but_not_in_ref;
512  std::set_difference(
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()));
516 
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) {
519  if (warn) {
520  BOOST_WARN_MESSAGE(
521  false, "False contacts: " << in_i_but_not_in_ref[j].b1 << ", "
522  << in_i_but_not_in_ref[j].b2);
523  } else {
524  BOOST_CHECK_MESSAGE(
525  false, "False contacts: " << in_i_but_not_in_ref[j].b1 << ", "
526  << in_i_but_not_in_ref[j].b2);
527  }
528  }
529  }
530  }
531  }
532 
533  template <typename BV>
534  void check() {
535  if (benchmark) return;
536  const std::size_t N = transforms.size();
537 
538  BOOST_REQUIRE_EQUAL(contacts.size(), 3 * N);
539  BOOST_REQUIRE_EQUAL(contacts.size(), contacts_ref.size());
540 
542  BOOST_TEST_MESSAGE(getindent() << "BV: " << str<BV>() << " oriented");
543  ++indent;
544  check_contacts(0, N, false);
545  --indent;
546  }
548  BOOST_TEST_MESSAGE(getindent() << "BV: " << str<BV>());
549  ++indent;
550  check_contacts(N, 2 * N, true);
551  --indent;
552  }
554  BOOST_TEST_MESSAGE(getindent()
555  << "BV: " << str<BV>() << " oriented non-recursive");
556  ++indent;
557  check_contacts(2 * N, 3 * N, false);
558  --indent;
559  }
560  }
561 
562  template <typename BV>
564  for (std::size_t i = 0; i < splitMethods.size(); ++i) {
565  BOOST_TEST_MESSAGE(getindent() << "splitMethod: " << splitMethods[i]);
566  ++indent;
567  query<BV>(transforms, splitMethods[i], request,
568  (isInit ? contacts : contacts_ref));
569  if (isInit) check<BV>();
570  isInit = true;
571  --indent;
572  }
573  }
574 };
575 
576 // This test
577 // 1. load two objects "env.obj" and "rob.obj" from directory
578 // fcl_resources,
579 // 2. generates n random transformation and for each of them denote tf,
580 // 2.1 performs a collision test where object 1 is in pose tf. All
581 // the contacts are stored in vector global_pairs.
582 // 2.2 performs a series of collision tests with the same object and
583 // the same poses using various methods and various types of bounding
584 // volumes. Each time the contacts are stored in vector global_pairs_now.
585 //
586 // The methods used to test collision are
587 // - collide_Test that calls function collide with tf for object1 pose and
588 // identity for the second object pose,
589 // - collide_Test2 that moves all vertices of object1 in pose tf and that
590 // calls function collide with identity for both object poses,
591 //
593  std::vector<Transform3f> transforms;
594  FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
595 #ifndef NDEBUG // if debug mode
596  std::size_t n = 1;
597 #else
598  std::size_t n = 10;
599 #endif
600  n = getNbRun(utf::master_test_suite().argc, utf::master_test_suite().argv, n);
601 
602  generateRandomTransforms(extents, transforms, n);
603 
604  Eigen::IOFormat f(Eigen::FullPrecision, 0, ", ", ",", "", "", "(", ")");
605  for (std::size_t i = 0; i < transforms.size(); ++i) {
606  BOOST_TEST_MESSAGE(
607  "q" << i << "=" << transforms[i].getTranslation().format(f) << "+"
608  << transforms[i].getQuatRotation().coeffs().format(f));
609  }
610 
611  // Request all contacts and check that all methods give the same result.
612  mesh_mesh_run_test runner(
613  transforms, CollisionRequest(CONTACT, (size_t)num_max_contacts));
614  runner.enable_statistics = true;
615  boost::mpl::for_each<BVs_t, wrap<boost::mpl::placeholders::_1> >(runner);
616 }
617 
618 BOOST_AUTO_TEST_CASE(mesh_mesh_benchmark) {
619  std::vector<Transform3f> transforms;
620  FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
621 #ifndef NDEBUG
622  std::size_t n = 0;
623 #else
624  std::size_t n = 10;
625 #endif
626  n = getNbRun(utf::master_test_suite().argc, utf::master_test_suite().argv, n);
627 
628  generateRandomTransforms(extents, transforms, n);
629 
630  Eigen::IOFormat f(Eigen::FullPrecision, 0, ", ", ",", "", "", "(", ")");
631  for (std::size_t i = 0; i < transforms.size(); ++i) {
632  BOOST_TEST_MESSAGE(
633  "q" << i << "=" << transforms[i].getTranslation().format(f) << "+"
634  << transforms[i].getQuatRotation().coeffs().format(f));
635  }
636 
637  // Request all contacts and check that all methods give the same result.
638  typedef boost::mpl::vector<OBB, RSS, AABB, KDOP<24>, KDOP<18>, KDOP<16>, kIOS,
639  OBBRSS>
640  BVs_t;
641 
642  std::ofstream ofs("./collision.benchmark.csv", std::ofstream::out);
643  bench_stream = &ofs;
644 
645  // without lower bound.
646  mesh_mesh_run_test runner1(transforms, CollisionRequest());
647  runner1.enable_statistics = false;
648  runner1.benchmark = true;
649  boost::mpl::for_each<BVs_t, wrap<boost::mpl::placeholders::_1> >(runner1);
650 
651  // with lower bound.
652  mesh_mesh_run_test runner2(transforms,
654  runner2.enable_statistics = false;
655  runner2.benchmark = true;
656  boost::mpl::for_each<BVs_t, wrap<boost::mpl::placeholders::_1> >(runner2);
657 
658  bench_stream = NULL;
659  ofs.close();
660 }
hpp::fcl::OBB::axes
Matrix3f axes
Orientation of OBB. axis[i] is the ith column of the orientation matrix for the box; it is also the i...
Definition: include/hpp/fcl/BV/OBB.h:58
mesh_mesh_run_test::check_contacts
void check_contacts(std::size_t i0, std::size_t i1, bool warn)
Definition: test/collision.cpp:490
hpp::fcl::OBB::extent
Vec3f extent
Half dimensions of OBB.
Definition: include/hpp/fcl/BV/OBB.h:64
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
hpp::fcl::AABB::overlap
bool overlap(const AABB &other) const
Check whether two AABB are overlap.
Definition: BV/AABB.h:110
splitMethods
std::vector< SplitMethodType > splitMethods
Definition: test/collision.cpp:242
hpp::fcl::Timer::stop
void stop()
Definition: timings.h:77
hpp::fcl::loadPolyhedronFromResource
void loadPolyhedronFromResource(const std::string &resource_path, const fcl::Vec3f &scale, const shared_ptr< BVHModel< BoundingVolume > > &polyhedron)
Read a mesh file and convert it to a polyhedral mesh.
Definition: assimp.h:118
hpp::fcl::Timer::start
void start()
Definition: timings.h:66
mesh_mesh_run_test::benchmark
bool benchmark
Definition: test/collision.cpp:295
mesh_mesh_run_test::indent
int indent
Definition: test/collision.cpp:300
collision_manager.sphere
sphere
Definition: collision_manager.py:4
mesh_mesh_run_test::operator()
void operator()(wrap< BV >)
Definition: test/collision.cpp:563
mesh_mesh_run_test::enable_statistics
bool enable_statistics
Definition: test/collision.cpp:295
gjk.tf1
tuple tf1
Definition: test/scripts/gjk.py:27
hpp::fcl::Sphere
Center at zero point sphere.
Definition: shape/geometric_shapes.h:196
hpp::fcl::AABB::min_
Vec3f min_
The min point in the AABB.
Definition: BV/AABB.h:57
hpp::fcl::CONTACT
@ CONTACT
Definition: collision_data.h:229
hpp::fcl::kIOS
A class describing the kIOS collision structure, which is a set of spheres.
Definition: kIOS.h:53
BV_STR_SPECIALIZATION
#define BV_STR_SPECIALIZATION(bv)
Definition: test/collision.cpp:245
bench_stream
std::ostream * bench_stream
Definition: test/collision.cpp:220
initialize
bool initialize(MeshCollisionTraversalNode< BV, RelativeTransformationIsIdentity > &node, BVHModel< BV > &model1, Transform3f &tf1, BVHModel< BV > &model2, Transform3f &tf2, CollisionResult &result, bool use_refit=false, bool refit_bottomup=false)
Initialize traversal node for collision between two meshes, given the current transforms.
Definition: traversal_node_setup.h:423
hpp::fcl::CollisionResult::numContacts
size_t numContacts() const
number of contacts found
Definition: collision_data.h:342
utility.h
hpp::fcl::AABB::max_
Vec3f max_
The max point in the AABB.
Definition: BV/AABB.h:59
hpp::fcl::computeBV
void computeBV(const S &s, const Transform3f &tf, BV &bv)
calculate a bounding volume for a shape in a specific configuration
Definition: geometric_shapes_utility.h:74
hpp::fcl::distance
HPP_FCL_DLLAPI FCL_REAL distance(const Matrix3f &R0, const Vec3f &T0, const kIOS &b1, const kIOS &b2, Vec3f *P=NULL, Vec3f *Q=NULL)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS.cpp:181
hpp::fcl::Timer
This class mimics the way "boost/timer/timer.hpp" operates while using the modern std::chrono library...
Definition: timings.h:32
hpp::fcl::SPLIT_METHOD_MEDIAN
@ SPLIT_METHOD_MEDIAN
Definition: internal/BV_splitter.h:53
bs_hp
bool bs_hp
Definition: test/collision.cpp:222
extents
FCL_REAL extents[6]
Definition: test/geometric_shapes.cpp:51
hpp::fcl::Cylinder
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: shape/geometric_shapes.h:501
timings.h
narrowphase.h
BENCHMARK
#define BENCHMARK(stream)
Definition: test/collision.cpp:223
BENCHMARK_HEADER
#define BENCHMARK_HEADER(stream)
Definition: test/collision.cpp:228
mesh_mesh_run_test
Definition: test/collision.cpp:283
hpp::fcl::collide
HPP_FCL_DLLAPI std::size_t collide(const CollisionObject *o1, const CollisionObject *o2, const CollisionRequest &request, CollisionResult &result)
Main collision interface: given two collision objects, and the requirements for contacts,...
Definition: src/collision.cpp:63
Recursive
@ Recursive
Definition: test/collision.cpp:271
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
bs_nl
bool bs_nl
Definition: test/collision.cpp:221
hpp::fcl::OBB::overlap
bool overlap(const OBB &other) const
Definition: OBB.cpp:396
hpp::fcl::GJKSolver
collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the ...
Definition: narrowphase.h:54
hpp::fcl::SPLIT_METHOD_MEAN
@ SPLIT_METHOD_MEAN
Definition: internal/BV_splitter.h:52
mesh_mesh_run_test::transforms
const std::vector< Transform3f > & transforms
Definition: test/collision.cpp:293
NonOriented
@ NonOriented
Definition: test/collision.cpp:270
hpp::fcl::SplitMethodType
SplitMethodType
Three types of split algorithms are provided in FCL as default.
Definition: internal/BV_splitter.h:51
hpp::fcl::convertBV
static void convertBV(const BV1 &bv1, const Transform3f &tf1, BV2 &bv2)
Convert a bounding volume of type BV1 in configuration tf1 to bounding volume of type BV2 in identity...
Definition: BV.h:277
hpp::fcl::constructBox
HPP_FCL_DLLAPI void constructBox(const AABB &bv, Box &box, Transform3f &tf)
construct a box shape (with a configuration) from a given bounding volume
Definition: geometric_shapes_utility.cpp:911
mesh_mesh_run_test::contacts
std::vector< Contacts_t > contacts
Definition: test/collision.cpp:296
traits
Definition: benchmark.cpp:54
BV.h
hpp::fcl::CollisionRequest
request to the collision algorithm
Definition: collision_data.h:235
wrap
Definition: test/collision.cpp:262
mesh_mesh_run_test::query
void query(const std::vector< Transform3f > &transforms, SplitMethodType splitMethod, const CollisionRequest request, std::vector< Contacts_t > &contacts)
Definition: test/collision.cpp:317
Contacts_t
std::vector< Contact > Contacts_t
Definition: test/collision.cpp:239
hpp::fcl::OBB
Oriented bounding box class.
Definition: include/hpp/fcl/BV/OBB.h:52
hpp::fcl::getNbRun
std::size_t getNbRun(const int &argc, char const *const *argv, std::size_t defaultValue)
Get the argument –nb-run from argv.
Definition: utility.cpp:382
t
tuple t
gjk.tf2
tuple tf2
Definition: test/scripts/gjk.py:36
hpp::fcl::CollisionResult
collision result
Definition: collision_data.h:302
hpp::fcl::SPLIT_METHOD_BV_CENTER
@ SPLIT_METHOD_BV_CENTER
Definition: internal/BV_splitter.h:54
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(OBB_Box_test)
Definition: test/collision.cpp:67
base_traits
Definition: test/collision.cpp:264
hpp::fcl::Cone
Cone The base of the cone is at and the top is at .
Definition: shape/geometric_shapes.h:414
hpp::fcl::Capsule
Capsule It is where is the distance between the point x and the capsule segment AB,...
Definition: shape/geometric_shapes.h:333
hpp::fcl::AABB
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: BV/AABB.h:54
hpp::fcl::generateRandomTransforms
void generateRandomTransforms(FCL_REAL extents[6], std::vector< Transform3f > &transforms, std::size_t n)
Generate n random transforms whose translations are constrained by extents.
Definition: utility.cpp:224
mesh_mesh_run_test::mesh_mesh_run_test
mesh_mesh_run_test(const std::vector< Transform3f > &_transforms, const CollisionRequest _request)
Definition: test/collision.cpp:284
hpp::fcl
Definition: broadphase_bruteforce.h:45
hpp::fcl::OBB::To
Vec3f To
Center of OBB.
Definition: include/hpp/fcl/BV/OBB.h:61
Oriented
@ Oriented
Definition: test/collision.cpp:269
hpp::fcl::RSS
A class for rectangle sphere-swept bounding volume.
Definition: BV/RSS.h:53
mesh_mesh_run_test::getindent
const char * getindent()
Definition: test/collision.cpp:302
mesh_mesh_run_test::check
void check()
Definition: test/collision.cpp:534
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
hpp::fcl::BVHModel
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH/BVH_model.h:273
hpp::fcl::BVSplitter
A class describing the split rule that splits each BV node.
Definition: BVH/BVH_model.h:59
traversal_node_bvhs.h
BV_splitter.h
BENCHMARK_NEXT
#define BENCHMARK_NEXT()
Definition: test/collision.cpp:232
mesh_mesh_run_test::contacts_ref
std::vector< Contacts_t > contacts_ref
Definition: test/collision.cpp:297
assimp.h
num_max_contacts
int num_max_contacts
Definition: test/collision.cpp:65
BVs_t
boost::mpl::vector< OBB, RSS, KDOP< 24 >, KDOP< 18 >, KDOP< 16 >, kIOS, OBBRSS > BVs_t
Definition: test/collision.cpp:241
mesh_mesh_run_test::request
const CollisionRequest request
Definition: test/collision.cpp:294
geometric_shapes.h
hpp::fcl::CollisionResult::getContacts
void getContacts(std::vector< Contact > &contacts_) const
get all the contacts
Definition: collision_data.h:369
traversal_node_setup.h
mesh_mesh_run_test::isInit
bool isInit
Definition: test/collision.cpp:298
NonRecursive
@ NonRecursive
Definition: test/collision.cpp:272
hpp::fcl::OBBRSS
Class merging the OBB and RSS, can handle collision and distance simultaneously.
Definition: BV/OBBRSS.h:54
str
const char * str()
Definition: doxygen_xml_parser.py:882
hpp::fcl::DISTANCE_LOWER_BOUND
@ DISTANCE_LOWER_BOUND
Definition: collision_data.h:230
hpp::fcl::translate
static AABB translate(const AABB &aabb, const Vec3f &t)
translate the center of AABB by t
Definition: BV/AABB.h:226
hpp::fcl::GJKSolver::shapeIntersect
bool shapeIntersect(const S1 &s1, const Transform3f &tf1, const S2 &s2, const Transform3f &tf2, FCL_REAL &distance_lower_bound, bool enable_penetration, Vec3f *contact_points, Vec3f *normal) const
intersection checking between two shapes
Definition: narrowphase.h:104
hpp::fcl::CollisionResult::distance_lower_bound
FCL_REAL distance_lower_bound
Definition: collision_data.h:312
hpp::fcl::KDOP
KDOP class describes the KDOP collision structures. K is set as the template parameter,...
Definition: kDOP.h:92
collision.h
hpp::fcl::Box
Center at zero point, axis aligned box.
Definition: shape/geometric_shapes.h:125


hpp-fcl
Author(s):
autogenerated on Fri Aug 2 2024 02:45:13