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));
239 managers.push_back(m);
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) {
307 managers[i]->collide(&callback);
309 ts[i].push_back(timers[i].getElapsedTime());
312 for (
auto obj : env)
delete obj;
314 if (!verbose)
return;
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));
411 managers.push_back(m);
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) {
485 managers[i]->collide(&callback);
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;
BOOST_AUTO_TEST_CASE(test_broad_phase_dont_duplicate_check)
Collision manager based on interval tree.
CollisionDataForUniquenessChecking data
void generateEnvironmentsMesh(std::vector< CollisionObject *> &env, FCL_REAL env_scale, std::size_t n)
void generateEnvironments(std::vector< CollisionObject *> &env, FCL_REAL env_scale, std::size_t n)
Simple SAP collision manager.
Base callback class for collision queries. This class can be supersed by child classes to provide des...
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
static void computeBound(std::vector< CollisionObject *> &objs, Vec3f &l, Vec3f &u)
compute the bound for the environent
spatial hashing collision mananger
Rigorous SAP collision manager.
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Default collision callback to check collision between collision objects.
Brute force N-body collision manager.
Spatial hash function: hash an AABB to a set of integer values.
bool checkUniquenessAndAddPair(CollisionObject *o1, CollisionObject *o2)
std::set< std::pair< CollisionObject *, CollisionObject * > > checkedPairs
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
the object for collision or distance computation, contains the geometry and the transform information...
bool collide(CollisionObject *o1, CollisionObject *o2)
Collision evaluation between two objects in collision. This callback will cause the broadphase evalua...
A hash table implemented using unordered_map.
void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size, bool verbose=false)
make sure if broadphase algorithms doesn't check twice for the same collision object pair ...
void broad_phase_update_collision_test(FCL_REAL 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 update