broadphase_collision_1.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-2016, Open Source Robotics Foundation
6  * Copyright (c) 2022, INRIA
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Open Source Robotics Foundation nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  */
38 #define BOOST_TEST_MODULE BROADPHASE_COLLISION_1
39 #include <boost/test/included/unit_test.hpp>
40 
41 #include "hpp/fcl/broadphase/broadphase_bruteforce.h"
42 #include "hpp/fcl/broadphase/broadphase_spatialhash.h"
43 #include "hpp/fcl/broadphase/broadphase_SaP.h"
44 #include "hpp/fcl/broadphase/broadphase_SSaP.h"
45 #include "hpp/fcl/broadphase/broadphase_interval_tree.h"
46 #include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h"
47 #include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h"
48 #include "hpp/fcl/broadphase/default_broadphase_callbacks.h"
49 #include "hpp/fcl/broadphase/detail/sparse_hash_table.h"
50 #include "hpp/fcl/broadphase/detail/spatial_hash.h"
51 #include "utility.h"
52 
53 #include <boost/math/constants/constants.hpp>
54 
55 #if USE_GOOGLEHASH
56 #include <sparsehash/sparse_hash_map>
57 #include <sparsehash/dense_hash_map>
58 #include <hash_map>
59 #endif
60 
61 #include <iostream>
62 #include <iomanip>
63 
64 using namespace hpp::fcl;
65 
68 void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size,
69  bool verbose = false);
70 
72 void broad_phase_update_collision_test(FCL_REAL env_scale, std::size_t env_size,
73  std::size_t query_size,
74  std::size_t num_max_contacts = 1,
75  bool exhaustive = false,
76  bool use_mesh = false);
77 
78 #if USE_GOOGLEHASH
79 template <typename U, typename V>
80 struct GoogleSparseHashTable
81  : public google::sparse_hash_map<U, V, std::tr1::hash<size_t>,
82  std::equal_to<size_t>> {};
83 
84 template <typename U, typename V>
85 struct GoogleDenseHashTable
86  : public google::dense_hash_map<U, V, std::tr1::hash<size_t>,
87  std::equal_to<size_t>> {
88  GoogleDenseHashTable()
89  : google::dense_hash_map<U, V, std::tr1::hash<size_t>,
90  std::equal_to<size_t>>() {
91  this->set_empty_key(nullptr);
92  }
93 };
94 #endif
95 
98 BOOST_AUTO_TEST_CASE(test_broad_phase_dont_duplicate_check) {
99 #ifdef NDEBUG
100  broad_phase_duplicate_check_test(2000, 1000);
101 #else
102  broad_phase_duplicate_check_test(2000, 100);
103 #endif
104 }
105 
107 BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_update_collision_binary) {
108 #ifdef NDEBUG
109  broad_phase_update_collision_test(2000, 100, 1000, 1, false);
110  broad_phase_update_collision_test(2000, 1000, 1000, 1, false);
111 #else
112  broad_phase_update_collision_test(2000, 10, 100, 1, false);
113  broad_phase_update_collision_test(2000, 100, 100, 1, false);
114 #endif
115 }
116 
118 BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_update_collision) {
119 #ifdef NDEBUG
120  broad_phase_update_collision_test(2000, 100, 1000, 10, false);
121  broad_phase_update_collision_test(2000, 1000, 1000, 10, false);
122 #else
123  broad_phase_update_collision_test(2000, 10, 100, 10, false);
124  broad_phase_update_collision_test(2000, 100, 100, 10, false);
125 #endif
126 }
127 
129 BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_update_collision_exhaustive) {
130 #ifdef NDEBUG
131  broad_phase_update_collision_test(2000, 100, 1000, 1, true);
132  broad_phase_update_collision_test(2000, 1000, 1000, 1, true);
133 #else
134  broad_phase_update_collision_test(2000, 10, 100, 1, true);
135  broad_phase_update_collision_test(2000, 100, 100, 1, true);
136 #endif
137 }
138 
140 BOOST_AUTO_TEST_CASE(
141  test_core_mesh_bf_broad_phase_update_collision_mesh_binary) {
142 #ifdef NDEBUG
143  broad_phase_update_collision_test(2000, 100, 1000, 1, false, true);
144  broad_phase_update_collision_test(2000, 1000, 1000, 1, false, true);
145 #else
146  broad_phase_update_collision_test(2000, 2, 4, 1, false, true);
147  broad_phase_update_collision_test(2000, 4, 4, 1, false, true);
148 #endif
149 }
150 
152 BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_update_collision_mesh) {
153 #ifdef NDEBUG
154  broad_phase_update_collision_test(2000, 100, 1000, 10, false, true);
155  broad_phase_update_collision_test(2000, 1000, 1000, 10, false, true);
156 #else
157  broad_phase_update_collision_test(200, 2, 4, 10, false, true);
158  broad_phase_update_collision_test(200, 4, 4, 10, false, true);
159 #endif
160 }
161 
163 BOOST_AUTO_TEST_CASE(
164  test_core_mesh_bf_broad_phase_update_collision_mesh_exhaustive) {
165 #ifdef NDEBUG
166  broad_phase_update_collision_test(2000, 100, 1000, 1, true, true);
167  broad_phase_update_collision_test(2000, 1000, 1000, 1, true, true);
168 #else
169  broad_phase_update_collision_test(2000, 2, 4, 1, true, true);
170  broad_phase_update_collision_test(2000, 4, 4, 1, true, true);
171 #endif
172 }
173 
174 //==============================================================================
175 struct CollisionDataForUniquenessChecking {
176  std::set<std::pair<CollisionObject*, CollisionObject*>> checkedPairs;
177 
178  bool checkUniquenessAndAddPair(CollisionObject* o1, CollisionObject* o2) {
179  auto search = checkedPairs.find(std::make_pair(o1, o2));
180 
181  if (search != checkedPairs.end()) return false;
182 
183  checkedPairs.emplace(o1, o2);
184 
185  return true;
186  }
187 };
188 
189 //==============================================================================
190 struct CollisionFunctionForUniquenessChecking : CollisionCallBackBase {
191  bool collide(CollisionObject* o1, CollisionObject* o2) {
192  BOOST_CHECK(data.checkUniquenessAndAddPair(o1, o2));
193  return false;
194  }
195 
196  CollisionDataForUniquenessChecking data;
197 };
198 
199 //==============================================================================
200 void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size,
201  bool verbose) {
202  std::vector<TStruct> ts;
203  std::vector<BenchTimer> timers;
204 
205  std::vector<CollisionObject*> env;
206  generateEnvironments(env, env_scale, env_size);
207 
208  std::vector<BroadPhaseCollisionManager*> managers;
209  managers.push_back(new NaiveCollisionManager());
210  managers.push_back(new SSaPCollisionManager());
211  managers.push_back(new SaPCollisionManager());
212  managers.push_back(new IntervalTreeCollisionManager());
213  Vec3f lower_limit, upper_limit;
214  SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit);
215  FCL_REAL cell_size =
216  std::min(std::min((upper_limit[0] - lower_limit[0]) / 20,
217  (upper_limit[1] - lower_limit[1]) / 20),
218  (upper_limit[2] - lower_limit[2]) / 20);
219  managers.push_back(
220  new SpatialHashingCollisionManager<
221  detail::SparseHashTable<AABB, CollisionObject*, detail::SpatialHash>>(
222  cell_size, lower_limit, upper_limit));
223 #if USE_GOOGLEHASH
224  managers.push_back(
225  new SpatialHashingCollisionManager<detail::SparseHashTable<
226  AABB, CollisionObject*, detail::SpatialHash, GoogleSparseHashTable>>(
227  cell_size, lower_limit, upper_limit));
228  managers.push_back(
229  new SpatialHashingCollisionManager<detail::SparseHashTable<
230  AABB, CollisionObject*, detail::SpatialHash, GoogleDenseHashTable>>(
231  cell_size, lower_limit, upper_limit));
232 #endif
233  managers.push_back(new DynamicAABBTreeCollisionManager());
234  managers.push_back(new DynamicAABBTreeArrayCollisionManager());
235 
236  {
237  DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager();
238  m->tree_init_level = 2;
239  managers.push_back(m);
240  }
241 
242  {
243  DynamicAABBTreeArrayCollisionManager* m =
244  new DynamicAABBTreeArrayCollisionManager();
245  m->tree_init_level = 2;
246  managers.push_back(m);
247  }
248 
249  ts.resize(managers.size());
250  timers.resize(managers.size());
251 
252  for (size_t i = 0; i < managers.size(); ++i) {
253  timers[i].start();
254  managers[i]->registerObjects(env);
255  timers[i].stop();
256  ts[i].push_back(timers[i].getElapsedTime());
257  }
258 
259  for (size_t i = 0; i < managers.size(); ++i) {
260  timers[i].start();
261  managers[i]->setup();
262  timers[i].stop();
263  ts[i].push_back(timers[i].getElapsedTime());
264  }
265 
266  // update the environment
267  FCL_REAL delta_angle_max =
268  10 / 360.0 * 2 * boost::math::constants::pi<FCL_REAL>();
269  FCL_REAL delta_trans_max = 0.01 * env_scale;
270  for (size_t i = 0; i < env.size(); ++i) {
271  FCL_REAL rand_angle_x =
272  2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max;
273  FCL_REAL rand_trans_x =
274  2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max;
275  FCL_REAL rand_angle_y =
276  2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max;
277  FCL_REAL rand_trans_y =
278  2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max;
279  FCL_REAL rand_angle_z =
280  2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max;
281  FCL_REAL rand_trans_z =
282  2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max;
283 
284  Matrix3f dR(Eigen::AngleAxisd(rand_angle_x, Vec3f::UnitX()) *
285  Eigen::AngleAxisd(rand_angle_y, Vec3f::UnitY()) *
286  Eigen::AngleAxisd(rand_angle_z, Vec3f::UnitZ()));
287  Vec3f dT(rand_trans_x, rand_trans_y, rand_trans_z);
288 
289  Matrix3f R = env[i]->getRotation();
290  Vec3f T = env[i]->getTranslation();
291  env[i]->setTransform(dR * R, dR * T + dT);
292  env[i]->computeAABB();
293  }
294 
295  for (size_t i = 0; i < managers.size(); ++i) {
296  timers[i].start();
297  managers[i]->update();
298  timers[i].stop();
299  ts[i].push_back(timers[i].getElapsedTime());
300  }
301 
302  std::vector<CollisionDataForUniquenessChecking> self_data(managers.size());
303 
304  for (size_t i = 0; i < managers.size(); ++i) {
305  CollisionFunctionForUniquenessChecking callback;
306  timers[i].start();
307  managers[i]->collide(&callback);
308  timers[i].stop();
309  ts[i].push_back(timers[i].getElapsedTime());
310  }
311 
312  for (auto obj : env) delete obj;
313 
314  if (!verbose) return;
315 
316  std::cout.setf(std::ios_base::left, std::ios_base::adjustfield);
317  int w = 7;
318 
319  std::cout << "collision timing summary" << std::endl;
320  std::cout << env_size << " objs" << std::endl;
321  std::cout << "register time" << std::endl;
322  for (size_t i = 0; i < ts.size(); ++i)
323  std::cout << std::setw(w) << ts[i].records[0] << " ";
324  std::cout << std::endl;
325 
326  std::cout << "setup time" << std::endl;
327  for (size_t i = 0; i < ts.size(); ++i)
328  std::cout << std::setw(w) << ts[i].records[1] << " ";
329  std::cout << std::endl;
330 
331  std::cout << "update time" << std::endl;
332  for (size_t i = 0; i < ts.size(); ++i)
333  std::cout << std::setw(w) << ts[i].records[2] << " ";
334  std::cout << std::endl;
335 
336  std::cout << "self collision time" << std::endl;
337  for (size_t i = 0; i < ts.size(); ++i)
338  std::cout << std::setw(w) << ts[i].records[3] << " ";
339  std::cout << std::endl;
340 
341  std::cout << "collision time" << std::endl;
342  for (size_t i = 0; i < ts.size(); ++i) {
343  FCL_REAL tmp = 0;
344  for (size_t j = 4; j < ts[i].records.size(); ++j) tmp += ts[i].records[j];
345  std::cout << std::setw(w) << tmp << " ";
346  }
347  std::cout << std::endl;
348 
349  std::cout << "overall time" << std::endl;
350  for (size_t i = 0; i < ts.size(); ++i)
351  std::cout << std::setw(w) << ts[i].overall_time << " ";
352  std::cout << std::endl;
353  std::cout << std::endl;
354 }
355 
356 void broad_phase_update_collision_test(FCL_REAL env_scale, std::size_t env_size,
357  std::size_t query_size,
358  std::size_t num_max_contacts,
359  bool exhaustive, bool use_mesh) {
360  std::vector<TStruct> ts;
361  std::vector<BenchTimer> timers;
362 
363  std::vector<CollisionObject*> env;
364  if (use_mesh)
365  generateEnvironmentsMesh(env, env_scale, env_size);
366  else
367  generateEnvironments(env, env_scale, env_size);
368 
369  std::vector<CollisionObject*> query;
370  if (use_mesh)
371  generateEnvironmentsMesh(query, env_scale, query_size);
372  else
373  generateEnvironments(query, env_scale, query_size);
374 
375  std::vector<BroadPhaseCollisionManager*> managers;
376 
377  managers.push_back(new NaiveCollisionManager());
378  managers.push_back(new SSaPCollisionManager());
379 
380  managers.push_back(new SaPCollisionManager());
381  managers.push_back(new IntervalTreeCollisionManager());
382 
383  Vec3f lower_limit, upper_limit;
384  SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit);
385  FCL_REAL cell_size =
386  std::min(std::min((upper_limit[0] - lower_limit[0]) / 20,
387  (upper_limit[1] - lower_limit[1]) / 20),
388  (upper_limit[2] - lower_limit[2]) / 20);
389  // managers.push_back(new SpatialHashingCollisionManager(cell_size,
390  // lower_limit, upper_limit));
391  managers.push_back(
392  new SpatialHashingCollisionManager<
393  detail::SparseHashTable<AABB, CollisionObject*, detail::SpatialHash>>(
394  cell_size, lower_limit, upper_limit));
395 #if USE_GOOGLEHASH
396  managers.push_back(
397  new SpatialHashingCollisionManager<detail::SparseHashTable<
398  AABB, CollisionObject*, detail::SpatialHash, GoogleSparseHashTable>>(
399  cell_size, lower_limit, upper_limit));
400  managers.push_back(
401  new SpatialHashingCollisionManager<detail::SparseHashTable<
402  AABB, CollisionObject*, detail::SpatialHash, GoogleDenseHashTable>>(
403  cell_size, lower_limit, upper_limit));
404 #endif
405  managers.push_back(new DynamicAABBTreeCollisionManager());
406  managers.push_back(new DynamicAABBTreeArrayCollisionManager());
407 
408  {
409  DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager();
410  m->tree_init_level = 2;
411  managers.push_back(m);
412  }
413 
414  {
415  DynamicAABBTreeArrayCollisionManager* m =
416  new DynamicAABBTreeArrayCollisionManager();
417  m->tree_init_level = 2;
418  managers.push_back(m);
419  }
420 
421  ts.resize(managers.size());
422  timers.resize(managers.size());
423 
424  for (size_t i = 0; i < managers.size(); ++i) {
425  timers[i].start();
426  managers[i]->registerObjects(env);
427  timers[i].stop();
428  ts[i].push_back(timers[i].getElapsedTime());
429  }
430 
431  for (size_t i = 0; i < managers.size(); ++i) {
432  timers[i].start();
433  managers[i]->setup();
434  timers[i].stop();
435  ts[i].push_back(timers[i].getElapsedTime());
436  }
437 
438  // update the environment
439  FCL_REAL delta_angle_max =
440  10 / 360.0 * 2 * boost::math::constants::pi<FCL_REAL>();
441  FCL_REAL delta_trans_max = 0.01 * env_scale;
442  for (size_t i = 0; i < env.size(); ++i) {
443  FCL_REAL rand_angle_x =
444  2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max;
445  FCL_REAL rand_trans_x =
446  2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max;
447  FCL_REAL rand_angle_y =
448  2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max;
449  FCL_REAL rand_trans_y =
450  2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max;
451  FCL_REAL rand_angle_z =
452  2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max;
453  FCL_REAL rand_trans_z =
454  2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max;
455 
456  Matrix3f dR(Eigen::AngleAxisd(rand_angle_x, Vec3f::UnitX()) *
457  Eigen::AngleAxisd(rand_angle_y, Vec3f::UnitY()) *
458  Eigen::AngleAxisd(rand_angle_z, Vec3f::UnitZ()));
459  Vec3f dT(rand_trans_x, rand_trans_y, rand_trans_z);
460 
461  Matrix3f R = env[i]->getRotation();
462  Vec3f T = env[i]->getTranslation();
463  env[i]->setTransform(dR * R, dR * T + dT);
464  env[i]->computeAABB();
465  }
466 
467  for (size_t i = 0; i < managers.size(); ++i) {
468  timers[i].start();
469  managers[i]->update();
470  timers[i].stop();
471  ts[i].push_back(timers[i].getElapsedTime());
472  }
473 
474  std::vector<CollisionData> self_data(managers.size());
475  for (size_t i = 0; i < managers.size(); ++i) {
476  if (exhaustive)
477  self_data[i].request.num_max_contacts = 100000;
478  else
479  self_data[i].request.num_max_contacts = num_max_contacts;
480  }
481 
482  for (size_t i = 0; i < managers.size(); ++i) {
483  CollisionCallBackDefault callback;
484  timers[i].start();
485  managers[i]->collide(&callback);
486  timers[i].stop();
487  ts[i].push_back(timers[i].getElapsedTime());
488  }
489 
490  for (size_t i = 0; i < managers.size(); ++i)
491  std::cout << self_data[i].result.numContacts() << " ";
492  std::cout << std::endl;
493 
494  if (exhaustive) {
495  for (size_t i = 1; i < managers.size(); ++i)
496  BOOST_CHECK(self_data[i].result.numContacts() ==
497  self_data[0].result.numContacts());
498  } else {
499  std::vector<bool> self_res(managers.size());
500  for (size_t i = 0; i < self_res.size(); ++i)
501  self_res[i] = (self_data[i].result.numContacts() > 0);
502 
503  for (size_t i = 1; i < self_res.size(); ++i)
504  BOOST_CHECK(self_res[0] == self_res[i]);
505 
506  for (size_t i = 1; i < managers.size(); ++i)
507  BOOST_CHECK(self_data[i].result.numContacts() ==
508  self_data[0].result.numContacts());
509  }
510 
511  for (size_t i = 0; i < query.size(); ++i) {
512  std::vector<CollisionCallBackDefault> query_callbacks(managers.size());
513 
514  for (size_t j = 0; j < query_callbacks.size(); ++j) {
515  if (exhaustive)
516  query_callbacks[j].data.request.num_max_contacts = 100000;
517  else
518  query_callbacks[j].data.request.num_max_contacts = num_max_contacts;
519  }
520 
521  for (size_t j = 0; j < query_callbacks.size(); ++j) {
522  timers[j].start();
523  managers[j]->collide(query[i], &query_callbacks[j]);
524  timers[j].stop();
525  ts[j].push_back(timers[j].getElapsedTime());
526  }
527 
528  // for(size_t j = 0; j < managers.size(); ++j)
529  // std::cout << query_callbacks[j].result.numContacts() << " ";
530  // std::cout << std::endl;
531 
532  if (exhaustive) {
533  for (size_t j = 1; j < managers.size(); ++j)
534  BOOST_CHECK(query_callbacks[j].data.result.numContacts() ==
535  query_callbacks[0].data.result.numContacts());
536  } else {
537  std::vector<bool> query_res(managers.size());
538  for (size_t j = 0; j < query_res.size(); ++j)
539  query_res[j] = (query_callbacks[j].data.result.numContacts() > 0);
540  for (size_t j = 1; j < query_res.size(); ++j)
541  BOOST_CHECK(query_res[0] == query_res[j]);
542 
543  for (size_t j = 1; j < managers.size(); ++j)
544  BOOST_CHECK(query_callbacks[j].data.result.numContacts() ==
545  query_callbacks[0].data.result.numContacts());
546  }
547  }
548 
549  for (size_t i = 0; i < env.size(); ++i) delete env[i];
550  for (size_t i = 0; i < query.size(); ++i) delete query[i];
551 
552  for (size_t i = 0; i < managers.size(); ++i) delete managers[i];
553 
554  std::cout.setf(std::ios_base::left, std::ios_base::adjustfield);
555  int w = 7;
556 
557  std::cout << "collision timing summary" << std::endl;
558  std::cout << env_size << " objs, " << query_size << " queries" << std::endl;
559  std::cout << "register time" << std::endl;
560  for (size_t i = 0; i < ts.size(); ++i)
561  std::cout << std::setw(w) << ts[i].records[0] << " ";
562  std::cout << std::endl;
563 
564  std::cout << "setup time" << std::endl;
565  for (size_t i = 0; i < ts.size(); ++i)
566  std::cout << std::setw(w) << ts[i].records[1] << " ";
567  std::cout << std::endl;
568 
569  std::cout << "update time" << std::endl;
570  for (size_t i = 0; i < ts.size(); ++i)
571  std::cout << std::setw(w) << ts[i].records[2] << " ";
572  std::cout << std::endl;
573 
574  std::cout << "self collision time" << std::endl;
575  for (size_t i = 0; i < ts.size(); ++i)
576  std::cout << std::setw(w) << ts[i].records[3] << " ";
577  std::cout << std::endl;
578 
579  std::cout << "collision time" << std::endl;
580  for (size_t i = 0; i < ts.size(); ++i) {
581  FCL_REAL tmp = 0;
582  for (size_t j = 4; j < ts[i].records.size(); ++j) tmp += ts[i].records[j];
583  std::cout << std::setw(w) << tmp << " ";
584  }
585  std::cout << std::endl;
586 
587  std::cout << "overall time" << std::endl;
588  for (size_t i = 0; i < ts.size(); ++i)
589  std::cout << std::setw(w) << ts[i].overall_time << " ";
590  std::cout << std::endl;
591  std::cout << std::endl;
592 }
verbose
bool verbose
Definition: benchmark.cpp:32
spatial_hash.h
generate_distance_plot.m
float m
Definition: generate_distance_plot.py:6
hpp::fcl::UnitX
const Vec3f UnitX
Definition: utility.cpp:87
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
V
V
collision_manager.callback
callback
Definition: collision_manager.py:27
hpp::fcl::SpatialHashingCollisionManager
spatial hashing collision mananger
Definition: broadphase_spatialhash.h:55
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_broad_phase_dont_duplicate_check)
Definition: broadphase_collision_1.cpp:98
sparse_hash_table.h
CollisionFunctionForUniquenessChecking
Definition: broadphase_collision_1.cpp:190
default_broadphase_callbacks.h
data
data
utility.h
R
R
CollisionDataForUniquenessChecking
Definition: broadphase_collision_1.cpp:175
broadphase_spatialhash.h
hpp::fcl::generateEnvironmentsMesh
void generateEnvironmentsMesh(std::vector< CollisionObject * > &env, FCL_REAL env_scale, std::size_t n)
Definition: utility.cpp:421
hpp::fcl::generateEnvironments
void generateEnvironments(std::vector< CollisionObject * > &env, FCL_REAL env_scale, std::size_t n)
Definition: utility.cpp:390
hpp::fcl::DynamicAABBTreeCollisionManager
Definition: broadphase_dynamic_AABB_tree.h:54
hpp::fcl::detail::SpatialHash
Spatial hash function: hash an AABB to a set of integer values.
Definition: spatial_hash.h:50
CollisionFunctionForUniquenessChecking::data
CollisionDataForUniquenessChecking data
Definition: broadphase_collision_1.cpp:196
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
CollisionDataForUniquenessChecking::checkUniquenessAndAddPair
bool checkUniquenessAndAddPair(CollisionObject *o1, CollisionObject *o2)
Definition: broadphase_collision_1.cpp:178
hpp::fcl::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information
Definition: collision_object.h:215
broadphase_dynamic_AABB_tree_array.h
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
broadphase_SaP.h
hpp::fcl::SaPCollisionManager
Rigorous SAP collision manager.
Definition: broadphase_SaP.h:50
broadphase_interval_tree.h
hpp::fcl
Definition: broadphase_bruteforce.h:45
broad_phase_duplicate_check_test
void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size, bool verbose=false)
make sure if broadphase algorithms doesn't check twice for the same collision object pair
Definition: broadphase_collision_1.cpp:200
hpp::fcl::CollisionCallBackDefault
Default collision callback to check collision between collision objects.
Definition: default_broadphase_callbacks.h:197
hpp::fcl::Matrix3f
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
hpp::fcl::NaiveCollisionManager
Brute force N-body collision manager.
Definition: broadphase_bruteforce.h:48
broadphase_dynamic_AABB_tree.h
broadphase_SSaP.h
hpp::fcl::UnitY
const Vec3f UnitY
Definition: utility.cpp:88
ts
ts
hpp::fcl::UnitZ
const Vec3f UnitZ
Definition: utility.cpp:89
hpp::fcl::SpatialHashingCollisionManager::computeBound
static void computeBound(std::vector< CollisionObject * > &objs, Vec3f &l, Vec3f &u)
compute the bound for the environent
Definition: broadphase_spatialhash-inl.h:502
hpp::fcl::CollisionCallBackBase
Base callback class for collision queries. This class can be supersed by child classes to provide des...
Definition: broadphase_callbacks.h:50
hpp::fcl::DynamicAABBTreeArrayCollisionManager
Definition: broadphase_dynamic_AABB_tree_array.h:55
CollisionDataForUniquenessChecking::checkedPairs
std::set< std::pair< CollisionObject *, CollisionObject * > > checkedPairs
Definition: broadphase_collision_1.cpp:176
num_max_contacts
int num_max_contacts
Definition: test/collision.cpp:65
broad_phase_update_collision_test
void broad_phase_update_collision_test(FCL_REAL env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts=1, bool exhaustive=false, bool use_mesh=false)
test for broad phase update
Definition: broadphase_collision_1.cpp:356
broadphase_bruteforce.h
hpp::fcl::detail::SparseHashTable
A hash table implemented using unordered_map.
Definition: sparse_hash_table.h:62
hpp::fcl::SSaPCollisionManager
Simple SAP collision manager.
Definition: broadphase_SSaP.h:48
CollisionFunctionForUniquenessChecking::collide
bool collide(CollisionObject *o1, CollisionObject *o2)
Collision evaluation between two objects in collision. This callback will cause the broadphase evalua...
Definition: broadphase_collision_1.cpp:191
hpp::fcl::IntervalTreeCollisionManager
Collision manager based on interval tree.
Definition: broadphase_interval_tree.h:51

hpp-fcl
Author(s):
autogenerated on Fri Jan 26 2024 03:46:12