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