test_fcl_broadphase_distance.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  * 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 <gtest/gtest.h>
39 
40 #include "fcl/config.h"
41 #include "fcl/broadphase/broadphase_bruteforce.h"
42 #include "fcl/broadphase/broadphase_spatialhash.h"
43 #include "fcl/broadphase/broadphase_SaP.h"
44 #include "fcl/broadphase/broadphase_SSaP.h"
45 #include "fcl/broadphase/broadphase_interval_tree.h"
46 #include "fcl/broadphase/broadphase_dynamic_AABB_tree.h"
47 #include "fcl/broadphase/broadphase_dynamic_AABB_tree_array.h"
48 #include "fcl/broadphase/default_broadphase_callbacks.h"
49 #include "fcl/broadphase/detail/sparse_hash_table.h"
50 #include "fcl/broadphase/detail/spatial_hash.h"
51 #include "fcl/geometry/geometric_shape_to_BVH_model.h"
52 #include "test_fcl_utility.h"
53 
54 #if USE_GOOGLEHASH
55 #include <sparsehash/sparse_hash_map>
56 #include <sparsehash/dense_hash_map>
57 #include <hash_map>
58 #endif
59 
60 #include <iostream>
61 #include <iomanip>
62 
63 using namespace fcl;
64 
66 template <typename S>
67 void generateSelfDistanceEnvironments(std::vector<CollisionObject<S>*>& env, S env_scale, std::size_t n);
68 
70 template <typename S>
71 void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject<S>*>& env, S env_scale, std::size_t n);
72 
74 template <typename S>
75 void broad_phase_distance_test(S env_scale, std::size_t env_size, std::size_t query_size, bool use_mesh = false);
76 
78 template <typename S>
79 void broad_phase_self_distance_test(S env_scale, std::size_t env_size, bool use_mesh = false);
80 
81 template <typename S>
82 S getDELTA() { return 0.01; }
83 
84 #if USE_GOOGLEHASH
85 template<typename U, typename V>
86 struct GoogleSparseHashTable : public google::sparse_hash_map<U, V, std::tr1::hash<size_t>, std::equal_to<size_t> > {};
87 
88 template<typename U, typename V>
89 struct GoogleDenseHashTable : public google::dense_hash_map<U, V, std::tr1::hash<size_t>, std::equal_to<size_t> >
90 {
91  GoogleDenseHashTable() : google::dense_hash_map<U, V, std::tr1::hash<size_t>, std::equal_to<size_t> >()
92  {
93  this->set_empty_key(nullptr);
94  }
95 };
96 #endif
97 
99 GTEST_TEST(FCL_BROADPHASE, test_core_bf_broad_phase_distance)
100 {
101 #ifdef NDEBUG
102  broad_phase_distance_test<double>(200, 100, 100);
103  broad_phase_distance_test<double>(200, 1000, 100);
104  broad_phase_distance_test<double>(2000, 100, 100);
105  broad_phase_distance_test<double>(2000, 1000, 100);
106 #else
107  broad_phase_distance_test<double>(200, 10, 10);
108  broad_phase_distance_test<double>(200, 100, 10);
109  broad_phase_distance_test<double>(2000, 10, 10);
110  broad_phase_distance_test<double>(2000, 100, 10);
111 #endif
112 }
113 
115 GTEST_TEST(FCL_BROADPHASE, test_core_bf_broad_phase_self_distance)
116 {
117 #ifdef NDEBUG
118  broad_phase_self_distance_test<double>(200, 512);
119  broad_phase_self_distance_test<double>(200, 1000);
120  broad_phase_self_distance_test<double>(200, 5000);
121 #else
122  broad_phase_self_distance_test<double>(200, 256);
123  broad_phase_self_distance_test<double>(200, 500);
124  broad_phase_self_distance_test<double>(200, 2500);
125 #endif
126 }
127 
129 GTEST_TEST(FCL_BROADPHASE, test_core_mesh_bf_broad_phase_distance_mesh)
130 {
131 #ifdef NDEBUG
132  broad_phase_distance_test<double>(200, 100, 100, true);
133  broad_phase_distance_test<double>(200, 1000, 100, true);
134  broad_phase_distance_test<double>(2000, 100, 100, true);
135  broad_phase_distance_test<double>(2000, 1000, 100, true);
136 #else
137  broad_phase_distance_test<double>(200, 2, 2, true);
138  broad_phase_distance_test<double>(200, 4, 2, true);
139  broad_phase_distance_test<double>(2000, 2, 2, true);
140  broad_phase_distance_test<double>(2000, 4, 2, true);
141 #endif
142 }
143 
145 GTEST_TEST(FCL_BROADPHASE, test_core_mesh_bf_broad_phase_self_distance_mesh)
146 {
147 #ifdef NDEBUG
148  broad_phase_self_distance_test<double>(200, 512, true);
149  broad_phase_self_distance_test<double>(200, 1000, true);
150  broad_phase_self_distance_test<double>(200, 5000, true);
151 #else
152  broad_phase_self_distance_test<double>(200, 128, true);
153  broad_phase_self_distance_test<double>(200, 250, true);
154  broad_phase_self_distance_test<double>(200, 1250, true);
155 #endif
156 }
157 
158 template <typename S>
159 void generateSelfDistanceEnvironments(std::vector<CollisionObject<S>*>& env, S env_scale, std::size_t n)
160 {
161  unsigned int n_edge = std::floor(std::pow(n, 1/3.0));
162 
163  S step_size = env_scale * 2 / n_edge;
164  S delta_size = step_size * 0.05;
165  S single_size = step_size - 2 * delta_size;
166 
167  unsigned int i = 0;
168  for(; i < n_edge * n_edge * n_edge / 4; ++i)
169  {
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  Box<S>* box = new Box<S>(single_size, single_size, single_size);
175  env.push_back(new CollisionObject<S>(std::shared_ptr<CollisionGeometry<S>>(box),
176  Transform3<S>(Translation3<S>(Vector3<S>(x * step_size + delta_size + 0.5 * single_size - env_scale,
177  y * step_size + delta_size + 0.5 * single_size - env_scale,
178  z * step_size + delta_size + 0.5 * single_size - env_scale)))));
179  }
180 
181  for(; i < n_edge * n_edge * n_edge / 4; ++i)
182  {
183  int x = i % (n_edge * n_edge);
184  int y = (i - n_edge * n_edge * x) % n_edge;
185  int z = i - n_edge * n_edge * x - n_edge * y;
186 
187  Sphere<S>* sphere = new Sphere<S>(single_size / 2);
188  env.push_back(new CollisionObject<S>(std::shared_ptr<CollisionGeometry<S>>(sphere),
189  Transform3<S>(Translation3<S>(Vector3<S>(x * step_size + delta_size + 0.5 * single_size - env_scale,
190  y * step_size + delta_size + 0.5 * single_size - env_scale,
191  z * step_size + delta_size + 0.5 * single_size - env_scale)))));
192  }
193 
194  for(; i < n_edge * n_edge * n_edge / 4; ++i)
195  {
196  int x = i % (n_edge * n_edge);
197  int y = (i - n_edge * n_edge * x) % n_edge;
198  int z = i - n_edge * n_edge * x - n_edge * y;
199 
200  Ellipsoid<S>* ellipsoid = new Ellipsoid<S>(single_size / 2, single_size / 2, single_size / 2);
201  env.push_back(new CollisionObject<S>(std::shared_ptr<CollisionGeometry<S>>(ellipsoid),
202  Transform3<S>(Translation3<S>(Vector3<S>(x * step_size + delta_size + 0.5 * single_size - env_scale,
203  y * step_size + delta_size + 0.5 * single_size - env_scale,
204  z * step_size + delta_size + 0.5 * single_size - env_scale)))));
205  }
206 
207  for(; i < n_edge * n_edge * n_edge / 4; ++i)
208  {
209  int x = i % (n_edge * n_edge);
210  int y = (i - n_edge * n_edge * x) % n_edge;
211  int z = i - n_edge * n_edge * x - n_edge * y;
212 
213  Cylinder<S>* cylinder = new Cylinder<S>(single_size / 2, single_size);
214  env.push_back(new CollisionObject<S>(std::shared_ptr<CollisionGeometry<S>>(cylinder),
215  Transform3<S>(Translation3<S>(Vector3<S>(x * step_size + delta_size + 0.5 * single_size - env_scale,
216  y * step_size + delta_size + 0.5 * single_size - env_scale,
217  z * step_size + delta_size + 0.5 * single_size - env_scale)))));
218  }
219 
220  for(; i < n_edge * n_edge * n_edge / 4; ++i)
221  {
222  int x = i % (n_edge * n_edge);
223  int y = (i - n_edge * n_edge * x) % n_edge;
224  int z = i - n_edge * n_edge * x - n_edge * y;
225 
226  Cone<S>* cone = new Cone<S>(single_size / 2, single_size);
227  env.push_back(new CollisionObject<S>(std::shared_ptr<CollisionGeometry<S>>(cone),
228  Transform3<S>(Translation3<S>(Vector3<S>(x * step_size + delta_size + 0.5 * single_size - env_scale,
229  y * step_size + delta_size + 0.5 * single_size - env_scale,
230  z * step_size + delta_size + 0.5 * single_size - env_scale)))));
231  }
232 }
233 
234 template <typename S>
235 void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject<S>*>& env, S env_scale, std::size_t n)
236 {
237  unsigned int n_edge = std::floor(std::pow(n, 1/3.0));
238 
239  S step_size = env_scale * 2 / n_edge;
240  S delta_size = step_size * 0.05;
241  S single_size = step_size - 2 * delta_size;
242 
243  std::size_t i = 0;
244  for(; i < n_edge * n_edge * n_edge / 4; ++i)
245  {
246  int x = i % (n_edge * n_edge);
247  int y = (i - n_edge * n_edge * x) % n_edge;
248  int z = i - n_edge * n_edge * x - n_edge * y;
249 
250  Box<S> box(single_size, single_size, single_size);
251  BVHModel<OBBRSS<S>>* model = new BVHModel<OBBRSS<S>>();
252  generateBVHModel(*model, box, Transform3<S>::Identity());
253  env.push_back(new CollisionObject<S>(std::shared_ptr<CollisionGeometry<S>>(model),
254  Transform3<S>(Translation3<S>(Vector3<S>(x * step_size + delta_size + 0.5 * single_size - env_scale,
255  y * step_size + delta_size + 0.5 * single_size - env_scale,
256  z * step_size + delta_size + 0.5 * single_size - env_scale)))));
257  }
258 
259  for(; i < n_edge * n_edge * n_edge / 4; ++i)
260  {
261  int x = i % (n_edge * n_edge);
262  int y = (i - n_edge * n_edge * x) % n_edge;
263  int z = i - n_edge * n_edge * x - n_edge * y;
264 
265  Sphere<S> sphere(single_size / 2);
266  BVHModel<OBBRSS<S>>* model = new BVHModel<OBBRSS<S>>();
267  generateBVHModel(*model, sphere, Transform3<S>::Identity(), 16, 16);
268  env.push_back(new CollisionObject<S>(std::shared_ptr<CollisionGeometry<S>>(model),
269  Transform3<S>(Translation3<S>(Vector3<S>(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  }
273 
274  for(; i < n_edge * n_edge * n_edge / 4; ++i)
275  {
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  Ellipsoid<S> ellipsoid(single_size / 2, single_size / 2, single_size / 2);
281  BVHModel<OBBRSS<S>>* model = new BVHModel<OBBRSS<S>>();
282  generateBVHModel(*model, ellipsoid, Transform3<S>::Identity(), 16, 16);
283  env.push_back(new CollisionObject<S>(std::shared_ptr<CollisionGeometry<S>>(model),
284  Transform3<S>(Translation3<S>(Vector3<S>(x * step_size + delta_size + 0.5 * single_size - env_scale,
285  y * step_size + delta_size + 0.5 * single_size - env_scale,
286  z * step_size + delta_size + 0.5 * single_size - env_scale)))));
287  }
288 
289  for(; i < n_edge * n_edge * n_edge / 4; ++i)
290  {
291  int x = i % (n_edge * n_edge);
292  int y = (i - n_edge * n_edge * x) % n_edge;
293  int z = i - n_edge * n_edge * x - n_edge * y;
294 
295  Cylinder<S> cylinder(single_size / 2, single_size);
296  BVHModel<OBBRSS<S>>* model = new BVHModel<OBBRSS<S>>();
297  generateBVHModel(*model, cylinder, Transform3<S>::Identity(), 16, 16);
298  env.push_back(new CollisionObject<S>(std::shared_ptr<CollisionGeometry<S>>(model),
299  Transform3<S>(Translation3<S>(Vector3<S>(x * step_size + delta_size + 0.5 * single_size - env_scale,
300  y * step_size + delta_size + 0.5 * single_size - env_scale,
301  z * step_size + delta_size + 0.5 * single_size - env_scale)))));
302  }
303 
304  for(; i < n_edge * n_edge * n_edge / 4; ++i)
305  {
306  int x = i % (n_edge * n_edge);
307  int y = (i - n_edge * n_edge * x) % n_edge;
308  int z = i - n_edge * n_edge * x - n_edge * y;
309 
310  Cone<S> cone(single_size / 2, single_size);
311  BVHModel<OBBRSS<S>>* model = new BVHModel<OBBRSS<S>>();
312  generateBVHModel(*model, cone, Transform3<S>::Identity(), 16, 16);
313  env.push_back(new CollisionObject<S>(std::shared_ptr<CollisionGeometry<S>>(model),
314  Transform3<S>(Translation3<S>(Vector3<S>(x * step_size + delta_size + 0.5 * single_size - env_scale,
315  y * step_size + delta_size + 0.5 * single_size - env_scale,
316  z * step_size + delta_size + 0.5 * single_size - env_scale)))));
317  }
318 }
319 
320 template <typename S>
321 void broad_phase_self_distance_test(S env_scale, std::size_t env_size, bool use_mesh)
322 {
323  std::vector<test::TStruct> ts;
324  std::vector<test::Timer> timers;
325 
326  std::vector<CollisionObject<S>*> env;
327  if(use_mesh)
328  generateSelfDistanceEnvironmentsMesh(env, env_scale, env_size);
329  else
330  generateSelfDistanceEnvironments(env, env_scale, env_size);
331 
332  std::vector<BroadPhaseCollisionManager<S>*> managers;
333 
334  managers.push_back(new NaiveCollisionManager<S>());
335  managers.push_back(new SSaPCollisionManager<S>());
336  managers.push_back(new SaPCollisionManager<S>());
337  managers.push_back(new IntervalTreeCollisionManager<S>());
338 
339  Vector3<S> lower_limit, upper_limit;
340  SpatialHashingCollisionManager<S>::computeBound(env, lower_limit, upper_limit);
341  S cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 5, (upper_limit[1] - lower_limit[1]) / 5), (upper_limit[2] - lower_limit[2]) / 5);
342  // managers.push_back(new SpatialHashingCollisionManager<S>(cell_size, lower_limit, upper_limit));
343  managers.push_back(new SpatialHashingCollisionManager<S, detail::SparseHashTable<AABB<S>, CollisionObject<S>*, detail::SpatialHash<S>> >(cell_size, lower_limit, upper_limit));
344 #if USE_GOOGLEHASH
345  managers.push_back(new SpatialHashingCollisionManager<S, detail::SparseHashTable<AABB<S>, CollisionObject<S>*, detail::SpatialHash<S>, GoogleSparseHashTable> >(cell_size, lower_limit, upper_limit));
346  managers.push_back(new SpatialHashingCollisionManager<S, detail::SparseHashTable<AABB<S>, CollisionObject<S>*, detail::SpatialHash<S>, GoogleDenseHashTable> >(cell_size, lower_limit, upper_limit));
347 #endif
348  managers.push_back(new DynamicAABBTreeCollisionManager<S>());
349  managers.push_back(new DynamicAABBTreeCollisionManager_Array<S>());
350 
351  {
352  DynamicAABBTreeCollisionManager<S>* m = new DynamicAABBTreeCollisionManager<S>();
353  m->tree_init_level = 2;
354  managers.push_back(m);
355  }
356 
357  {
358  DynamicAABBTreeCollisionManager_Array<S>* m = new DynamicAABBTreeCollisionManager_Array<S>();
359  m->tree_init_level = 2;
360  managers.push_back(m);
361  }
362 
363  ts.resize(managers.size());
364  timers.resize(managers.size());
365 
366  for(size_t i = 0; i < managers.size(); ++i)
367  {
368  timers[i].start();
369  managers[i]->registerObjects(env);
370  timers[i].stop();
371  ts[i].push_back(timers[i].getElapsedTime());
372  }
373 
374  for(size_t i = 0; i < managers.size(); ++i)
375  {
376  timers[i].start();
377  managers[i]->setup();
378  timers[i].stop();
379  ts[i].push_back(timers[i].getElapsedTime());
380  }
381 
382 
383  std::vector<DefaultDistanceData<S>> self_data(managers.size());
384 
385  for(size_t i = 0; i < self_data.size(); ++i)
386  {
387  timers[i].start();
388  managers[i]->distance(&self_data[i], DefaultDistanceFunction);
389  timers[i].stop();
390  ts[i].push_back(timers[i].getElapsedTime());
391  // std::cout << self_data[i].result.min_distance << " ";
392  }
393  // std::cout << std::endl;
394 
395  for(size_t i = 1; i < managers.size(); ++i)
396  EXPECT_TRUE(fabs(self_data[0].result.min_distance - self_data[i].result.min_distance) < getDELTA<S>() ||
397  fabs(self_data[0].result.min_distance - self_data[i].result.min_distance) / fabs(self_data[0].result.min_distance) < getDELTA<S>());
398 
399  for(size_t i = 0; i < env.size(); ++i)
400  delete env[i];
401 
402  for(size_t i = 0; i < managers.size(); ++i)
403  delete managers[i];
404 
405  std::cout.setf(std::ios_base::left, std::ios_base::adjustfield);
406  size_t w = 7;
407 
408  std::cout << "self distance timing summary" << std::endl;
409  std::cout << env.size() << " objs" << std::endl;
410  std::cout << "register time" << std::endl;
411  for(size_t i = 0; i < ts.size(); ++i)
412  std::cout << std::setw(w) << ts[i].records[0] << " ";
413  std::cout << std::endl;
414 
415  std::cout << "setup time" << std::endl;
416  for(size_t i = 0; i < ts.size(); ++i)
417  std::cout << std::setw(w) << ts[i].records[1] << " ";
418  std::cout << std::endl;
419 
420  std::cout << "self distance time" << std::endl;
421  for(size_t i = 0; i < ts.size(); ++i)
422  std::cout << std::setw(w) << ts[i].records[2] << " ";
423  std::cout << std::endl;
424 
425  std::cout << "overall time" << std::endl;
426  for(size_t i = 0; i < ts.size(); ++i)
427  std::cout << std::setw(w) << ts[i].overall_time << " ";
428  std::cout << std::endl;
429  std::cout << std::endl;
430 }
431 
432 template <typename S>
433 void broad_phase_distance_test(S env_scale, std::size_t env_size, std::size_t query_size, bool use_mesh)
434 {
435  std::vector<test::TStruct> ts;
436  std::vector<test::Timer> timers;
437 
438  std::vector<CollisionObject<S>*> env;
439  if(use_mesh)
440  test::generateEnvironmentsMesh(env, env_scale, env_size);
441  else
442  test::generateEnvironments(env, env_scale, env_size);
443 
444  std::vector<CollisionObject<S>*> query;
445 
446  BroadPhaseCollisionManager<S>* manager = new NaiveCollisionManager<S>();
447  for(std::size_t i = 0; i < env.size(); ++i)
448  manager->registerObject(env[i]);
449  manager->setup();
450 
451  while(1)
452  {
453  std::vector<CollisionObject<S>*> candidates;
454  if(use_mesh)
455  test::generateEnvironmentsMesh(candidates, env_scale, query_size);
456  else
457  test::generateEnvironments(candidates, env_scale, query_size);
458 
459  for(std::size_t i = 0; i < candidates.size(); ++i)
460  {
461  DefaultCollisionData<S> query_data;
462  manager->collide(candidates[i], &query_data, DefaultCollisionFunction);
463  if(query_data.result.numContacts() == 0)
464  query.push_back(candidates[i]);
465  else
466  delete candidates[i];
467  if(query.size() == query_size) break;
468  }
469 
470  if(query.size() == query_size) break;
471  }
472 
473  delete manager;
474 
475  std::vector<BroadPhaseCollisionManager<S>*> managers;
476 
477  managers.push_back(new NaiveCollisionManager<S>());
478  managers.push_back(new SSaPCollisionManager<S>());
479  managers.push_back(new SaPCollisionManager<S>());
480  managers.push_back(new IntervalTreeCollisionManager<S>());
481 
482  Vector3<S> lower_limit, upper_limit;
483  SpatialHashingCollisionManager<S>::computeBound(env, lower_limit, upper_limit);
484  S cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, (upper_limit[1] - lower_limit[1]) / 20), (upper_limit[2] - lower_limit[2])/20);
485  // managers.push_back(new SpatialHashingCollisionManager<S>(cell_size, lower_limit, upper_limit));
486  managers.push_back(new SpatialHashingCollisionManager<S, detail::SparseHashTable<AABB<S>, CollisionObject<S>*, detail::SpatialHash<S>> >(cell_size, lower_limit, upper_limit));
487 #if USE_GOOGLEHASH
488  managers.push_back(new SpatialHashingCollisionManager<S, detail::SparseHashTable<AABB<S>, CollisionObject<S>*, detail::SpatialHash<S>, GoogleSparseHashTable> >(cell_size, lower_limit, upper_limit));
489  managers.push_back(new SpatialHashingCollisionManager<S, detail::SparseHashTable<AABB<S>, CollisionObject<S>*, detail::SpatialHash<S>, GoogleDenseHashTable> >(cell_size, lower_limit, upper_limit));
490 #endif
491  managers.push_back(new DynamicAABBTreeCollisionManager<S>());
492  managers.push_back(new DynamicAABBTreeCollisionManager_Array<S>());
493 
494  {
495  DynamicAABBTreeCollisionManager<S>* m = new DynamicAABBTreeCollisionManager<S>();
496  m->tree_init_level = 2;
497  managers.push_back(m);
498  }
499 
500  {
501  DynamicAABBTreeCollisionManager_Array<S>* m = new DynamicAABBTreeCollisionManager_Array<S>();
502  m->tree_init_level = 2;
503  managers.push_back(m);
504  }
505 
506  ts.resize(managers.size());
507  timers.resize(managers.size());
508 
509  for(size_t i = 0; i < managers.size(); ++i)
510  {
511  timers[i].start();
512  managers[i]->registerObjects(env);
513  timers[i].stop();
514  ts[i].push_back(timers[i].getElapsedTime());
515  }
516 
517  for(size_t i = 0; i < managers.size(); ++i)
518  {
519  timers[i].start();
520  managers[i]->setup();
521  timers[i].stop();
522  ts[i].push_back(timers[i].getElapsedTime());
523  }
524 
525 
526  for(size_t i = 0; i < query.size(); ++i)
527  {
528  std::vector<DefaultDistanceData<S>> query_data(managers.size());
529  for(size_t j = 0; j < managers.size(); ++j)
530  {
531  timers[j].start();
532  managers[j]->distance(query[i], &query_data[j], DefaultDistanceFunction);
533  timers[j].stop();
534  ts[j].push_back(timers[j].getElapsedTime());
535  // std::cout << query_data[j].result.min_distance << " ";
536  }
537  // std::cout << std::endl;
538 
539  for(size_t j = 1; j < managers.size(); ++j)
540  EXPECT_TRUE(fabs(query_data[0].result.min_distance - query_data[j].result.min_distance) < getDELTA<S>() ||
541  fabs(query_data[0].result.min_distance - query_data[j].result.min_distance) / fabs(query_data[0].result.min_distance) < getDELTA<S>());
542  }
543 
544 
545  for(std::size_t i = 0; i < env.size(); ++i)
546  delete env[i];
547  for(std::size_t i = 0; i < query.size(); ++i)
548  delete query[i];
549 
550  for(size_t i = 0; i < managers.size(); ++i)
551  delete managers[i];
552 
553 
554  std::cout.setf(std::ios_base::left, std::ios_base::adjustfield);
555  size_t w = 7;
556 
557  std::cout << "distance 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 << "distance time" << std::endl;
570  for(size_t i = 0; i < ts.size(); ++i)
571  {
572  S tmp = 0;
573  for(size_t j = 2; j < ts[i].records.size(); ++j)
574  tmp += ts[i].records[j];
575  std::cout << std::setw(w) << tmp << " ";
576  }
577  std::cout << std::endl;
578 
579  std::cout << "overall time" << std::endl;
580  for(size_t i = 0; i < ts.size(); ++i)
581  std::cout << std::setw(w) << ts[i].overall_time << " ";
582  std::cout << std::endl;
583  std::cout << std::endl;
584 }
585 
586 //==============================================================================
587 int main(int argc, char* argv[])
588 {
589  ::testing::InitGoogleTest(&argc, argv);
590  return RUN_ALL_TESTS();
591 }
fcl::test::generateEnvironmentsMesh
void generateEnvironmentsMesh(std::vector< CollisionObject< S > *> &env, S env_scale, std::size_t n)
Generate environment with 3 * n objects, but all in meshes.
Definition: test_fcl_utility.h:505
GTEST_TEST
GTEST_TEST(FCL_BROADPHASE, test_core_bf_broad_phase_distance)
check broad phase distance
Definition: test_fcl_broadphase_distance.cpp:99
default_broadphase_callbacks.h
broadphase_SSaP.h
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
sparse_hash_table.h
fcl::SaPCollisionManager< S >
Functor to help unregister one object.
Definition: broadphase_SaP.h:218
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
main
int main(int argc, char *argv[])
Definition: test_fcl_broadphase_distance.cpp:587
fcl::DefaultDistanceFunction
bool DefaultDistanceFunction(CollisionObject< S > *o1, CollisionObject< S > *o2, void *data, S &dist)
Provides a simple callback for the distance query in the BroadPhaseCollisionManager. It assumes the data parameter is non-null and points to an instance of DefaultDistanceData. It simply invokes the distance() method on the culled pair of geometries and stores the results in the data&#39;s DistanceResult instance.
Definition: default_broadphase_callbacks.h:196
test_fcl_utility.h
fcl::Ellipsoid
Center at zero point ellipsoid.
Definition: ellipsoid.h:51
fcl::SpatialHashingCollisionManager::computeBound
static void computeBound(std::vector< CollisionObject< S > *> &objs, Vector3< S > &l, Vector3< S > &u)
compute the bound for the environent
Definition: broadphase_spatialhash-inl.h:619
fcl::DynamicAABBTreeCollisionManager_Array::tree_init_level
int tree_init_level
Definition: broadphase_dynamic_AABB_tree_array.h:66
fcl::BroadPhaseCollisionManager::setup
virtual void setup()=0
initialize the manager, related with the specific type of manager
fcl::SpatialHashingCollisionManager
spatial hashing collision mananger
Definition: broadphase_spatialhash.h:56
fcl::BroadPhaseCollisionManager::collide
virtual void collide(CollisionObject< S > *obj, void *cdata, CollisionCallBack< S > callback) const =0
perform collision test between one object and all the objects belonging to the manager ...
broadphase_dynamic_AABB_tree_array.h
getDELTA
S getDELTA()
Definition: test_fcl_broadphase_distance.cpp:82
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::BroadPhaseCollisionManager::registerObject
virtual void registerObject(CollisionObject< S > *obj)=0
add one object to the manager
fcl::CollisionGeometry
The geometry for the object for collision or distance computation.
Definition: collision_geometry.h:58
broad_phase_self_distance_test
void broad_phase_self_distance_test(S env_scale, std::size_t env_size, bool use_mesh=false)
test for broad phase self distance
Definition: test_fcl_broadphase_distance.cpp:321
broadphase_interval_tree.h
fcl::detail::SparseHashTable
A hash table implemented using unordered_map.
Definition: sparse_hash_table.h:59
fcl::DynamicAABBTreeCollisionManager_Array
Definition: broadphase_dynamic_AABB_tree_array.h:55
fcl::test::generateEnvironments
void generateEnvironments(std::vector< CollisionObject< S > *> &env, S env_scale, std::size_t n)
Generate environment with 3 * n objects: n boxes, n spheres and n cylinders.
Definition: test_fcl_utility.h:476
fcl::Box
Center at zero point, axis aligned box.
Definition: box.h:51
fcl::SSaPCollisionManager
Simple SAP collision manager.
Definition: broadphase_SSaP.h:49
fcl::generateBVHModel
int generateBVHModel(BVHModel< BV > &model, const Box< typename BV::S > &shape, const Transform3< typename BV::S > &pose, FinalizeModel finalize_model)
Generate BVH model from box.
Definition: geometric_shape_to_BVH_model-inl.h:69
broadphase_dynamic_AABB_tree.h
generateSelfDistanceEnvironments
void generateSelfDistanceEnvironments(std::vector< CollisionObject< S > *> &env, S 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: test_fcl_broadphase_distance.cpp:159
geometric_shape_to_BVH_model.h
fcl::Translation3
Eigen::Translation< S, 3 > Translation3
Definition: types.h:94
fcl::detail::SpatialHash
Spatial hash function: hash an AABB to a set of integer values.
Definition: spatial_hash.h:51
spatial_hash.h
min
static T min(T x, T y)
Definition: svm.cpp:49
fcl::BVHModel
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH_model.h:57
fcl::Cylinder
Center at zero cylinder.
Definition: cylinder.h:51
fcl::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information...
Definition: collision_object.h:51
fcl::DefaultCollisionData
Collision data for use with the DefaultCollisionFunction. It stores the collision request and the res...
Definition: default_broadphase_callbacks.h:57
fcl::NaiveCollisionManager
Brute force N-body collision manager.
Definition: broadphase_bruteforce.h:49
fcl::Cone
Center at zero cone.
Definition: cone.h:51
fcl::DefaultCollisionFunction
bool DefaultCollisionFunction(CollisionObject< S > *o1, CollisionObject< S > *o2, void *data)
Provides a simple callback for the collision query in the BroadPhaseCollisionManager. It assumes the data parameter is non-null and points to an instance of DefaultCollisionData. It simply invokes the collide() method on the culled pair of geometries and stores the results in the data&#39;s CollisionResult instance.
Definition: default_broadphase_callbacks.h:88
fcl::BroadPhaseCollisionManager
Base class for broad phase collision. It helps to accelerate the collision/distance between N objects...
Definition: broadphase_collision_manager.h:66
broadphase_SaP.h
fcl::Sphere
Center at zero point sphere.
Definition: sphere.h:51
broad_phase_distance_test
void broad_phase_distance_test(S env_scale, std::size_t env_size, std::size_t query_size, bool use_mesh=false)
test for broad phase distance
Definition: test_fcl_broadphase_distance.cpp:433
broadphase_spatialhash.h
EXPECT_TRUE
#define EXPECT_TRUE(args)
generateSelfDistanceEnvironmentsMesh
void generateSelfDistanceEnvironmentsMesh(std::vector< CollisionObject< S > *> &env, S env_scale, std::size_t n)
Generate environment with 3 * n objects for self distance, but all in meshes.
Definition: test_fcl_broadphase_distance.cpp:235
broadphase_bruteforce.h

fcl_catkin
Author(s):
autogenerated on Thu Mar 23 2023 03:00:19