38 #include <gtest/gtest.h>
40 #include "fcl/config.h"
55 #include <sparsehash/sparse_hash_map>
56 #include <sparsehash/dense_hash_map>
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> > {};
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> >
91 GoogleDenseHashTable() : google::dense_hash_map<U, V, std::tr1::hash<size_t>, std::equal_to<size_t> >()
93 this->set_empty_key(
nullptr);
99 GTEST_TEST(FCL_BROADPHASE, test_core_bf_broad_phase_distance)
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);
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);
115 GTEST_TEST(FCL_BROADPHASE, test_core_bf_broad_phase_self_distance)
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);
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);
129 GTEST_TEST(FCL_BROADPHASE, test_core_mesh_bf_broad_phase_distance_mesh)
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);
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);
145 GTEST_TEST(FCL_BROADPHASE, test_core_mesh_bf_broad_phase_self_distance_mesh)
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);
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);
158 template <
typename S>
161 unsigned int n_edge = std::floor(std::pow(n, 1/3.0));
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;
168 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;
174 Box<S>* box =
new Box<S>(single_size, single_size, single_size);
177 y * step_size + delta_size + 0.5 * single_size - env_scale,
178 z * step_size + delta_size + 0.5 * single_size - env_scale)))));
181 for(; i < n_edge * n_edge * n_edge / 4; ++i)
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;
190 y * step_size + delta_size + 0.5 * single_size - env_scale,
191 z * step_size + delta_size + 0.5 * single_size - env_scale)))));
194 for(; i < n_edge * n_edge * n_edge / 4; ++i)
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;
203 y * step_size + delta_size + 0.5 * single_size - env_scale,
204 z * step_size + delta_size + 0.5 * single_size - env_scale)))));
207 for(; i < n_edge * n_edge * n_edge / 4; ++i)
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;
216 y * step_size + delta_size + 0.5 * single_size - env_scale,
217 z * step_size + delta_size + 0.5 * single_size - env_scale)))));
220 for(; i < n_edge * n_edge * n_edge / 4; ++i)
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;
229 y * step_size + delta_size + 0.5 * single_size - env_scale,
230 z * step_size + delta_size + 0.5 * single_size - env_scale)))));
234 template <
typename S>
237 unsigned int n_edge = std::floor(std::pow(n, 1/3.0));
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;
244 for(; i < n_edge * n_edge * n_edge / 4; ++i)
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;
250 Box<S> box(single_size, single_size, single_size);
255 y * step_size + delta_size + 0.5 * single_size - env_scale,
256 z * step_size + delta_size + 0.5 * single_size - env_scale)))));
259 for(; i < n_edge * n_edge * n_edge / 4; ++i)
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;
270 y * step_size + delta_size + 0.5 * single_size - env_scale,
271 z * step_size + delta_size + 0.5 * single_size - env_scale)))));
274 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 Ellipsoid<S> ellipsoid(single_size / 2, single_size / 2, single_size / 2);
285 y * step_size + delta_size + 0.5 * single_size - env_scale,
286 z * step_size + delta_size + 0.5 * single_size - env_scale)))));
289 for(; i < n_edge * n_edge * n_edge / 4; ++i)
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;
295 Cylinder<S> cylinder(single_size / 2, single_size);
300 y * step_size + delta_size + 0.5 * single_size - env_scale,
301 z * step_size + delta_size + 0.5 * single_size - env_scale)))));
304 for(; i < n_edge * n_edge * n_edge / 4; ++i)
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;
310 Cone<S> cone(single_size / 2, single_size);
315 y * step_size + delta_size + 0.5 * single_size - env_scale,
316 z * step_size + delta_size + 0.5 * single_size - env_scale)))));
320 template <
typename S>
323 std::vector<test::TStruct> ts;
324 std::vector<test::Timer> timers;
326 std::vector<CollisionObject<S>*> env;
332 std::vector<BroadPhaseCollisionManager<S>*> managers;
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);
354 managers.push_back(m);
360 managers.push_back(m);
363 ts.resize(managers.size());
364 timers.resize(managers.size());
366 for(
size_t i = 0; i < managers.size(); ++i)
369 managers[i]->registerObjects(env);
371 ts[i].push_back(timers[i].getElapsedTime());
374 for(
size_t i = 0; i < managers.size(); ++i)
377 managers[i]->setup();
379 ts[i].push_back(timers[i].getElapsedTime());
383 std::vector<DefaultDistanceData<S>> self_data(managers.size());
385 for(
size_t i = 0; i < self_data.size(); ++i)
390 ts[i].push_back(timers[i].getElapsedTime());
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>());
399 for(
size_t i = 0; i < env.size(); ++i)
402 for(
size_t i = 0; i < managers.size(); ++i)
405 std::cout.setf(std::ios_base::left, std::ios_base::adjustfield);
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;
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;
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;
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;
432 template <
typename S>
435 std::vector<test::TStruct> ts;
436 std::vector<test::Timer> timers;
438 std::vector<CollisionObject<S>*> env;
444 std::vector<CollisionObject<S>*> query;
447 for(std::size_t i = 0; i < env.size(); ++i)
453 std::vector<CollisionObject<S>*> candidates;
459 for(std::size_t i = 0; i < candidates.size(); ++i)
463 if(query_data.
result.numContacts() == 0)
464 query.push_back(candidates[i]);
466 delete candidates[i];
467 if(query.size() == query_size)
break;
470 if(query.size() == query_size)
break;
475 std::vector<BroadPhaseCollisionManager<S>*> managers;
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);
497 managers.push_back(m);
503 managers.push_back(m);
506 ts.resize(managers.size());
507 timers.resize(managers.size());
509 for(
size_t i = 0; i < managers.size(); ++i)
512 managers[i]->registerObjects(env);
514 ts[i].push_back(timers[i].getElapsedTime());
517 for(
size_t i = 0; i < managers.size(); ++i)
520 managers[i]->setup();
522 ts[i].push_back(timers[i].getElapsedTime());
526 for(
size_t i = 0; i < query.size(); ++i)
528 std::vector<DefaultDistanceData<S>> query_data(managers.size());
529 for(
size_t j = 0; j < managers.size(); ++j)
534 ts[j].push_back(timers[j].getElapsedTime());
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>());
545 for(std::size_t i = 0; i < env.size(); ++i)
547 for(std::size_t i = 0; i < query.size(); ++i)
550 for(
size_t i = 0; i < managers.size(); ++i)
554 std::cout.setf(std::ios_base::left, std::ios_base::adjustfield);
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;
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 <<
"distance time" << std::endl;
570 for(
size_t i = 0; i < ts.size(); ++i)
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 <<
" ";
577 std::cout << std::endl;
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;
587 int main(
int argc,
char* argv[])
589 ::testing::InitGoogleTest(&argc, argv);
590 return RUN_ALL_TESTS();