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
36 /** @author Jia Pan */
37
38 #include <gtest/gtest.h>
39
40 #include "fcl/config.h"
41 #include "fcl/broadphase/broadphase_bruteforce.h"
42 #include "fcl/broadphase/broadphase_spatialhash.h"
43 #include "fcl/broadphase/broadphase_SaP.h"
44 #include "fcl/broadphase/broadphase_SSaP.h"
45 #include "fcl/broadphase/broadphase_interval_tree.h"
46 #include "fcl/broadphase/broadphase_dynamic_AABB_tree.h"
47 #include "fcl/broadphase/broadphase_dynamic_AABB_tree_array.h"
48 #include "fcl/broadphase/default_broadphase_callbacks.h"
49 #include "fcl/broadphase/detail/sparse_hash_table.h"
50 #include "fcl/broadphase/detail/spatial_hash.h"
51 #include "fcl/geometry/geometric_shape_to_BVH_model.h"
52 #include "test_fcl_utility.h"
53
54 #if USE_GOOGLEHASH
55 #include <sparsehash/sparse_hash_map>
56 #include <sparsehash/dense_hash_map>
57 #include <hash_map>
58 #endif
59
60 #include <iostream>
61 #include <iomanip>
62
63 using namespace fcl;
64
65 /// @brief make sure if broadphase algorithms doesn't check twice for the same
66 /// collision object pair
67 template <typename S>
68 void broad_phase_duplicate_check_test(S env_scale, std::size_t env_size, bool verbose = false);
69
70 /// @brief test for broad phase update
71 template <typename S>
72 void broad_phase_update_collision_test(S 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);
73
74 #if USE_GOOGLEHASH
75 template<typename U, typename V>
76 struct GoogleSparseHashTable : public google::sparse_hash_map<U, V, std::tr1::hash<size_t>, std::equal_to<size_t> > {};
77
78 template<typename U, typename V>
79 struct GoogleDenseHashTable : public google::dense_hash_map<U, V, std::tr1::hash<size_t>, std::equal_to<size_t> >
80 {
GoogleDenseHashTableGoogleDenseHashTable81 GoogleDenseHashTable() : google::dense_hash_map<U, V, std::tr1::hash<size_t>, std::equal_to<size_t> >()
82 {
83 this->set_empty_key(nullptr);
84 }
85 };
86 #endif
87
88 /// make sure if broadphase algorithms doesn't check twice for the same
89 /// collision object pair
GTEST_TEST(FCL_BROADPHASE,test_broad_phase_dont_duplicate_check)90 GTEST_TEST(FCL_BROADPHASE, test_broad_phase_dont_duplicate_check)
91 {
92 #ifdef NDEBUG
93 broad_phase_duplicate_check_test<double>(2000, 1000);
94 #else
95 broad_phase_duplicate_check_test<double>(2000, 100);
96 #endif
97 }
98
99 /// check the update, only return collision or not
GTEST_TEST(FCL_BROADPHASE,test_core_bf_broad_phase_update_collision_binary)100 GTEST_TEST(FCL_BROADPHASE, test_core_bf_broad_phase_update_collision_binary)
101 {
102 #ifdef NDEBUG
103 broad_phase_update_collision_test<double>(2000, 100, 1000, 1, false);
104 broad_phase_update_collision_test<double>(2000, 1000, 1000, 1, false);
105 #else
106 broad_phase_update_collision_test<double>(2000, 10, 100, 1, false);
107 broad_phase_update_collision_test<double>(2000, 100, 100, 1, false);
108 #endif
109 }
110
111 /// check the update, return 10 contacts
GTEST_TEST(FCL_BROADPHASE,test_core_bf_broad_phase_update_collision)112 GTEST_TEST(FCL_BROADPHASE, test_core_bf_broad_phase_update_collision)
113 {
114 #ifdef NDEBUG
115 broad_phase_update_collision_test<double>(2000, 100, 1000, 10, false);
116 broad_phase_update_collision_test<double>(2000, 1000, 1000, 10, false);
117 #else
118 broad_phase_update_collision_test<double>(2000, 10, 100, 10, false);
119 broad_phase_update_collision_test<double>(2000, 100, 100, 10, false);
120 #endif
121 }
122
123 /// check the update, exhaustive
GTEST_TEST(FCL_BROADPHASE,test_core_bf_broad_phase_update_collision_exhaustive)124 GTEST_TEST(FCL_BROADPHASE, test_core_bf_broad_phase_update_collision_exhaustive)
125 {
126 #ifdef NDEBUG
127 broad_phase_update_collision_test<double>(2000, 100, 1000, 1, true);
128 broad_phase_update_collision_test<double>(2000, 1000, 1000, 1, true);
129 #else
130 broad_phase_update_collision_test<double>(2000, 10, 100, 1, true);
131 broad_phase_update_collision_test<double>(2000, 100, 100, 1, true);
132 #endif
133 }
134
135 /// check broad phase update, in mesh, only return collision or not
GTEST_TEST(FCL_BROADPHASE,test_core_mesh_bf_broad_phase_update_collision_mesh_binary)136 GTEST_TEST(FCL_BROADPHASE, test_core_mesh_bf_broad_phase_update_collision_mesh_binary)
137 {
138 #ifdef NDEBUG
139 broad_phase_update_collision_test<double>(2000, 100, 1000, 1, false, true);
140 broad_phase_update_collision_test<double>(2000, 1000, 1000, 1, false, true);
141 #else
142 broad_phase_update_collision_test<double>(2000, 2, 4, 1, false, true);
143 broad_phase_update_collision_test<double>(2000, 4, 4, 1, false, true);
144 #endif
145 }
146
147 /// check broad phase update, in mesh, return 10 contacts
GTEST_TEST(FCL_BROADPHASE,test_core_mesh_bf_broad_phase_update_collision_mesh)148 GTEST_TEST(FCL_BROADPHASE, test_core_mesh_bf_broad_phase_update_collision_mesh)
149 {
150 #ifdef NDEBUG
151 broad_phase_update_collision_test<double>(2000, 100, 1000, 10, false, true);
152 broad_phase_update_collision_test<double>(2000, 1000, 1000, 10, false, true);
153 #else
154 broad_phase_update_collision_test<double>(200, 2, 4, 10, false, true);
155 broad_phase_update_collision_test<double>(200, 4, 4, 10, false, true);
156 #endif
157 }
158
159 /// check broad phase update, in mesh, exhaustive
GTEST_TEST(FCL_BROADPHASE,test_core_mesh_bf_broad_phase_update_collision_mesh_exhaustive)160 GTEST_TEST(FCL_BROADPHASE, test_core_mesh_bf_broad_phase_update_collision_mesh_exhaustive)
161 {
162 #ifdef NDEBUG
163 broad_phase_update_collision_test<double>(2000, 100, 1000, 1, true, true);
164 broad_phase_update_collision_test<double>(2000, 1000, 1000, 1, true, true);
165 #else
166 broad_phase_update_collision_test<double>(2000, 2, 4, 1, true, true);
167 broad_phase_update_collision_test<double>(2000, 4, 4, 1, true, true);
168 #endif
169 }
170
171 //==============================================================================
172 template <typename S>
173 struct CollisionDataForUniquenessChecking
174 {
175 std::set<std::pair<CollisionObject<S>*, CollisionObject<S>*>> checkedPairs;
176
checkUniquenessAndAddPairCollisionDataForUniquenessChecking177 bool checkUniquenessAndAddPair(CollisionObject<S>* o1, CollisionObject<S>* o2)
178 {
179 auto search = checkedPairs.find(std::make_pair(o1, o2));
180
181 if (search != checkedPairs.end())
182 return false;
183
184 checkedPairs.emplace(o1, o2);
185
186 return true;
187 }
188 };
189
190 //==============================================================================
191 template <typename S>
collisionFunctionForUniquenessChecking(CollisionObject<S> * o1,CollisionObject<S> * o2,void * cdata_)192 bool collisionFunctionForUniquenessChecking(
193 CollisionObject<S>* o1, CollisionObject<S>* o2, void* cdata_)
194 {
195 auto* cdata = static_cast<CollisionDataForUniquenessChecking<S>*>(cdata_);
196
197 EXPECT_TRUE(cdata->checkUniquenessAndAddPair(o1, o2));
198
199 return false;
200 }
201
202 //==============================================================================
203 template <typename S>
broad_phase_duplicate_check_test(S env_scale,std::size_t env_size,bool verbose)204 void broad_phase_duplicate_check_test(S env_scale, std::size_t env_size, bool verbose)
205 {
206 std::vector<test::TStruct> ts;
207 std::vector<test::Timer> timers;
208
209 std::vector<CollisionObject<S>*> env;
210 test::generateEnvironments(env, env_scale, env_size);
211
212 std::vector<BroadPhaseCollisionManager<S>*> managers;
213 managers.push_back(new NaiveCollisionManager<S>());
214 managers.push_back(new SSaPCollisionManager<S>());
215 managers.push_back(new SaPCollisionManager<S>());
216 managers.push_back(new IntervalTreeCollisionManager<S>());
217 Vector3<S> lower_limit, upper_limit;
218 SpatialHashingCollisionManager<S>::computeBound(env, lower_limit, upper_limit);
219 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);
220 managers.push_back(new SpatialHashingCollisionManager<S, detail::SparseHashTable<AABB<S>, CollisionObject<S>*, detail::SpatialHash<S>> >(cell_size, lower_limit, upper_limit));
221 #if USE_GOOGLEHASH
222 managers.push_back(new SpatialHashingCollisionManager<S, detail::SparseHashTable<AABB<S>, CollisionObject<S>*, detail::SpatialHash<S>, GoogleSparseHashTable> >(cell_size, lower_limit, upper_limit));
223 managers.push_back(new SpatialHashingCollisionManager<S, detail::SparseHashTable<AABB<S>, CollisionObject<S>*, detail::SpatialHash<S>, GoogleDenseHashTable> >(cell_size, lower_limit, upper_limit));
224 #endif
225 managers.push_back(new DynamicAABBTreeCollisionManager<S>());
226 managers.push_back(new DynamicAABBTreeCollisionManager_Array<S>());
227
228 {
229 DynamicAABBTreeCollisionManager<S>* m = new DynamicAABBTreeCollisionManager<S>();
230 m->tree_init_level = 2;
231 managers.push_back(m);
232 }
233
234 {
235 DynamicAABBTreeCollisionManager_Array<S>* m = new DynamicAABBTreeCollisionManager_Array<S>();
236 m->tree_init_level = 2;
237 managers.push_back(m);
238 }
239
240 ts.resize(managers.size());
241 timers.resize(managers.size());
242
243 for(size_t i = 0; i < managers.size(); ++i)
244 {
245 timers[i].start();
246 managers[i]->registerObjects(env);
247 timers[i].stop();
248 ts[i].push_back(timers[i].getElapsedTime());
249 }
250
251 for(size_t i = 0; i < managers.size(); ++i)
252 {
253 timers[i].start();
254 managers[i]->setup();
255 timers[i].stop();
256 ts[i].push_back(timers[i].getElapsedTime());
257 }
258
259 // update the environment
260 S delta_angle_max = 10 / 360.0 * 2 * constants<S>::pi();
261 S delta_trans_max = 0.01 * env_scale;
262 for(size_t i = 0; i < env.size(); ++i)
263 {
264 S rand_angle_x = 2 * (rand() / (S)RAND_MAX - 0.5) * delta_angle_max;
265 S rand_trans_x = 2 * (rand() / (S)RAND_MAX - 0.5) * delta_trans_max;
266 S rand_angle_y = 2 * (rand() / (S)RAND_MAX - 0.5) * delta_angle_max;
267 S rand_trans_y = 2 * (rand() / (S)RAND_MAX - 0.5) * delta_trans_max;
268 S rand_angle_z = 2 * (rand() / (S)RAND_MAX - 0.5) * delta_angle_max;
269 S rand_trans_z = 2 * (rand() / (S)RAND_MAX - 0.5) * delta_trans_max;
270
271 Matrix3<S> dR(
272 AngleAxis<S>(rand_angle_x, Vector3<S>::UnitX())
273 * AngleAxis<S>(rand_angle_y, Vector3<S>::UnitY())
274 * AngleAxis<S>(rand_angle_z, Vector3<S>::UnitZ()));
275 Vector3<S> dT(rand_trans_x, rand_trans_y, rand_trans_z);
276
277 Matrix3<S> R = env[i]->getRotation();
278 Vector3<S> T = env[i]->getTranslation();
279 env[i]->setTransform(dR * R, dR * T + dT);
280 env[i]->computeAABB();
281 }
282
283 for(size_t i = 0; i < managers.size(); ++i)
284 {
285 timers[i].start();
286 managers[i]->update();
287 timers[i].stop();
288 ts[i].push_back(timers[i].getElapsedTime());
289 }
290
291 std::vector<CollisionDataForUniquenessChecking<S>> self_data(managers.size());
292
293 for(size_t i = 0; i < managers.size(); ++i)
294 {
295 timers[i].start();
296 managers[i]->collide(&self_data[i], collisionFunctionForUniquenessChecking);
297 timers[i].stop();
298 ts[i].push_back(timers[i].getElapsedTime());
299 }
300
301 for (auto obj : env)
302 delete obj;
303
304 if (!verbose)
305 return;
306
307 std::cout.setf(std::ios_base::left, std::ios_base::adjustfield);
308 size_t w = 7;
309
310 std::cout << "collision timing summary" << std::endl;
311 std::cout << env_size << " objs" << std::endl;
312 std::cout << "register time" << std::endl;
313 for(size_t i = 0; i < ts.size(); ++i)
314 std::cout << std::setw(w) << ts[i].records[0] << " ";
315 std::cout << std::endl;
316
317 std::cout << "setup time" << std::endl;
318 for(size_t i = 0; i < ts.size(); ++i)
319 std::cout << std::setw(w) << ts[i].records[1] << " ";
320 std::cout << std::endl;
321
322 std::cout << "update time" << std::endl;
323 for(size_t i = 0; i < ts.size(); ++i)
324 std::cout << std::setw(w) << ts[i].records[2] << " ";
325 std::cout << std::endl;
326
327 std::cout << "self collision time" << std::endl;
328 for(size_t i = 0; i < ts.size(); ++i)
329 std::cout << std::setw(w) << ts[i].records[3] << " ";
330 std::cout << std::endl;
331
332 std::cout << "collision time" << std::endl;
333 for(size_t i = 0; i < ts.size(); ++i)
334 {
335 S tmp = 0;
336 for(size_t j = 4; j < ts[i].records.size(); ++j)
337 tmp += ts[i].records[j];
338 std::cout << std::setw(w) << tmp << " ";
339 }
340 std::cout << std::endl;
341
342 std::cout << "overall time" << std::endl;
343 for(size_t i = 0; i < ts.size(); ++i)
344 std::cout << std::setw(w) << ts[i].overall_time << " ";
345 std::cout << std::endl;
346 std::cout << std::endl;
347 }
348
349 template <typename S>
broad_phase_update_collision_test(S env_scale,std::size_t env_size,std::size_t query_size,std::size_t num_max_contacts,bool exhaustive,bool use_mesh)350 void broad_phase_update_collision_test(S env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts, bool exhaustive, bool use_mesh)
351 {
352 std::vector<test::TStruct> ts;
353 std::vector<test::Timer> timers;
354
355 std::vector<CollisionObject<S>*> env;
356 if(use_mesh)
357 test::generateEnvironmentsMesh(env, env_scale, env_size);
358 else
359 test::generateEnvironments(env, env_scale, env_size);
360
361 std::vector<CollisionObject<S>*> query;
362 if(use_mesh)
363 test::generateEnvironmentsMesh(query, env_scale, query_size);
364 else
365 test::generateEnvironments(query, env_scale, query_size);
366
367 std::vector<BroadPhaseCollisionManager<S>*> managers;
368
369 managers.push_back(new NaiveCollisionManager<S>());
370 managers.push_back(new SSaPCollisionManager<S>());
371
372
373 managers.push_back(new SaPCollisionManager<S>());
374 managers.push_back(new IntervalTreeCollisionManager<S>());
375
376 Vector3<S> lower_limit, upper_limit;
377 SpatialHashingCollisionManager<S>::computeBound(env, lower_limit, upper_limit);
378 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);
379 // managers.push_back(new SpatialHashingCollisionManager<S>(cell_size, lower_limit, upper_limit));
380 managers.push_back(new SpatialHashingCollisionManager<S, detail::SparseHashTable<AABB<S>, CollisionObject<S>*, detail::SpatialHash<S>> >(cell_size, lower_limit, upper_limit));
381 #if USE_GOOGLEHASH
382 managers.push_back(new SpatialHashingCollisionManager<S, detail::SparseHashTable<AABB<S>, CollisionObject<S>*, detail::SpatialHash<S>, GoogleSparseHashTable> >(cell_size, lower_limit, upper_limit));
383 managers.push_back(new SpatialHashingCollisionManager<S, detail::SparseHashTable<AABB<S>, CollisionObject<S>*, detail::SpatialHash<S>, GoogleDenseHashTable> >(cell_size, lower_limit, upper_limit));
384 #endif
385 managers.push_back(new DynamicAABBTreeCollisionManager<S>());
386 managers.push_back(new DynamicAABBTreeCollisionManager_Array<S>());
387
388 {
389 DynamicAABBTreeCollisionManager<S>* m = new DynamicAABBTreeCollisionManager<S>();
390 m->tree_init_level = 2;
391 managers.push_back(m);
392 }
393
394 {
395 DynamicAABBTreeCollisionManager_Array<S>* m = new DynamicAABBTreeCollisionManager_Array<S>();
396 m->tree_init_level = 2;
397 managers.push_back(m);
398 }
399
400 ts.resize(managers.size());
401 timers.resize(managers.size());
402
403 for(size_t i = 0; i < managers.size(); ++i)
404 {
405 timers[i].start();
406 managers[i]->registerObjects(env);
407 timers[i].stop();
408 ts[i].push_back(timers[i].getElapsedTime());
409 }
410
411 for(size_t i = 0; i < managers.size(); ++i)
412 {
413 timers[i].start();
414 managers[i]->setup();
415 timers[i].stop();
416 ts[i].push_back(timers[i].getElapsedTime());
417 }
418
419 // update the environment
420 S delta_angle_max = 10 / 360.0 * 2 * constants<S>::pi();
421 S delta_trans_max = 0.01 * env_scale;
422 for(size_t i = 0; i < env.size(); ++i)
423 {
424 S rand_angle_x = 2 * (rand() / (S)RAND_MAX - 0.5) * delta_angle_max;
425 S rand_trans_x = 2 * (rand() / (S)RAND_MAX - 0.5) * delta_trans_max;
426 S rand_angle_y = 2 * (rand() / (S)RAND_MAX - 0.5) * delta_angle_max;
427 S rand_trans_y = 2 * (rand() / (S)RAND_MAX - 0.5) * delta_trans_max;
428 S rand_angle_z = 2 * (rand() / (S)RAND_MAX - 0.5) * delta_angle_max;
429 S rand_trans_z = 2 * (rand() / (S)RAND_MAX - 0.5) * delta_trans_max;
430
431 Matrix3<S> dR(
432 AngleAxis<S>(rand_angle_x, Vector3<S>::UnitX())
433 * AngleAxis<S>(rand_angle_y, Vector3<S>::UnitY())
434 * AngleAxis<S>(rand_angle_z, Vector3<S>::UnitZ()));
435 Vector3<S> dT(rand_trans_x, rand_trans_y, rand_trans_z);
436
437 Matrix3<S> R = env[i]->getRotation();
438 Vector3<S> T = env[i]->getTranslation();
439 env[i]->setTransform(dR * R, dR * T + dT);
440 env[i]->computeAABB();
441 }
442
443 for(size_t i = 0; i < managers.size(); ++i)
444 {
445 timers[i].start();
446 managers[i]->update();
447 timers[i].stop();
448 ts[i].push_back(timers[i].getElapsedTime());
449 }
450
451 std::vector<DefaultCollisionData<S>> self_data(managers.size());
452 for(size_t i = 0; i < managers.size(); ++i)
453 {
454 if(exhaustive) self_data[i].request.num_max_contacts = 100000;
455 else self_data[i].request.num_max_contacts = num_max_contacts;
456 }
457
458 for(size_t i = 0; i < managers.size(); ++i)
459 {
460 timers[i].start();
461 managers[i]->collide(&self_data[i], DefaultCollisionFunction);
462 timers[i].stop();
463 ts[i].push_back(timers[i].getElapsedTime());
464 }
465
466
467 for(size_t i = 0; i < managers.size(); ++i)
468 std::cout << self_data[i].result.numContacts() << " ";
469 std::cout << std::endl;
470
471 if(exhaustive)
472 {
473 for(size_t i = 1; i < managers.size(); ++i)
474 EXPECT_TRUE(self_data[i].result.numContacts() == self_data[0].result.numContacts());
475 }
476 else
477 {
478 std::vector<bool> self_res(managers.size());
479 for(size_t i = 0; i < self_res.size(); ++i)
480 self_res[i] = (self_data[i].result.numContacts() > 0);
481
482 for(size_t i = 1; i < self_res.size(); ++i)
483 EXPECT_TRUE(self_res[0] == self_res[i]);
484
485 for(size_t i = 1; i < managers.size(); ++i)
486 EXPECT_TRUE(self_data[i].result.numContacts() == self_data[0].result.numContacts());
487 }
488
489
490 for(size_t i = 0; i < query.size(); ++i)
491 {
492 std::vector<DefaultCollisionData<S>> query_data(managers.size());
493 for(size_t j = 0; j < query_data.size(); ++j)
494 {
495 if(exhaustive) query_data[j].request.num_max_contacts = 100000;
496 else query_data[j].request.num_max_contacts = num_max_contacts;
497 }
498
499 for(size_t j = 0; j < query_data.size(); ++j)
500 {
501 timers[j].start();
502 managers[j]->collide(query[i], &query_data[j], DefaultCollisionFunction);
503 timers[j].stop();
504 ts[j].push_back(timers[j].getElapsedTime());
505 }
506
507
508 // for(size_t j = 0; j < managers.size(); ++j)
509 // std::cout << query_data[j].result.numContacts() << " ";
510 // std::cout << std::endl;
511
512 if(exhaustive)
513 {
514 for(size_t j = 1; j < managers.size(); ++j)
515 EXPECT_TRUE(query_data[j].result.numContacts() == query_data[0].result.numContacts());
516 }
517 else
518 {
519 std::vector<bool> query_res(managers.size());
520 for(size_t j = 0; j < query_res.size(); ++j)
521 query_res[j] = (query_data[j].result.numContacts() > 0);
522 for(size_t j = 1; j < query_res.size(); ++j)
523 EXPECT_TRUE(query_res[0] == query_res[j]);
524
525 for(size_t j = 1; j < managers.size(); ++j)
526 EXPECT_TRUE(query_data[j].result.numContacts() == query_data[0].result.numContacts());
527 }
528 }
529
530
531 for(size_t i = 0; i < env.size(); ++i)
532 delete env[i];
533 for(size_t i = 0; i < query.size(); ++i)
534 delete query[i];
535
536 for(size_t i = 0; i < managers.size(); ++i)
537 delete managers[i];
538
539
540 std::cout.setf(std::ios_base::left, std::ios_base::adjustfield);
541 size_t w = 7;
542
543 std::cout << "collision timing summary" << std::endl;
544 std::cout << env_size << " objs, " << query_size << " queries" << std::endl;
545 std::cout << "register time" << std::endl;
546 for(size_t i = 0; i < ts.size(); ++i)
547 std::cout << std::setw(w) << ts[i].records[0] << " ";
548 std::cout << std::endl;
549
550 std::cout << "setup time" << std::endl;
551 for(size_t i = 0; i < ts.size(); ++i)
552 std::cout << std::setw(w) << ts[i].records[1] << " ";
553 std::cout << std::endl;
554
555 std::cout << "update time" << std::endl;
556 for(size_t i = 0; i < ts.size(); ++i)
557 std::cout << std::setw(w) << ts[i].records[2] << " ";
558 std::cout << std::endl;
559
560 std::cout << "self collision time" << std::endl;
561 for(size_t i = 0; i < ts.size(); ++i)
562 std::cout << std::setw(w) << ts[i].records[3] << " ";
563 std::cout << std::endl;
564
565 std::cout << "collision time" << std::endl;
566 for(size_t i = 0; i < ts.size(); ++i)
567 {
568 S tmp = 0;
569 for(size_t j = 4; j < ts[i].records.size(); ++j)
570 tmp += ts[i].records[j];
571 std::cout << std::setw(w) << tmp << " ";
572 }
573 std::cout << std::endl;
574
575
576 std::cout << "overall time" << std::endl;
577 for(size_t i = 0; i < ts.size(); ++i)
578 std::cout << std::setw(w) << ts[i].overall_time << " ";
579 std::cout << std::endl;
580 std::cout << std::endl;
581 }
582
583 //==============================================================================
main(int argc,char * argv[])584 int main(int argc, char* argv[])
585 {
586 ::testing::InitGoogleTest(&argc, argv);
587 return RUN_ALL_TESTS();
588 }
589