broadphase.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 #define BOOST_TEST_MODULE FCL_BROADPHASE
39 #include <boost/test/included/unit_test.hpp>
40 
41 #include <hpp/fcl/config.hh>
42 #include <hpp/fcl/broadphase/broadphase.h>
43 #include <hpp/fcl/shape/geometric_shape_to_BVH_model.h>
44 #include <hpp/fcl/math/transform.h>
45 #include "utility.h"
46 
47 #if USE_GOOGLEHASH
48 #include <sparsehash/sparse_hash_map>
49 #include <sparsehash/dense_hash_map>
50 #include <hash_map>
51 #endif
52 
53 #include <boost/math/constants/constants.hpp>
54 #include <iostream>
55 #include <iomanip>
56 
57 using namespace hpp::fcl;
58 using namespace hpp::fcl::detail;
59 
62 void generateSelfDistanceEnvironments(std::vector<CollisionObject*>& env,
63  double env_scale, std::size_t n);
64 
67 void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject*>& env,
68  double env_scale, std::size_t n);
69 
71 void broad_phase_distance_test(double env_scale, std::size_t env_size,
72  std::size_t query_size, bool use_mesh = false);
73 
75 void broad_phase_self_distance_test(double env_scale, std::size_t env_size,
76  bool use_mesh = false);
77 
78 FCL_REAL DELTA = 0.01;
79 
80 #if USE_GOOGLEHASH
81 template <typename U, typename V>
82 struct GoogleSparseHashTable
83  : public google::sparse_hash_map<U, V, std::tr1::hash<size_t>,
84  std::equal_to<size_t> > {};
85 
86 template <typename U, typename V>
87 struct GoogleDenseHashTable
88  : public google::dense_hash_map<U, V, std::tr1::hash<size_t>,
89  std::equal_to<size_t> > {
90  GoogleDenseHashTable()
91  : google::dense_hash_map<U, V, std::tr1::hash<size_t>,
92  std::equal_to<size_t> >() {
93  this->set_empty_key(NULL);
94  }
95 };
96 #endif
97 
98 // TODO(jcarpent): fix this test
99 // - test_core_bf_broad_phase_distance
100 
102 BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_distance) {
103 #ifndef NDEBUG
104  broad_phase_distance_test(200, 100, 10);
105  broad_phase_distance_test(200, 1000, 10);
106  broad_phase_distance_test(2000, 100, 10);
107  broad_phase_distance_test(2000, 1000, 10);
108 #else
109  broad_phase_distance_test(200, 100, 100);
110  broad_phase_distance_test(200, 1000, 100);
111  broad_phase_distance_test(2000, 100, 100);
112  broad_phase_distance_test(2000, 1000, 100);
113 #endif
114 }
115 
117 BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_self_distance) {
118  broad_phase_self_distance_test(200, 512);
119  broad_phase_self_distance_test(200, 1000);
120  broad_phase_self_distance_test(200, 5000);
121 }
122 
124 BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_distance_mesh) {
125 #ifndef NDEBUG
126  broad_phase_distance_test(200, 10, 10, true);
127  broad_phase_distance_test(200, 100, 10, true);
128  broad_phase_distance_test(2000, 10, 10, true);
129  broad_phase_distance_test(2000, 100, 10, true);
130 #else
131  broad_phase_distance_test(200, 100, 100, true);
132  broad_phase_distance_test(200, 1000, 100, true);
133  broad_phase_distance_test(2000, 100, 100, true);
134  broad_phase_distance_test(2000, 1000, 100, true);
135 #endif
136 }
137 
139 BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_self_distance_mesh) {
140  broad_phase_self_distance_test(200, 512, true);
141  broad_phase_self_distance_test(200, 1000, true);
142  broad_phase_self_distance_test(200, 5000, true);
143 }
144 
145 void generateSelfDistanceEnvironments(std::vector<CollisionObject*>& env,
146  double env_scale, std::size_t n) {
147  int n_edge = static_cast<int>(std::floor(std::pow(n, 1 / 3.0)));
148 
149  FCL_REAL step_size = env_scale * 2 / n_edge;
150  FCL_REAL delta_size = step_size * 0.05;
151  FCL_REAL single_size = step_size - 2 * delta_size;
152 
153  int i = 0;
154  for (; i < n_edge * n_edge * n_edge / 4; ++i) {
155  int x = i % (n_edge * n_edge);
156  int y = (i - n_edge * n_edge * x) % n_edge;
157  int z = i - n_edge * n_edge * x - n_edge * y;
158 
159  Box* box = new Box(single_size, single_size, single_size);
160  env.push_back(new CollisionObject(
161  shared_ptr<CollisionGeometry>(box),
162  Transform3f(Vec3f(
163  x * step_size + delta_size + 0.5 * single_size - env_scale,
164  y * step_size + delta_size + 0.5 * single_size - env_scale,
165  z * step_size + delta_size + 0.5 * single_size - env_scale))));
166  env.back()->collisionGeometry()->computeLocalAABB();
167  }
168 
169  for (; i < n_edge * n_edge * n_edge / 4; ++i) {
170  int x = i % (n_edge * n_edge);
171  int y = (i - n_edge * n_edge * x) % n_edge;
172  int z = i - n_edge * n_edge * x - n_edge * y;
173 
174  Sphere* sphere = new Sphere(single_size / 2);
175  env.push_back(new CollisionObject(
176  shared_ptr<CollisionGeometry>(sphere),
177  Transform3f(Vec3f(
178  x * step_size + delta_size + 0.5 * single_size - env_scale,
179  y * step_size + delta_size + 0.5 * single_size - env_scale,
180  z * step_size + delta_size + 0.5 * single_size - env_scale))));
181  env.back()->collisionGeometry()->computeLocalAABB();
182  }
183 
184  for (; i < n_edge * n_edge * n_edge / 4; ++i) {
185  int x = i % (n_edge * n_edge);
186  int y = (i - n_edge * n_edge * x) % n_edge;
187  int z = i - n_edge * n_edge * x - n_edge * y;
188 
189  Cylinder* cylinder = new Cylinder(single_size / 2, single_size);
190  env.push_back(new CollisionObject(
191  shared_ptr<CollisionGeometry>(cylinder),
192  Transform3f(Vec3f(
193  x * step_size + delta_size + 0.5 * single_size - env_scale,
194  y * step_size + delta_size + 0.5 * single_size - env_scale,
195  z * step_size + delta_size + 0.5 * single_size - env_scale))));
196  env.back()->collisionGeometry()->computeLocalAABB();
197  }
198 
199  for (; i < n_edge * n_edge * n_edge / 4; ++i) {
200  int x = i % (n_edge * n_edge);
201  int y = (i - n_edge * n_edge * x) % n_edge;
202  int z = i - n_edge * n_edge * x - n_edge * y;
203 
204  Cone* cone = new Cone(single_size / 2, single_size);
205  env.push_back(new CollisionObject(
206  shared_ptr<CollisionGeometry>(cone),
207  Transform3f(Vec3f(
208  x * step_size + delta_size + 0.5 * single_size - env_scale,
209  y * step_size + delta_size + 0.5 * single_size - env_scale,
210  z * step_size + delta_size + 0.5 * single_size - env_scale))));
211  env.back()->collisionGeometry()->computeLocalAABB();
212  }
213 }
214 
215 void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject*>& env,
216  double env_scale, std::size_t n) {
217  int n_edge = static_cast<int>(std::floor(std::pow(n, 1 / 3.0)));
218 
219  FCL_REAL step_size = env_scale * 2 / n_edge;
220  FCL_REAL delta_size = step_size * 0.05;
221  FCL_REAL single_size = step_size - 2 * delta_size;
222 
223  int i = 0;
224  for (; i < n_edge * n_edge * n_edge / 4; ++i) {
225  int x = i % (n_edge * n_edge);
226  int y = (i - n_edge * n_edge * x) % n_edge;
227  int z = i - n_edge * n_edge * x - n_edge * y;
228 
229  Box box(single_size, single_size, single_size);
230  BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
231  generateBVHModel(*model, box, Transform3f());
232  env.push_back(new CollisionObject(
233  shared_ptr<CollisionGeometry>(model),
234  Transform3f(Vec3f(
235  x * step_size + delta_size + 0.5 * single_size - env_scale,
236  y * step_size + delta_size + 0.5 * single_size - env_scale,
237  z * step_size + delta_size + 0.5 * single_size - env_scale))));
238  env.back()->collisionGeometry()->computeLocalAABB();
239  }
240 
241  for (; i < n_edge * n_edge * n_edge / 4; ++i) {
242  int x = i % (n_edge * n_edge);
243  int y = (i - n_edge * n_edge * x) % n_edge;
244  int z = i - n_edge * n_edge * x - n_edge * y;
245 
246  Sphere sphere(single_size / 2);
247  BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
248  generateBVHModel(*model, sphere, Transform3f(), 16, 16);
249  env.push_back(new CollisionObject(
250  shared_ptr<CollisionGeometry>(model),
251  Transform3f(Vec3f(
252  x * step_size + delta_size + 0.5 * single_size - env_scale,
253  y * step_size + delta_size + 0.5 * single_size - env_scale,
254  z * step_size + delta_size + 0.5 * single_size - env_scale))));
255  env.back()->collisionGeometry()->computeLocalAABB();
256  }
257 
258  for (; i < n_edge * n_edge * n_edge / 4; ++i) {
259  int x = i % (n_edge * n_edge);
260  int y = (i - n_edge * n_edge * x) % n_edge;
261  int z = i - n_edge * n_edge * x - n_edge * y;
262 
263  Cylinder cylinder(single_size / 2, single_size);
264  BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
265  generateBVHModel(*model, cylinder, Transform3f(), 16, 16);
266  env.push_back(new CollisionObject(
267  shared_ptr<CollisionGeometry>(model),
268  Transform3f(Vec3f(
269  x * step_size + delta_size + 0.5 * single_size - env_scale,
270  y * step_size + delta_size + 0.5 * single_size - env_scale,
271  z * step_size + delta_size + 0.5 * single_size - env_scale))));
272  env.back()->collisionGeometry()->computeLocalAABB();
273  }
274 
275  for (; i < n_edge * n_edge * n_edge / 4; ++i) {
276  int x = i % (n_edge * n_edge);
277  int y = (i - n_edge * n_edge * x) % n_edge;
278  int z = i - n_edge * n_edge * x - n_edge * y;
279 
280  Cone cone(single_size / 2, single_size);
281  BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
282  generateBVHModel(*model, cone, Transform3f(), 16, 16);
283  env.push_back(new CollisionObject(
284  shared_ptr<CollisionGeometry>(model),
285  Transform3f(Vec3f(
286  x * step_size + delta_size + 0.5 * single_size - env_scale,
287  y * step_size + delta_size + 0.5 * single_size - env_scale,
288  z * step_size + delta_size + 0.5 * single_size - env_scale))));
289  env.back()->collisionGeometry()->computeLocalAABB();
290  }
291 }
292 
293 void broad_phase_self_distance_test(double env_scale, std::size_t env_size,
294  bool use_mesh) {
295  std::vector<TStruct> ts;
296  std::vector<BenchTimer> timers;
297 
298  std::vector<CollisionObject*> env;
299  if (use_mesh)
300  generateSelfDistanceEnvironmentsMesh(env, env_scale, env_size);
301  else
302  generateSelfDistanceEnvironments(env, env_scale, env_size);
303 
304  std::vector<BroadPhaseCollisionManager*> managers;
305 
306  managers.push_back(new NaiveCollisionManager());
307  managers.push_back(new SSaPCollisionManager());
308  managers.push_back(new SaPCollisionManager());
309  managers.push_back(new IntervalTreeCollisionManager());
310 
311  Vec3f lower_limit, upper_limit;
312  SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit);
313  FCL_REAL cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 5,
314  (upper_limit[1] - lower_limit[1]) / 5),
315  (upper_limit[2] - lower_limit[2]) / 5);
316  // managers.push_back(new SpatialHashingCollisionManager<>(cell_size,
317  // lower_limit, upper_limit));
318  managers.push_back(new SpatialHashingCollisionManager<
319  SparseHashTable<AABB, CollisionObject*, SpatialHash> >(
320  cell_size, lower_limit, upper_limit));
321 #if USE_GOOGLEHASH
322  managers.push_back(
323  new SpatialHashingCollisionManager<SparseHashTable<
324  AABB, CollisionObject*, SpatialHash, GoogleSparseHashTable> >(
325  cell_size, lower_limit, upper_limit));
326  managers.push_back(
327  new SpatialHashingCollisionManager<SparseHashTable<
328  AABB, CollisionObject*, SpatialHash, GoogleDenseHashTable> >(
329  cell_size, lower_limit, upper_limit));
330 #endif
331  managers.push_back(new DynamicAABBTreeCollisionManager());
332  managers.push_back(new DynamicAABBTreeArrayCollisionManager());
333 
334  {
335  DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager();
336  m->tree_init_level = 2;
337  managers.push_back(m);
338  }
339 
340  {
341  DynamicAABBTreeArrayCollisionManager* m =
342  new DynamicAABBTreeArrayCollisionManager();
343  m->tree_init_level = 2;
344  managers.push_back(m);
345  }
346 
347  ts.resize(managers.size());
348  timers.resize(managers.size());
349 
350  for (size_t i = 0; i < managers.size(); ++i) {
351  timers[i].start();
352  managers[i]->registerObjects(env);
353  timers[i].stop();
354  ts[i].push_back(timers[i].getElapsedTime());
355  }
356 
357  for (size_t i = 0; i < managers.size(); ++i) {
358  timers[i].start();
359  managers[i]->setup();
360  timers[i].stop();
361  ts[i].push_back(timers[i].getElapsedTime());
362  }
363 
364  std::vector<DistanceCallBackDefault> self_callbacks(managers.size());
365 
366  for (size_t i = 0; i < self_callbacks.size(); ++i) {
367  timers[i].start();
368  managers[i]->distance(&self_callbacks[i]);
369  timers[i].stop();
370  ts[i].push_back(timers[i].getElapsedTime());
371  // std::cout << self_data[i].result.min_distance << " ";
372  }
373  // std::cout << std::endl;
374 
375  for (size_t i = 1; i < managers.size(); ++i)
376  BOOST_CHECK(fabs(self_callbacks[0].data.result.min_distance -
377  self_callbacks[i].data.result.min_distance) < DELTA ||
378  fabs(self_callbacks[0].data.result.min_distance -
379  self_callbacks[i].data.result.min_distance) /
380  fabs(self_callbacks[0].data.result.min_distance) <
381  DELTA);
382 
383  for (size_t i = 0; i < env.size(); ++i) delete env[i];
384 
385  for (size_t i = 0; i < managers.size(); ++i) delete managers[i];
386 
387  std::cout.setf(std::ios_base::left, std::ios_base::adjustfield);
388  int w = 7;
389 
390  std::cout << "self distance timing summary" << std::endl;
391  std::cout << env.size() << " objs" << std::endl;
392  std::cout << "register time" << std::endl;
393  for (size_t i = 0; i < ts.size(); ++i)
394  std::cout << std::setw(w) << ts[i].records[0] << " ";
395  std::cout << std::endl;
396 
397  std::cout << "setup time" << std::endl;
398  for (size_t i = 0; i < ts.size(); ++i)
399  std::cout << std::setw(w) << ts[i].records[1] << " ";
400  std::cout << std::endl;
401 
402  std::cout << "self distance time" << std::endl;
403  for (size_t i = 0; i < ts.size(); ++i)
404  std::cout << std::setw(w) << ts[i].records[2] << " ";
405  std::cout << std::endl;
406 
407  std::cout << "overall time" << std::endl;
408  for (size_t i = 0; i < ts.size(); ++i)
409  std::cout << std::setw(w) << ts[i].overall_time << " ";
410  std::cout << std::endl;
411  std::cout << std::endl;
412 }
413 
414 void broad_phase_distance_test(double env_scale, std::size_t env_size,
415  std::size_t query_size, bool use_mesh) {
416  std::vector<TStruct> ts;
417  std::vector<BenchTimer> timers;
418 
419  std::vector<CollisionObject*> env;
420  if (use_mesh)
421  generateEnvironmentsMesh(env, env_scale, env_size);
422  else
423  generateEnvironments(env, env_scale, env_size);
424 
425  std::vector<CollisionObject*> query;
426 
427  BroadPhaseCollisionManager* manager = new NaiveCollisionManager();
428  for (std::size_t i = 0; i < env.size(); ++i) manager->registerObject(env[i]);
429  manager->setup();
430 
431  while (1) {
432  std::vector<CollisionObject*> candidates;
433  if (use_mesh)
434  generateEnvironmentsMesh(candidates, env_scale, query_size);
435  else
436  generateEnvironments(candidates, env_scale, query_size);
437 
438  for (std::size_t i = 0; i < candidates.size(); ++i) {
439  CollisionCallBackDefault callback;
440  manager->collide(candidates[i], &callback);
441  if (callback.data.result.numContacts() == 0)
442  query.push_back(candidates[i]);
443  else
444  delete candidates[i];
445  if (query.size() == query_size) break;
446  }
447 
448  if (query.size() == query_size) break;
449  }
450 
451  delete manager;
452 
453  std::vector<BroadPhaseCollisionManager*> managers;
454 
455  managers.push_back(new NaiveCollisionManager());
456  managers.push_back(new SSaPCollisionManager());
457  managers.push_back(new SaPCollisionManager());
458  managers.push_back(new IntervalTreeCollisionManager());
459 
460  Vec3f lower_limit, upper_limit;
461  SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit);
462  FCL_REAL cell_size =
463  std::min(std::min((upper_limit[0] - lower_limit[0]) / 20,
464  (upper_limit[1] - lower_limit[1]) / 20),
465  (upper_limit[2] - lower_limit[2]) / 20);
466  // managers.push_back(new SpatialHashingCollisionManager<>(cell_size,
467  // lower_limit, upper_limit));
468  managers.push_back(new SpatialHashingCollisionManager<
469  SparseHashTable<AABB, CollisionObject*, SpatialHash> >(
470  cell_size, lower_limit, upper_limit));
471 #if USE_GOOGLEHASH
472  managers.push_back(
473  new SpatialHashingCollisionManager<SparseHashTable<
474  AABB, CollisionObject*, SpatialHash, GoogleSparseHashTable> >(
475  cell_size, lower_limit, upper_limit));
476  managers.push_back(
477  new SpatialHashingCollisionManager<SparseHashTable<
478  AABB, CollisionObject*, SpatialHash, GoogleDenseHashTable> >(
479  cell_size, lower_limit, upper_limit));
480 #endif
481  managers.push_back(new DynamicAABBTreeCollisionManager());
482  managers.push_back(new DynamicAABBTreeArrayCollisionManager());
483 
484  {
485  DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager();
486  m->tree_init_level = 2;
487  managers.push_back(m);
488  }
489 
490  {
491  DynamicAABBTreeArrayCollisionManager* m =
492  new DynamicAABBTreeArrayCollisionManager();
493  m->tree_init_level = 2;
494  managers.push_back(m);
495  }
496 
497  ts.resize(managers.size());
498  timers.resize(managers.size());
499 
500  for (size_t i = 0; i < managers.size(); ++i) {
501  timers[i].start();
502  managers[i]->registerObjects(env);
503  timers[i].stop();
504  ts[i].push_back(timers[i].getElapsedTime());
505  }
506 
507  for (size_t i = 0; i < managers.size(); ++i) {
508  timers[i].start();
509  managers[i]->setup();
510  timers[i].stop();
511  ts[i].push_back(timers[i].getElapsedTime());
512  }
513 
514  for (size_t i = 0; i < query.size(); ++i) {
515  std::vector<DistanceCallBackDefault> query_callbacks(managers.size());
516  for (size_t j = 0; j < managers.size(); ++j) {
517  timers[j].start();
518  managers[j]->distance(query[i], &query_callbacks[j]);
519  timers[j].stop();
520  ts[j].push_back(timers[j].getElapsedTime());
521  std::cout << query_callbacks[j].data.result.min_distance << " ";
522  }
523  std::cout << std::endl;
524 
525  for (size_t j = 1; j < managers.size(); ++j) {
526  bool test = fabs(query_callbacks[0].data.result.min_distance -
527  query_callbacks[j].data.result.min_distance) < DELTA ||
528  fabs(query_callbacks[0].data.result.min_distance -
529  query_callbacks[j].data.result.min_distance) /
530  fabs(query_callbacks[0].data.result.min_distance) <
531  DELTA;
532  BOOST_CHECK(test);
533 
534  if (!test) {
535  const BroadPhaseCollisionManager& self = *managers[j];
536  std::cout << "j: " << typeid(self).name() << std::endl;
537  std::cout << "query_callbacks[0].data.result.min_distance: "
538  << query_callbacks[0].data.result.min_distance << std::endl;
539  std::cout << "query_callbacks[j].data.result.min_distance: "
540  << query_callbacks[j].data.result.min_distance << std::endl;
541  }
542  }
543  }
544 
545  for (std::size_t i = 0; i < env.size(); ++i) delete env[i];
546  for (std::size_t i = 0; i < query.size(); ++i) delete query[i];
547 
548  for (size_t i = 0; i < managers.size(); ++i) delete managers[i];
549 
550  std::cout.setf(std::ios_base::left, std::ios_base::adjustfield);
551  int w = 7;
552 
553  std::cout << "distance timing summary" << std::endl;
554  std::cout << env_size << " objs, " << query_size << " queries" << std::endl;
555  std::cout << "register time" << std::endl;
556  for (size_t i = 0; i < ts.size(); ++i)
557  std::cout << std::setw(w) << ts[i].records[0] << " ";
558  std::cout << std::endl;
559 
560  std::cout << "setup time" << std::endl;
561  for (size_t i = 0; i < ts.size(); ++i)
562  std::cout << std::setw(w) << ts[i].records[1] << " ";
563  std::cout << std::endl;
564 
565  std::cout << "distance time" << std::endl;
566  for (size_t i = 0; i < ts.size(); ++i) {
567  FCL_REAL tmp = 0;
568  for (size_t j = 2; j < ts[i].records.size(); ++j) tmp += ts[i].records[j];
569  std::cout << std::setw(w) << tmp << " ";
570  }
571  std::cout << std::endl;
572 
573  std::cout << "overall time" << std::endl;
574  for (size_t i = 0; i < ts.size(); ++i)
575  std::cout << std::setw(w) << ts[i].overall_time << " ";
576  std::cout << std::endl;
577  std::cout << std::endl;
578 }
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
hpp::fcl::IntervalTreeCollisionManager
Collision manager based on interval tree.
Definition: broadphase_interval_tree.h:51
y
y
hpp::fcl::generateEnvironmentsMesh
void generateEnvironmentsMesh(std::vector< CollisionObject *> &env, FCL_REAL env_scale, std::size_t n)
Definition: utility.cpp:421
hpp::fcl::BroadPhaseCollisionManager
Base class for broad phase collision. It helps to accelerate the collision/distance between N objects...
Definition: broadphase_collision_manager.h:54
hpp::fcl::Cylinder
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: shape/geometric_shapes.h:501
hpp::fcl::generateEnvironments
void generateEnvironments(std::vector< CollisionObject *> &env, FCL_REAL env_scale, std::size_t n)
Definition: utility.cpp:390
DELTA
FCL_REAL DELTA
Definition: broadphase.cpp:78
hpp::fcl::SSaPCollisionManager
Simple SAP collision manager.
Definition: broadphase_SSaP.h:48
test
void test(const Shape &original_shape, const FCL_REAL inflation, const FCL_REAL tol=1e-8)
Definition: shape_inflation.cpp:110
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::CollisionResult::numContacts
size_t numContacts() const
number of contacts found
Definition: collision_data.h:342
name
name
data
data
hpp::fcl::SpatialHashingCollisionManager
spatial hashing collision mananger
Definition: broadphase_spatialhash.h:55
generateSelfDistanceEnvironmentsMesh
void generateSelfDistanceEnvironmentsMesh(std::vector< CollisionObject *> &env, double env_scale, std::size_t n)
Generate environment with 3 * n objects for self distance, but all in meshes.
Definition: broadphase.cpp:215
broad_phase_distance_test
void broad_phase_distance_test(double env_scale, std::size_t env_size, std::size_t query_size, bool use_mesh=false)
test for broad phase distance
Definition: broadphase.cpp:414
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
x
x
hpp::fcl::Box
Center at zero point, axis aligned box.
Definition: shape/geometric_shapes.h:125
collision_manager.sphere
sphere
Definition: collision_manager.py:4
hpp::fcl::SaPCollisionManager
Rigorous SAP collision manager.
Definition: broadphase_SaP.h:50
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::Cone
Cone The base of the cone is at and the top is at .
Definition: shape/geometric_shapes.h:414
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::CollisionCallBackDefault
Default collision callback to check collision between collision objects.
Definition: default_broadphase_callbacks.h:185
hpp::fcl::CollisionData::result
CollisionResult result
Collision result.
Definition: default_broadphase_callbacks.h:63
geometric_shape_to_BVH_model.h
hpp::fcl::NaiveCollisionManager
Brute force N-body collision manager.
Definition: broadphase_bruteforce.h:48
generate_distance_plot.m
float m
Definition: generate_distance_plot.py:7
hpp::fcl::Sphere
Center at zero point sphere.
Definition: shape/geometric_shapes.h:196
broad_phase_self_distance_test
void broad_phase_self_distance_test(double env_scale, std::size_t env_size, bool use_mesh=false)
test for broad phase self distance
Definition: broadphase.cpp:293
hpp::fcl::BroadPhaseCollisionManager::setup
virtual void setup()=0
initialize the manager, related with the specific type of manager
hpp::fcl::detail::SpatialHash
Spatial hash function: hash an AABB to a set of integer values.
Definition: spatial_hash.h:50
collision_manager.callback
callback
Definition: collision_manager.py:27
collision_manager.box
box
Definition: collision_manager.py:11
hpp::fcl::BroadPhaseCollisionManager::collide
virtual void collide(CollisionObject *obj, CollisionCallBackBase *callback) const =0
perform collision test between one object and all the objects belonging to the manager ...
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_distance)
check broad phase distance
Definition: broadphase.cpp:102
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
hpp::fcl::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information...
Definition: collision_object.h:215
hpp::fcl::detail::SparseHashTable
A hash table implemented using unordered_map.
Definition: sparse_hash_table.h:62
generateSelfDistanceEnvironments
void generateSelfDistanceEnvironments(std::vector< CollisionObject *> &env, double env_scale, std::size_t n)
Generate environment with 3 * n objects for self distance, so we try to make sure none of them collid...
Definition: broadphase.cpp:145
hpp::fcl::generateBVHModel
void generateBVHModel(BVHModel< BV > &model, const Box &shape, const Transform3f &pose)
Generate BVH model from box.
Definition: geometric_shape_to_BVH_model.h:50
hpp::fcl::BroadPhaseCollisionManager::registerObject
virtual void registerObject(CollisionObject *obj)=0
add one object to the manager

hpp-fcl
Author(s):
autogenerated on Fri Jun 2 2023 02:39:00