38 #define BOOST_TEST_MODULE FCL_BROADPHASE
39 #include <boost/test/included/unit_test.hpp>
41 #include <hpp/fcl/config.hh>
48 #include <sparsehash/sparse_hash_map>
49 #include <sparsehash/dense_hash_map>
53 #include <boost/math/constants/constants.hpp>
63 double env_scale, std::size_t n);
68 double env_scale, std::size_t n);
72 std::size_t query_size,
bool use_mesh =
false);
76 bool use_mesh =
false);
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> > {};
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);
146 double env_scale, std::size_t n) {
147 int n_edge =
static_cast<int>(std::floor(std::pow(n, 1 / 3.0)));
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;
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;
159 Box*
box =
new Box(single_size, single_size, single_size);
161 shared_ptr<CollisionGeometry>(
box),
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();
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;
176 shared_ptr<CollisionGeometry>(
sphere),
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();
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;
191 shared_ptr<CollisionGeometry>(cylinder),
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();
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;
204 Cone* cone =
new Cone(single_size / 2, single_size);
206 shared_ptr<CollisionGeometry>(cone),
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();
216 double env_scale, std::size_t n) {
217 int n_edge =
static_cast<int>(std::floor(std::pow(n, 1 / 3.0)));
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;
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;
229 Box box(single_size, single_size, single_size);
233 shared_ptr<CollisionGeometry>(model),
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();
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;
250 shared_ptr<CollisionGeometry>(model),
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();
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;
263 Cylinder cylinder(single_size / 2, single_size);
267 shared_ptr<CollisionGeometry>(model),
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();
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;
280 Cone cone(single_size / 2, single_size);
284 shared_ptr<CollisionGeometry>(model),
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();
295 std::vector<TStruct>
ts;
296 std::vector<BenchTimer> timers;
298 std::vector<CollisionObject*> env;
304 std::vector<BroadPhaseCollisionManager*> managers;
311 Vec3f 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);
320 cell_size, lower_limit, upper_limit));
325 cell_size, lower_limit, upper_limit));
329 cell_size, lower_limit, upper_limit));
336 m->tree_init_level = 2;
337 managers.push_back(
m);
343 m->tree_init_level = 2;
344 managers.push_back(
m);
347 ts.resize(managers.size());
348 timers.resize(managers.size());
350 for (
size_t i = 0; i < managers.size(); ++i) {
352 managers[i]->registerObjects(env);
354 ts[i].push_back(timers[i].getElapsedTime());
357 for (
size_t i = 0; i < managers.size(); ++i) {
359 managers[i]->setup();
361 ts[i].push_back(timers[i].getElapsedTime());
364 std::vector<DistanceCallBackDefault> self_callbacks(managers.size());
366 for (
size_t i = 0; i < self_callbacks.size(); ++i) {
368 managers[i]->distance(&self_callbacks[i]);
370 ts[i].push_back(timers[i].getElapsedTime());
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) <
383 for (
size_t i = 0; i < env.size(); ++i)
delete env[i];
385 for (
size_t i = 0; i < managers.size(); ++i)
delete managers[i];
387 std::cout.setf(std::ios_base::left, std::ios_base::adjustfield);
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;
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;
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;
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;
415 std::size_t query_size,
bool use_mesh) {
416 std::vector<TStruct>
ts;
417 std::vector<BenchTimer> timers;
419 std::vector<CollisionObject*> env;
425 std::vector<CollisionObject*> query;
428 for (std::size_t i = 0; i < env.size(); ++i) manager->
registerObject(env[i]);
432 std::vector<CollisionObject*> candidates;
438 for (std::size_t i = 0; i < candidates.size(); ++i) {
441 if (
callback.data.result.numContacts() == 0)
442 query.push_back(candidates[i]);
444 delete candidates[i];
445 if (query.size() == query_size)
break;
448 if (query.size() == query_size)
break;
453 std::vector<BroadPhaseCollisionManager*> managers;
460 Vec3f lower_limit, upper_limit;
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);
470 cell_size, lower_limit, upper_limit));
475 cell_size, lower_limit, upper_limit));
479 cell_size, lower_limit, upper_limit));
486 m->tree_init_level = 2;
487 managers.push_back(
m);
493 m->tree_init_level = 2;
494 managers.push_back(
m);
497 ts.resize(managers.size());
498 timers.resize(managers.size());
500 for (
size_t i = 0; i < managers.size(); ++i) {
502 managers[i]->registerObjects(env);
504 ts[i].push_back(timers[i].getElapsedTime());
507 for (
size_t i = 0; i < managers.size(); ++i) {
509 managers[i]->setup();
511 ts[i].push_back(timers[i].getElapsedTime());
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) {
518 managers[j]->distance(query[i], &query_callbacks[j]);
520 ts[j].push_back(timers[j].getElapsedTime());
521 std::cout << query_callbacks[j].data.result.min_distance <<
" ";
523 std::cout << std::endl;
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) <
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;
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];
548 for (
size_t i = 0; i < managers.size(); ++i)
delete managers[i];
550 std::cout.setf(std::ios_base::left, std::ios_base::adjustfield);
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;
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;
565 std::cout <<
"distance time" << std::endl;
566 for (
size_t i = 0; i <
ts.size(); ++i) {
568 for (
size_t j = 2; j <
ts[i].records.size(); ++j) tmp +=
ts[i].records[j];
569 std::cout << std::setw(w) << tmp <<
" ";
571 std::cout << std::endl;
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;