38 #define BOOST_TEST_MODULE BROADPHASE_COLLISION_1
39 #include <boost/test/included/unit_test.hpp>
53 #include <boost/math/constants/constants.hpp>
56 #include <sparsehash/sparse_hash_map>
57 #include <sparsehash/dense_hash_map>
73 std::size_t query_size,
75 bool exhaustive =
false,
76 bool use_mesh =
false);
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>> {};
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);
141 test_core_mesh_bf_broad_phase_update_collision_mesh_binary) {
164 test_core_mesh_bf_broad_phase_update_collision_mesh_exhaustive) {
179 auto search = checkedPairs.find(std::make_pair(o1, o2));
181 if (search != checkedPairs.end())
return false;
183 checkedPairs.emplace(o1, o2);
192 BOOST_CHECK(
data.checkUniquenessAndAddPair(o1, o2));
202 std::vector<TStruct>
ts;
203 std::vector<BenchTimer> timers;
205 std::vector<CollisionObject*> env;
208 std::vector<BroadPhaseCollisionManager*> managers;
213 Vec3f lower_limit, upper_limit;
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);
222 cell_size, lower_limit, upper_limit));
227 cell_size, lower_limit, upper_limit));
231 cell_size, lower_limit, upper_limit));
238 m->tree_init_level = 2;
239 managers.push_back(
m);
245 m->tree_init_level = 2;
246 managers.push_back(
m);
249 ts.resize(managers.size());
250 timers.resize(managers.size());
252 for (
size_t i = 0; i < managers.size(); ++i) {
254 managers[i]->registerObjects(env);
256 ts[i].push_back(timers[i].getElapsedTime());
259 for (
size_t i = 0; i < managers.size(); ++i) {
261 managers[i]->setup();
263 ts[i].push_back(timers[i].getElapsedTime());
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) {
272 2 * (rand() / (
FCL_REAL)RAND_MAX - 0.5) * delta_angle_max;
274 2 * (rand() / (
FCL_REAL)RAND_MAX - 0.5) * delta_trans_max;
276 2 * (rand() / (
FCL_REAL)RAND_MAX - 0.5) * delta_angle_max;
278 2 * (rand() / (
FCL_REAL)RAND_MAX - 0.5) * delta_trans_max;
280 2 * (rand() / (
FCL_REAL)RAND_MAX - 0.5) * delta_angle_max;
282 2 * (rand() / (
FCL_REAL)RAND_MAX - 0.5) * delta_trans_max;
287 Vec3f dT(rand_trans_x, rand_trans_y, rand_trans_z);
290 Vec3f T = env[i]->getTranslation();
291 env[i]->setTransform(dR *
R, dR * T + dT);
292 env[i]->computeAABB();
295 for (
size_t i = 0; i < managers.size(); ++i) {
297 managers[i]->update();
299 ts[i].push_back(timers[i].getElapsedTime());
302 std::vector<CollisionDataForUniquenessChecking> self_data(managers.size());
304 for (
size_t i = 0; i < managers.size(); ++i) {
309 ts[i].push_back(timers[i].getElapsedTime());
312 for (
auto obj : env)
delete obj;
316 std::cout.setf(std::ios_base::left, std::ios_base::adjustfield);
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;
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;
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;
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;
341 std::cout <<
"collision time" << std::endl;
342 for (
size_t i = 0; i <
ts.size(); ++i) {
344 for (
size_t j = 4; j <
ts[i].records.size(); ++j) tmp +=
ts[i].records[j];
345 std::cout << std::setw(w) << tmp <<
" ";
347 std::cout << std::endl;
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;
357 std::size_t query_size,
359 bool exhaustive,
bool use_mesh) {
360 std::vector<TStruct>
ts;
361 std::vector<BenchTimer> timers;
363 std::vector<CollisionObject*> env;
369 std::vector<CollisionObject*> query;
375 std::vector<BroadPhaseCollisionManager*> managers;
383 Vec3f lower_limit, upper_limit;
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);
394 cell_size, lower_limit, upper_limit));
399 cell_size, lower_limit, upper_limit));
403 cell_size, lower_limit, upper_limit));
410 m->tree_init_level = 2;
411 managers.push_back(
m);
417 m->tree_init_level = 2;
418 managers.push_back(
m);
421 ts.resize(managers.size());
422 timers.resize(managers.size());
424 for (
size_t i = 0; i < managers.size(); ++i) {
426 managers[i]->registerObjects(env);
428 ts[i].push_back(timers[i].getElapsedTime());
431 for (
size_t i = 0; i < managers.size(); ++i) {
433 managers[i]->setup();
435 ts[i].push_back(timers[i].getElapsedTime());
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) {
444 2 * (rand() / (
FCL_REAL)RAND_MAX - 0.5) * delta_angle_max;
446 2 * (rand() / (
FCL_REAL)RAND_MAX - 0.5) * delta_trans_max;
448 2 * (rand() / (
FCL_REAL)RAND_MAX - 0.5) * delta_angle_max;
450 2 * (rand() / (
FCL_REAL)RAND_MAX - 0.5) * delta_trans_max;
452 2 * (rand() / (
FCL_REAL)RAND_MAX - 0.5) * delta_angle_max;
454 2 * (rand() / (
FCL_REAL)RAND_MAX - 0.5) * delta_trans_max;
459 Vec3f dT(rand_trans_x, rand_trans_y, rand_trans_z);
462 Vec3f T = env[i]->getTranslation();
463 env[i]->setTransform(dR *
R, dR * T + dT);
464 env[i]->computeAABB();
467 for (
size_t i = 0; i < managers.size(); ++i) {
469 managers[i]->update();
471 ts[i].push_back(timers[i].getElapsedTime());
474 std::vector<CollisionData> self_data(managers.size());
475 for (
size_t i = 0; i < managers.size(); ++i) {
477 self_data[i].request.num_max_contacts = 100000;
482 for (
size_t i = 0; i < managers.size(); ++i) {
487 ts[i].push_back(timers[i].getElapsedTime());
490 for (
size_t i = 0; i < managers.size(); ++i)
491 std::cout << self_data[i].result.numContacts() <<
" ";
492 std::cout << std::endl;
495 for (
size_t i = 1; i < managers.size(); ++i)
496 BOOST_CHECK(self_data[i].result.numContacts() ==
497 self_data[0].result.numContacts());
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);
503 for (
size_t i = 1; i < self_res.size(); ++i)
504 BOOST_CHECK(self_res[0] == self_res[i]);
506 for (
size_t i = 1; i < managers.size(); ++i)
507 BOOST_CHECK(self_data[i].result.numContacts() ==
508 self_data[0].result.numContacts());
511 for (
size_t i = 0; i < query.size(); ++i) {
512 std::vector<CollisionCallBackDefault> query_callbacks(managers.size());
514 for (
size_t j = 0; j < query_callbacks.size(); ++j) {
516 query_callbacks[j].data.request.num_max_contacts = 100000;
521 for (
size_t j = 0; j < query_callbacks.size(); ++j) {
523 managers[j]->collide(query[i], &query_callbacks[j]);
525 ts[j].push_back(timers[j].getElapsedTime());
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());
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]);
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());
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];
552 for (
size_t i = 0; i < managers.size(); ++i)
delete managers[i];
554 std::cout.setf(std::ios_base::left, std::ios_base::adjustfield);
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;
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;
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;
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;
579 std::cout <<
"collision time" << std::endl;
580 for (
size_t i = 0; i <
ts.size(); ++i) {
582 for (
size_t j = 4; j <
ts[i].records.size(); ++j) tmp +=
ts[i].records[j];
583 std::cout << std::setw(w) << tmp <<
" ";
585 std::cout << std::endl;
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;