broadphase_collision_2.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 #define BOOST_TEST_MODULE COAL_BROADPHASE_COLLISION_2
39 #include <boost/test/included/unit_test.hpp>
40 
41 #include "coal/broadphase/broadphase_bruteforce.h"
42 #include "coal/broadphase/broadphase_spatialhash.h"
43 #include "coal/broadphase/broadphase_SaP.h"
44 #include "coal/broadphase/broadphase_SSaP.h"
45 #include "coal/broadphase/broadphase_interval_tree.h"
46 #include "coal/broadphase/broadphase_dynamic_AABB_tree.h"
47 #include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h"
48 #include "coal/broadphase/default_broadphase_callbacks.h"
49 #include "coal/broadphase/detail/sparse_hash_table.h"
50 #include "coal/broadphase/detail/spatial_hash.h"
51 #include "utility.h"
52 
53 #if USE_GOOGLEHASH
54 #include <sparsehash/sparse_hash_map>
55 #include <sparsehash/dense_hash_map>
56 #include <hash_map>
57 #endif
58 
59 #include <iostream>
60 #include <iomanip>
61 
62 using namespace coal;
63 
65 void broad_phase_collision_test(CoalScalar env_scale, std::size_t env_size,
66  std::size_t query_size,
67  std::size_t num_max_contacts = 1,
68  bool exhaustive = false, bool use_mesh = false);
69 
70 #if USE_GOOGLEHASH
71 template <typename U, typename V>
72 struct GoogleSparseHashTable
73  : public google::sparse_hash_map<U, V, std::tr1::hash<size_t>,
74  std::equal_to<size_t> > {};
75 
76 template <typename U, typename V>
77 struct GoogleDenseHashTable
78  : public google::dense_hash_map<U, V, std::tr1::hash<size_t>,
79  std::equal_to<size_t> > {
80  GoogleDenseHashTable()
81  : google::dense_hash_map<U, V, std::tr1::hash<size_t>,
82  std::equal_to<size_t> >() {
83  this->set_empty_key(nullptr);
84  }
85 };
86 #endif
87 
89 BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision_empty) {
90 #ifdef NDEBUG
91  broad_phase_collision_test(2000, 0, 0, 10, false, false);
92  broad_phase_collision_test(2000, 0, 1000, 10, false, false);
93  broad_phase_collision_test(2000, 100, 0, 10, false, false);
94 
95  broad_phase_collision_test(2000, 0, 0, 10, false, true);
96  broad_phase_collision_test(2000, 0, 1000, 10, false, true);
97  broad_phase_collision_test(2000, 100, 0, 10, false, true);
98 
99  broad_phase_collision_test(2000, 0, 0, 10, true, false);
100  broad_phase_collision_test(2000, 0, 1000, 10, true, false);
101  broad_phase_collision_test(2000, 100, 0, 10, true, false);
102 
103  broad_phase_collision_test(2000, 0, 0, 10, true, true);
104  broad_phase_collision_test(2000, 0, 1000, 10, true, true);
105  broad_phase_collision_test(2000, 100, 0, 10, true, true);
106 #else
107  broad_phase_collision_test(2000, 0, 0, 10, false, false);
108  broad_phase_collision_test(2000, 0, 5, 10, false, false);
109  broad_phase_collision_test(2000, 2, 0, 10, false, false);
110 
111  broad_phase_collision_test(2000, 0, 0, 10, false, true);
112  broad_phase_collision_test(2000, 0, 5, 10, false, true);
113  broad_phase_collision_test(2000, 2, 0, 10, false, true);
114 
115  broad_phase_collision_test(2000, 0, 0, 10, true, false);
116  broad_phase_collision_test(2000, 0, 5, 10, true, false);
117  broad_phase_collision_test(2000, 2, 0, 10, true, false);
118 
119  broad_phase_collision_test(2000, 0, 0, 10, true, true);
120  broad_phase_collision_test(2000, 0, 5, 10, true, true);
121  broad_phase_collision_test(2000, 2, 0, 10, true, true);
122 #endif
123 }
124 
126 BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision_binary) {
127 #ifdef NDEBUG
128  broad_phase_collision_test(2000, 100, 1000, 1, false);
129  broad_phase_collision_test(2000, 1000, 1000, 1, false);
130  broad_phase_collision_test(2000, 100, 1000, 1, true);
131  broad_phase_collision_test(2000, 1000, 1000, 1, true);
132 #else
133  broad_phase_collision_test(2000, 10, 100, 1, false);
134  broad_phase_collision_test(2000, 100, 100, 1, false);
135  broad_phase_collision_test(2000, 10, 100, 1, true);
136  broad_phase_collision_test(2000, 100, 100, 1, true);
137 #endif
138 }
139 
141 BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision) {
142 #ifdef NDEBUG
143  broad_phase_collision_test(2000, 100, 1000, 10, false);
144  broad_phase_collision_test(2000, 1000, 1000, 10, false);
145 #else
146  broad_phase_collision_test(2000, 10, 100, 10, false);
147  broad_phase_collision_test(2000, 100, 100, 10, false);
148 #endif
149 }
150 
153 BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh_binary) {
154 #ifdef NDEBUG
155  broad_phase_collision_test(2000, 100, 1000, 1, false, true);
156  broad_phase_collision_test(2000, 1000, 1000, 1, false, true);
157 #else
158  broad_phase_collision_test(2000, 2, 5, 1, false, true);
159  broad_phase_collision_test(2000, 5, 5, 1, false, true);
160 #endif
161 }
162 
164 BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh) {
165 #ifdef NDEBUG
166  broad_phase_collision_test(2000, 100, 1000, 10, false, true);
167  broad_phase_collision_test(2000, 1000, 1000, 10, false, true);
168 #else
169  broad_phase_collision_test(2000, 2, 5, 10, false, true);
170  broad_phase_collision_test(2000, 5, 5, 10, false, true);
171 #endif
172 }
173 
175 BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh_exhaustive) {
176 #ifdef NDEBUG
177  broad_phase_collision_test(2000, 100, 1000, 1, true, true);
178  broad_phase_collision_test(2000, 1000, 1000, 1, true, true);
179 #else
180  broad_phase_collision_test(2000, 2, 5, 1, true, true);
181  broad_phase_collision_test(2000, 5, 5, 1, true, true);
182 #endif
183 }
184 
185 void broad_phase_collision_test(CoalScalar env_scale, std::size_t env_size,
186  std::size_t query_size,
187  std::size_t num_max_contacts, bool exhaustive,
188  bool use_mesh) {
189  std::vector<TStruct> ts;
190  std::vector<BenchTimer> timers;
191 
192  std::vector<CollisionObject*> env;
193  if (use_mesh)
194  generateEnvironmentsMesh(env, env_scale, env_size);
195  else
196  generateEnvironments(env, env_scale, env_size);
197 
198  std::vector<CollisionObject*> query;
199  if (use_mesh)
200  generateEnvironmentsMesh(query, env_scale, query_size);
201  else
202  generateEnvironments(query, env_scale, query_size);
203 
204  std::vector<BroadPhaseCollisionManager*> managers;
205 
206  managers.push_back(new NaiveCollisionManager());
207  managers.push_back(new SSaPCollisionManager());
208  managers.push_back(new SaPCollisionManager());
209  managers.push_back(new IntervalTreeCollisionManager());
210 
211  Vec3s lower_limit, upper_limit;
212  SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit);
213  // CoalScalar ncell_per_axis = std::pow((S)env_size / 10, 1 / 3.0);
214  CoalScalar ncell_per_axis = 20;
215  CoalScalar cell_size =
216  std::min(std::min((upper_limit[0] - lower_limit[0]) / ncell_per_axis,
217  (upper_limit[1] - lower_limit[1]) / ncell_per_axis),
218  (upper_limit[2] - lower_limit[2]) / ncell_per_axis);
219  // managers.push_back(new SpatialHashingCollisionManager(cell_size,
220  // lower_limit, upper_limit));
221  managers.push_back(new SpatialHashingCollisionManager<detail::SparseHashTable<
222  AABB, CollisionObject*, detail::SpatialHash> >(
223  cell_size, lower_limit, upper_limit));
224 #if USE_GOOGLEHASH
225  managers.push_back(
226  new SpatialHashingCollisionManager<detail::SparseHashTable<
227  AABB, CollisionObject*, detail::SpatialHash, GoogleSparseHashTable> >(
228  cell_size, lower_limit, upper_limit));
229  managers.push_back(
230  new SpatialHashingCollisionManager<detail::SparseHashTable<
231  AABB, CollisionObject*, detail::SpatialHash, GoogleDenseHashTable> >(
232  cell_size, lower_limit, upper_limit));
233 #endif
234  managers.push_back(new DynamicAABBTreeCollisionManager());
235 
236  managers.push_back(new DynamicAABBTreeArrayCollisionManager());
237 
238  {
239  DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager();
240  m->tree_init_level = 2;
241  managers.push_back(m);
242  }
243 
244  {
245  DynamicAABBTreeArrayCollisionManager* m =
246  new DynamicAABBTreeArrayCollisionManager();
247  m->tree_init_level = 2;
248  managers.push_back(m);
249  }
250 
251  ts.resize(managers.size());
252  timers.resize(managers.size());
253 
254  for (size_t i = 0; i < managers.size(); ++i) {
255  timers[i].start();
256  managers[i]->registerObjects(env);
257  timers[i].stop();
258  ts[i].push_back(timers[i].getElapsedTime());
259  }
260 
261  for (size_t i = 0; i < managers.size(); ++i) {
262  timers[i].start();
263  managers[i]->setup();
264  timers[i].stop();
265  ts[i].push_back(timers[i].getElapsedTime());
266  }
267 
268  std::vector<CollisionCallBackDefault> callbacks(managers.size());
269  for (size_t i = 0; i < managers.size(); ++i) {
270  if (exhaustive)
271  callbacks[i].data.request.num_max_contacts = 100000;
272  else
273  callbacks[i].data.request.num_max_contacts = num_max_contacts;
274  }
275 
276  for (size_t i = 0; i < managers.size(); ++i) {
277  timers[i].start();
278  managers[i]->collide(&callbacks[i]);
279  timers[i].stop();
280  ts[i].push_back(timers[i].getElapsedTime());
281  }
282 
283  for (size_t i = 0; i < managers.size(); ++i)
284  std::cout << callbacks[i].data.result.numContacts() << " ";
285  std::cout << std::endl;
286 
287  if (exhaustive) {
288  for (size_t i = 1; i < managers.size(); ++i)
289  BOOST_CHECK(callbacks[i].data.result.numContacts() ==
290  callbacks[0].data.result.numContacts());
291  } else {
292  std::vector<bool> self_res(managers.size());
293  for (size_t i = 0; i < self_res.size(); ++i)
294  self_res[i] = (callbacks[i].data.result.numContacts() > 0);
295 
296  for (size_t i = 1; i < self_res.size(); ++i)
297  BOOST_CHECK(self_res[0] == self_res[i]);
298 
299  for (size_t i = 1; i < managers.size(); ++i)
300  BOOST_CHECK(callbacks[i].data.result.numContacts() ==
301  callbacks[0].data.result.numContacts());
302  }
303 
304  for (size_t i = 0; i < query.size(); ++i) {
305  std::vector<CollisionCallBackDefault> callbacks(managers.size());
306  for (size_t j = 0; j < managers.size(); ++j) {
307  if (exhaustive)
308  callbacks[j].data.request.num_max_contacts = 100000;
309  else
310  callbacks[j].data.request.num_max_contacts = num_max_contacts;
311  }
312 
313  for (size_t j = 0; j < managers.size(); ++j) {
314  timers[j].start();
315  managers[j]->collide(query[i], &callbacks[j]);
316  timers[j].stop();
317  ts[j].push_back(timers[j].getElapsedTime());
318  }
319 
320  // for(size_t j = 0; j < managers.size(); ++j)
321  // std::cout << callbacks[i].data.result.numContacts() << " ";
322  // std::cout << std::endl;
323 
324  if (exhaustive) {
325  for (size_t j = 1; j < managers.size(); ++j)
326  BOOST_CHECK(callbacks[j].data.result.numContacts() ==
327  callbacks[0].data.result.numContacts());
328  } else {
329  std::vector<bool> query_res(managers.size());
330  for (size_t j = 0; j < query_res.size(); ++j)
331  query_res[j] = (callbacks[j].data.result.numContacts() > 0);
332  for (size_t j = 1; j < query_res.size(); ++j)
333  BOOST_CHECK(query_res[0] == query_res[j]);
334 
335  for (size_t j = 1; j < managers.size(); ++j)
336  BOOST_CHECK(callbacks[j].data.result.numContacts() ==
337  callbacks[0].data.result.numContacts());
338  }
339  }
340 
341  for (size_t i = 0; i < env.size(); ++i) delete env[i];
342  for (size_t i = 0; i < query.size(); ++i) delete query[i];
343 
344  for (size_t i = 0; i < managers.size(); ++i) delete managers[i];
345 
346  std::cout.setf(std::ios_base::left, std::ios_base::adjustfield);
347  int w = 7;
348 
349  std::cout << "collision timing summary" << std::endl;
350  std::cout << env_size << " objs, " << query_size << " queries" << std::endl;
351  std::cout << "register time" << std::endl;
352  for (size_t i = 0; i < ts.size(); ++i)
353  std::cout << std::setw(w) << ts[i].records[0] << " ";
354  std::cout << std::endl;
355 
356  std::cout << "setup time" << std::endl;
357  for (size_t i = 0; i < ts.size(); ++i)
358  std::cout << std::setw(w) << ts[i].records[1] << " ";
359  std::cout << std::endl;
360 
361  std::cout << "self collision time" << std::endl;
362  for (size_t i = 0; i < ts.size(); ++i)
363  std::cout << std::setw(w) << ts[i].records[2] << " ";
364  std::cout << std::endl;
365 
366  std::cout << "collision time" << std::endl;
367  for (size_t i = 0; i < ts.size(); ++i) {
368  CoalScalar tmp = 0;
369  for (size_t j = 3; j < ts[i].records.size(); ++j) tmp += ts[i].records[j];
370  std::cout << std::setw(w) << tmp << " ";
371  }
372  std::cout << std::endl;
373 
374  std::cout << "overall time" << std::endl;
375  for (size_t i = 0; i < ts.size(); ++i)
376  std::cout << std::setw(w) << ts[i].overall_time << " ";
377  std::cout << std::endl;
378  std::cout << std::endl;
379 }
coal::SpatialHashingCollisionManager
spatial hashing collision mananger
Definition: coal/broadphase/broadphase_spatialhash.h:54
U
U
generate_distance_plot.m
float m
Definition: generate_distance_plot.py:6
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision_empty)
check broad phase collision for empty collision object set and queries
Definition: broadphase_collision_2.cpp:89
coal::Vec3s
Eigen::Matrix< CoalScalar, 3, 1 > Vec3s
Definition: coal/data_types.h:77
V
V
broadphase_dynamic_AABB_tree.h
broadphase_SaP.h
coal::DynamicAABBTreeCollisionManager
Definition: coal/broadphase/broadphase_dynamic_AABB_tree.h:53
coal::generateEnvironments
void generateEnvironments(std::vector< CollisionObject * > &env, CoalScalar env_scale, std::size_t n)
Definition: utility.cpp:392
coal::generateEnvironmentsMesh
void generateEnvironmentsMesh(std::vector< CollisionObject * > &env, CoalScalar env_scale, std::size_t n)
Definition: utility.cpp:423
data
data
coal::detail::SparseHashTable
A hash table implemented using unordered_map.
Definition: coal/broadphase/detail/sparse_hash_table.h:61
coal
Main namespace.
Definition: coal/broadphase/broadphase_bruteforce.h:44
spatial_hash.h
utility.h
default_broadphase_callbacks.h
coal::AABB
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: coal/BV/AABB.h:55
sparse_hash_table.h
coal::SSaPCollisionManager
Simple SAP collision manager.
Definition: coal/broadphase/broadphase_SSaP.h:47
coal::NaiveCollisionManager
Brute force N-body collision manager.
Definition: coal/broadphase/broadphase_bruteforce.h:47
coal::DynamicAABBTreeArrayCollisionManager
Definition: coal/broadphase/broadphase_dynamic_AABB_tree_array.h:54
coal::detail::SpatialHash
Spatial hash function: hash an AABB to a set of integer values.
Definition: coal/broadphase/detail/spatial_hash.h:49
coal::IntervalTreeCollisionManager
Collision manager based on interval tree.
Definition: coal/broadphase/broadphase_interval_tree.h:50
broadphase_spatialhash.h
coal::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information
Definition: coal/collision_object.h:214
broadphase_bruteforce.h
broadphase_dynamic_AABB_tree_array.h
broad_phase_collision_test
void broad_phase_collision_test(CoalScalar 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 collision and self collision
Definition: broadphase_collision_2.cpp:185
coal::SpatialHashingCollisionManager::computeBound
static void computeBound(std::vector< CollisionObject * > &objs, Vec3s &l, Vec3s &u)
compute the bound for the environent
Definition: coal/broadphase/broadphase_spatialhash-inl.h:501
ts
ts
num_max_contacts
int num_max_contacts
Definition: test/collision.cpp:73
broadphase_SSaP.h
coal::CoalScalar
double CoalScalar
Definition: coal/data_types.h:76
broadphase_interval_tree.h
coal::SaPCollisionManager
Rigorous SAP collision manager.
Definition: coal/broadphase/broadphase_SaP.h:49

hpp-fcl
Author(s):
autogenerated on Fri Feb 14 2025 03:45:50