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 test for broad phase collision and self collision
66 template <typename S>
67 void broad_phase_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);
68
69 #if USE_GOOGLEHASH
70 template<typename U, typename V>
71 struct GoogleSparseHashTable : public google::sparse_hash_map<U, V, std::tr1::hash<size_t>, std::equal_to<size_t> > {};
72
73 template<typename U, typename V>
74 struct GoogleDenseHashTable : public google::dense_hash_map<U, V, std::tr1::hash<size_t>, std::equal_to<size_t> >
75 {
GoogleDenseHashTableGoogleDenseHashTable76 GoogleDenseHashTable() : google::dense_hash_map<U, V, std::tr1::hash<size_t>, std::equal_to<size_t> >()
77 {
78 this->set_empty_key(nullptr);
79 }
80 };
81 #endif
82
83 /// check broad phase collision for empty collision object set and queries
GTEST_TEST(FCL_BROADPHASE,test_core_bf_broad_phase_collision_empty)84 GTEST_TEST(FCL_BROADPHASE, test_core_bf_broad_phase_collision_empty)
85 {
86 #ifdef NDEBUG
87 broad_phase_collision_test<double>(2000, 0, 0, 10, false, false);
88 broad_phase_collision_test<double>(2000, 0, 1000, 10, false, false);
89 broad_phase_collision_test<double>(2000, 100, 0, 10, false, false);
90
91 broad_phase_collision_test<double>(2000, 0, 0, 10, false, true);
92 broad_phase_collision_test<double>(2000, 0, 1000, 10, false, true);
93 broad_phase_collision_test<double>(2000, 100, 0, 10, false, true);
94
95 broad_phase_collision_test<double>(2000, 0, 0, 10, true, false);
96 broad_phase_collision_test<double>(2000, 0, 1000, 10, true, false);
97 broad_phase_collision_test<double>(2000, 100, 0, 10, true, false);
98
99 broad_phase_collision_test<double>(2000, 0, 0, 10, true, true);
100 broad_phase_collision_test<double>(2000, 0, 1000, 10, true, true);
101 broad_phase_collision_test<double>(2000, 100, 0, 10, true, true);
102 #else
103 broad_phase_collision_test<double>(2000, 0, 0, 10, false, false);
104 broad_phase_collision_test<double>(2000, 0, 5, 10, false, false);
105 broad_phase_collision_test<double>(2000, 2, 0, 10, false, false);
106
107 broad_phase_collision_test<double>(2000, 0, 0, 10, false, true);
108 broad_phase_collision_test<double>(2000, 0, 5, 10, false, true);
109 broad_phase_collision_test<double>(2000, 2, 0, 10, false, true);
110
111 broad_phase_collision_test<double>(2000, 0, 0, 10, true, false);
112 broad_phase_collision_test<double>(2000, 0, 5, 10, true, false);
113 broad_phase_collision_test<double>(2000, 2, 0, 10, true, false);
114
115 broad_phase_collision_test<double>(2000, 0, 0, 10, true, true);
116 broad_phase_collision_test<double>(2000, 0, 5, 10, true, true);
117 broad_phase_collision_test<double>(2000, 2, 0, 10, true, true);
118 #endif
119 }
120
121 /// check broad phase collision and self collision, only return collision or not
GTEST_TEST(FCL_BROADPHASE,test_core_bf_broad_phase_collision_binary)122 GTEST_TEST(FCL_BROADPHASE, test_core_bf_broad_phase_collision_binary)
123 {
124 #ifdef NDEBUG
125 broad_phase_collision_test<double>(2000, 100, 1000, 1, false);
126 broad_phase_collision_test<double>(2000, 1000, 1000, 1, false);
127 broad_phase_collision_test<double>(2000, 100, 1000, 1, true);
128 broad_phase_collision_test<double>(2000, 1000, 1000, 1, true);
129 #else
130 broad_phase_collision_test<double>(2000, 10, 100, 1, false);
131 broad_phase_collision_test<double>(2000, 100, 100, 1, false);
132 broad_phase_collision_test<double>(2000, 10, 100, 1, true);
133 broad_phase_collision_test<double>(2000, 100, 100, 1, true);
134 #endif
135 }
136
137 /// check broad phase collision and self collision, return 10 contacts
GTEST_TEST(FCL_BROADPHASE,test_core_bf_broad_phase_collision)138 GTEST_TEST(FCL_BROADPHASE, test_core_bf_broad_phase_collision)
139 {
140 #ifdef NDEBUG
141 broad_phase_collision_test<double>(2000, 100, 1000, 10, false);
142 broad_phase_collision_test<double>(2000, 1000, 1000, 10, false);
143 #else
144 broad_phase_collision_test<double>(2000, 10, 100, 10, false);
145 broad_phase_collision_test<double>(2000, 100, 100, 10, false);
146 #endif
147 }
148
149 /// check broad phase collision and self collision, return only collision or not, in mesh
GTEST_TEST(FCL_BROADPHASE,test_core_mesh_bf_broad_phase_collision_mesh_binary)150 GTEST_TEST(FCL_BROADPHASE, test_core_mesh_bf_broad_phase_collision_mesh_binary)
151 {
152 #ifdef NDEBUG
153 broad_phase_collision_test<double>(2000, 100, 1000, 1, false, true);
154 broad_phase_collision_test<double>(2000, 1000, 1000, 1, false, true);
155 #else
156 broad_phase_collision_test<double>(2000, 2, 5, 1, false, true);
157 broad_phase_collision_test<double>(2000, 5, 5, 1, false, true);
158 #endif
159 }
160
161 /// check broad phase collision and self collision, return 10 contacts, in mesh
GTEST_TEST(FCL_BROADPHASE,test_core_mesh_bf_broad_phase_collision_mesh)162 GTEST_TEST(FCL_BROADPHASE, test_core_mesh_bf_broad_phase_collision_mesh)
163 {
164 #ifdef NDEBUG
165 broad_phase_collision_test<double>(2000, 100, 1000, 10, false, true);
166 broad_phase_collision_test<double>(2000, 1000, 1000, 10, false, true);
167 #else
168 broad_phase_collision_test<double>(2000, 2, 5, 10, false, true);
169 broad_phase_collision_test<double>(2000, 5, 5, 10, false, true);
170 #endif
171 }
172
173 /// check broad phase collision and self collision, exhaustive, in mesh
GTEST_TEST(FCL_BROADPHASE,test_core_mesh_bf_broad_phase_collision_mesh_exhaustive)174 GTEST_TEST(FCL_BROADPHASE, test_core_mesh_bf_broad_phase_collision_mesh_exhaustive)
175 {
176 #ifdef NDEBUG
177 broad_phase_collision_test<double>(2000, 100, 1000, 1, true, true);
178 broad_phase_collision_test<double>(2000, 1000, 1000, 1, true, true);
179 #else
180 broad_phase_collision_test<double>(2000, 2, 5, 1, true, true);
181 broad_phase_collision_test<double>(2000, 5, 5, 1, true, true);
182 #endif
183 }
184
185 template <typename S>
broad_phase_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)186 void broad_phase_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)
187 {
188 std::vector<test::TStruct> ts;
189 std::vector<test::Timer> timers;
190
191 std::vector<CollisionObject<S>*> env;
192 if(use_mesh)
193 test::generateEnvironmentsMesh(env, env_scale, env_size);
194 else
195 test::generateEnvironments(env, env_scale, env_size);
196
197 std::vector<CollisionObject<S>*> query;
198 if(use_mesh)
199 test::generateEnvironmentsMesh(query, env_scale, query_size);
200 else
201 test::generateEnvironments(query, env_scale, query_size);
202
203 std::vector<BroadPhaseCollisionManager<S>*> managers;
204
205 managers.push_back(new NaiveCollisionManager<S>());
206 managers.push_back(new SSaPCollisionManager<S>());
207 managers.push_back(new SaPCollisionManager<S>());
208 managers.push_back(new IntervalTreeCollisionManager<S>());
209
210 Vector3<S> lower_limit, upper_limit;
211 SpatialHashingCollisionManager<S>::computeBound(env, lower_limit, upper_limit);
212 // S ncell_per_axis = std::pow((S)env_size / 10, 1 / 3.0);
213 S ncell_per_axis = 20;
214 S cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / ncell_per_axis, (upper_limit[1] - lower_limit[1]) / ncell_per_axis), (upper_limit[2] - lower_limit[2]) / ncell_per_axis);
215 // managers.push_back(new SpatialHashingCollisionManager<S>(cell_size, lower_limit, upper_limit));
216 managers.push_back(new SpatialHashingCollisionManager<S, detail::SparseHashTable<AABB<S>, CollisionObject<S>*, detail::SpatialHash<S>> >(cell_size, lower_limit, upper_limit));
217 #if USE_GOOGLEHASH
218 managers.push_back(new SpatialHashingCollisionManager<S, detail::SparseHashTable<AABB<S>, CollisionObject<S>*, detail::SpatialHash<S>, GoogleSparseHashTable> >(cell_size, lower_limit, upper_limit));
219 managers.push_back(new SpatialHashingCollisionManager<S, detail::SparseHashTable<AABB<S>, CollisionObject<S>*, detail::SpatialHash<S>, GoogleDenseHashTable> >(cell_size, lower_limit, upper_limit));
220 #endif
221 managers.push_back(new DynamicAABBTreeCollisionManager<S>());
222
223 managers.push_back(new DynamicAABBTreeCollisionManager_Array<S>());
224
225 {
226 DynamicAABBTreeCollisionManager<S>* m = new DynamicAABBTreeCollisionManager<S>();
227 m->tree_init_level = 2;
228 managers.push_back(m);
229 }
230
231 {
232 DynamicAABBTreeCollisionManager_Array<S>* m = new DynamicAABBTreeCollisionManager_Array<S>();
233 m->tree_init_level = 2;
234 managers.push_back(m);
235 }
236
237 ts.resize(managers.size());
238 timers.resize(managers.size());
239
240 for(size_t i = 0; i < managers.size(); ++i)
241 {
242 timers[i].start();
243 managers[i]->registerObjects(env);
244 timers[i].stop();
245 ts[i].push_back(timers[i].getElapsedTime());
246 }
247
248 for(size_t i = 0; i < managers.size(); ++i)
249 {
250 timers[i].start();
251 managers[i]->setup();
252 timers[i].stop();
253 ts[i].push_back(timers[i].getElapsedTime());
254 }
255
256 std::vector<DefaultCollisionData<S>> self_data(managers.size());
257 for(size_t i = 0; i < managers.size(); ++i)
258 {
259 if(exhaustive) self_data[i].request.num_max_contacts = 100000;
260 else self_data[i].request.num_max_contacts = num_max_contacts;
261 }
262
263 for(size_t i = 0; i < managers.size(); ++i)
264 {
265 timers[i].start();
266 managers[i]->collide(&self_data[i], DefaultCollisionFunction);
267 timers[i].stop();
268 ts[i].push_back(timers[i].getElapsedTime());
269 }
270
271 for(size_t i = 0; i < managers.size(); ++i)
272 std::cout << self_data[i].result.numContacts() << " ";
273 std::cout << std::endl;
274
275 if(exhaustive)
276 {
277 for(size_t i = 1; i < managers.size(); ++i)
278 EXPECT_TRUE(self_data[i].result.numContacts() == self_data[0].result.numContacts());
279 }
280 else
281 {
282 std::vector<bool> self_res(managers.size());
283 for(size_t i = 0; i < self_res.size(); ++i)
284 self_res[i] = (self_data[i].result.numContacts() > 0);
285
286 for(size_t i = 1; i < self_res.size(); ++i)
287 EXPECT_TRUE(self_res[0] == self_res[i]);
288
289 for(size_t i = 1; i < managers.size(); ++i)
290 EXPECT_TRUE(self_data[i].result.numContacts() == self_data[0].result.numContacts());
291 }
292
293
294 for(size_t i = 0; i < query.size(); ++i)
295 {
296 std::vector<DefaultCollisionData<S>> query_data(managers.size());
297 for(size_t j = 0; j < query_data.size(); ++j)
298 {
299 if(exhaustive) query_data[j].request.num_max_contacts = 100000;
300 else query_data[j].request.num_max_contacts = num_max_contacts;
301 }
302
303 for(size_t j = 0; j < query_data.size(); ++j)
304 {
305 timers[j].start();
306 managers[j]->collide(query[i], &query_data[j], DefaultCollisionFunction);
307 timers[j].stop();
308 ts[j].push_back(timers[j].getElapsedTime());
309 }
310
311
312 // for(size_t j = 0; j < managers.size(); ++j)
313 // std::cout << query_data[j].result.numContacts() << " ";
314 // std::cout << std::endl;
315
316 if(exhaustive)
317 {
318 for(size_t j = 1; j < managers.size(); ++j)
319 EXPECT_TRUE(query_data[j].result.numContacts() == query_data[0].result.numContacts());
320 }
321 else
322 {
323 std::vector<bool> query_res(managers.size());
324 for(size_t j = 0; j < query_res.size(); ++j)
325 query_res[j] = (query_data[j].result.numContacts() > 0);
326 for(size_t j = 1; j < query_res.size(); ++j)
327 EXPECT_TRUE(query_res[0] == query_res[j]);
328
329 for(size_t j = 1; j < managers.size(); ++j)
330 EXPECT_TRUE(query_data[j].result.numContacts() == query_data[0].result.numContacts());
331 }
332 }
333
334 for(size_t i = 0; i < env.size(); ++i)
335 delete env[i];
336 for(size_t i = 0; i < query.size(); ++i)
337 delete query[i];
338
339 for(size_t i = 0; i < managers.size(); ++i)
340 delete managers[i];
341
342 std::cout.setf(std::ios_base::left, std::ios_base::adjustfield);
343 size_t w = 7;
344
345 std::cout << "collision timing summary" << std::endl;
346 std::cout << env_size << " objs, " << query_size << " queries" << std::endl;
347 std::cout << "register time" << std::endl;
348 for(size_t i = 0; i < ts.size(); ++i)
349 std::cout << std::setw(w) << ts[i].records[0] << " ";
350 std::cout << std::endl;
351
352 std::cout << "setup time" << std::endl;
353 for(size_t i = 0; i < ts.size(); ++i)
354 std::cout << std::setw(w) << ts[i].records[1] << " ";
355 std::cout << std::endl;
356
357 std::cout << "self collision time" << std::endl;
358 for(size_t i = 0; i < ts.size(); ++i)
359 std::cout << std::setw(w) << ts[i].records[2] << " ";
360 std::cout << std::endl;
361
362 std::cout << "collision time" << std::endl;
363 for(size_t i = 0; i < ts.size(); ++i)
364 {
365 S tmp = 0;
366 for(size_t j = 3; j < ts[i].records.size(); ++j)
367 tmp += ts[i].records[j];
368 std::cout << std::setw(w) << tmp << " ";
369 }
370 std::cout << std::endl;
371
372
373 std::cout << "overall time" << std::endl;
374 for(size_t i = 0; i < ts.size(); ++i)
375 std::cout << std::setw(w) << ts[i].overall_time << " ";
376 std::cout << std::endl;
377 std::cout << std::endl;
378
379 }
380
381 //==============================================================================
main(int argc,char * argv[])382 int main(int argc, char* argv[])
383 {
384 ::testing::InitGoogleTest(&argc, argv);
385 return RUN_ALL_TESTS();
386 }
387