| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
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 |
* Copyright (c) 2022, INRIA |
||
7 |
* All rights reserved. |
||
8 |
* |
||
9 |
* Redistribution and use in source and binary forms, with or without |
||
10 |
* modification, are permitted provided that the following conditions |
||
11 |
* are met: |
||
12 |
* |
||
13 |
* * Redistributions of source code must retain the above copyright |
||
14 |
* notice, this list of conditions and the following disclaimer. |
||
15 |
* * Redistributions in binary form must reproduce the above |
||
16 |
* copyright notice, this list of conditions and the following |
||
17 |
* disclaimer in the documentation and/or other materials provided |
||
18 |
* with the distribution. |
||
19 |
* * Neither the name of Open Source Robotics Foundation nor the names of its |
||
20 |
* contributors may be used to endorse or promote products derived |
||
21 |
* from this software without specific prior written permission. |
||
22 |
* |
||
23 |
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||
24 |
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||
25 |
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||
26 |
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||
27 |
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||
28 |
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||
29 |
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
||
30 |
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
||
31 |
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||
32 |
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||
33 |
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||
34 |
* POSSIBILITY OF SUCH DAMAGE. |
||
35 |
*/ |
||
36 |
/** @author Jia Pan */ |
||
37 |
|||
38 |
#define BOOST_TEST_MODULE BROADPHASE_COLLISION_1 |
||
39 |
#include <boost/test/included/unit_test.hpp> |
||
40 |
|||
41 |
#include "hpp/fcl/broadphase/broadphase_bruteforce.h" |
||
42 |
#include "hpp/fcl/broadphase/broadphase_spatialhash.h" |
||
43 |
#include "hpp/fcl/broadphase/broadphase_SaP.h" |
||
44 |
#include "hpp/fcl/broadphase/broadphase_SSaP.h" |
||
45 |
#include "hpp/fcl/broadphase/broadphase_interval_tree.h" |
||
46 |
#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h" |
||
47 |
#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h" |
||
48 |
#include "hpp/fcl/broadphase/default_broadphase_callbacks.h" |
||
49 |
#include "hpp/fcl/broadphase/detail/sparse_hash_table.h" |
||
50 |
#include "hpp/fcl/broadphase/detail/spatial_hash.h" |
||
51 |
#include "utility.h" |
||
52 |
|||
53 |
#include <boost/math/constants/constants.hpp> |
||
54 |
|||
55 |
#if USE_GOOGLEHASH |
||
56 |
#include <sparsehash/sparse_hash_map> |
||
57 |
#include <sparsehash/dense_hash_map> |
||
58 |
#include <hash_map> |
||
59 |
#endif |
||
60 |
|||
61 |
#include <iostream> |
||
62 |
#include <iomanip> |
||
63 |
|||
64 |
using namespace hpp::fcl; |
||
65 |
|||
66 |
/// @brief make sure if broadphase algorithms doesn't check twice for the same |
||
67 |
/// collision object pair |
||
68 |
void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size, |
||
69 |
bool verbose = false); |
||
70 |
|||
71 |
/// @brief test for broad phase update |
||
72 |
void broad_phase_update_collision_test(FCL_REAL env_scale, std::size_t env_size, |
||
73 |
std::size_t query_size, |
||
74 |
std::size_t num_max_contacts = 1, |
||
75 |
bool exhaustive = false, |
||
76 |
bool use_mesh = false); |
||
77 |
|||
78 |
#if USE_GOOGLEHASH |
||
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>> {}; |
||
83 |
|||
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); |
||
92 |
} |
||
93 |
}; |
||
94 |
#endif |
||
95 |
|||
96 |
/// make sure if broadphase algorithms doesn't check twice for the same |
||
97 |
/// collision object pair |
||
98 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_broad_phase_dont_duplicate_check) { |
99 |
#ifdef NDEBUG |
||
100 |
broad_phase_duplicate_check_test(2000, 1000); |
||
101 |
#else |
||
102 |
2 |
broad_phase_duplicate_check_test(2000, 100); |
|
103 |
#endif |
||
104 |
2 |
} |
|
105 |
|||
106 |
/// check the update, only return collision or not |
||
107 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_update_collision_binary) { |
108 |
#ifdef NDEBUG |
||
109 |
broad_phase_update_collision_test(2000, 100, 1000, 1, false); |
||
110 |
broad_phase_update_collision_test(2000, 1000, 1000, 1, false); |
||
111 |
#else |
||
112 |
2 |
broad_phase_update_collision_test(2000, 10, 100, 1, false); |
|
113 |
2 |
broad_phase_update_collision_test(2000, 100, 100, 1, false); |
|
114 |
#endif |
||
115 |
2 |
} |
|
116 |
|||
117 |
/// check the update, return 10 contacts |
||
118 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_update_collision) { |
119 |
#ifdef NDEBUG |
||
120 |
broad_phase_update_collision_test(2000, 100, 1000, 10, false); |
||
121 |
broad_phase_update_collision_test(2000, 1000, 1000, 10, false); |
||
122 |
#else |
||
123 |
2 |
broad_phase_update_collision_test(2000, 10, 100, 10, false); |
|
124 |
2 |
broad_phase_update_collision_test(2000, 100, 100, 10, false); |
|
125 |
#endif |
||
126 |
2 |
} |
|
127 |
|||
128 |
/// check the update, exhaustive |
||
129 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_update_collision_exhaustive) { |
130 |
#ifdef NDEBUG |
||
131 |
broad_phase_update_collision_test(2000, 100, 1000, 1, true); |
||
132 |
broad_phase_update_collision_test(2000, 1000, 1000, 1, true); |
||
133 |
#else |
||
134 |
2 |
broad_phase_update_collision_test(2000, 10, 100, 1, true); |
|
135 |
2 |
broad_phase_update_collision_test(2000, 100, 100, 1, true); |
|
136 |
#endif |
||
137 |
2 |
} |
|
138 |
|||
139 |
/// check broad phase update, in mesh, only return collision or not |
||
140 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE( |
141 |
test_core_mesh_bf_broad_phase_update_collision_mesh_binary) { |
||
142 |
#ifdef NDEBUG |
||
143 |
broad_phase_update_collision_test(2000, 100, 1000, 1, false, true); |
||
144 |
broad_phase_update_collision_test(2000, 1000, 1000, 1, false, true); |
||
145 |
#else |
||
146 |
2 |
broad_phase_update_collision_test(2000, 2, 4, 1, false, true); |
|
147 |
2 |
broad_phase_update_collision_test(2000, 4, 4, 1, false, true); |
|
148 |
#endif |
||
149 |
2 |
} |
|
150 |
|||
151 |
/// check broad phase update, in mesh, return 10 contacts |
||
152 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_update_collision_mesh) { |
153 |
#ifdef NDEBUG |
||
154 |
broad_phase_update_collision_test(2000, 100, 1000, 10, false, true); |
||
155 |
broad_phase_update_collision_test(2000, 1000, 1000, 10, false, true); |
||
156 |
#else |
||
157 |
2 |
broad_phase_update_collision_test(200, 2, 4, 10, false, true); |
|
158 |
2 |
broad_phase_update_collision_test(200, 4, 4, 10, false, true); |
|
159 |
#endif |
||
160 |
2 |
} |
|
161 |
|||
162 |
/// check broad phase update, in mesh, exhaustive |
||
163 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE( |
164 |
test_core_mesh_bf_broad_phase_update_collision_mesh_exhaustive) { |
||
165 |
#ifdef NDEBUG |
||
166 |
broad_phase_update_collision_test(2000, 100, 1000, 1, true, true); |
||
167 |
broad_phase_update_collision_test(2000, 1000, 1000, 1, true, true); |
||
168 |
#else |
||
169 |
2 |
broad_phase_update_collision_test(2000, 2, 4, 1, true, true); |
|
170 |
2 |
broad_phase_update_collision_test(2000, 4, 4, 1, true, true); |
|
171 |
#endif |
||
172 |
2 |
} |
|
173 |
|||
174 |
//============================================================================== |
||
175 |
struct CollisionDataForUniquenessChecking { |
||
176 |
std::set<std::pair<CollisionObject*, CollisionObject*>> checkedPairs; |
||
177 |
|||
178 |
345 |
bool checkUniquenessAndAddPair(CollisionObject* o1, CollisionObject* o2) { |
|
179 |
✓✗✓✗ |
345 |
auto search = checkedPairs.find(std::make_pair(o1, o2)); |
180 |
|||
181 |
✗✓ | 345 |
if (search != checkedPairs.end()) return false; |
182 |
|||
183 |
✓✗ | 345 |
checkedPairs.emplace(o1, o2); |
184 |
|||
185 |
345 |
return true; |
|
186 |
} |
||
187 |
}; |
||
188 |
|||
189 |
//============================================================================== |
||
190 |
struct CollisionFunctionForUniquenessChecking : CollisionCallBackBase { |
||
191 |
345 |
bool collide(CollisionObject* o1, CollisionObject* o2) { |
|
192 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
345 |
BOOST_CHECK(data.checkUniquenessAndAddPair(o1, o2)); |
193 |
345 |
return false; |
|
194 |
} |
||
195 |
|||
196 |
CollisionDataForUniquenessChecking data; |
||
197 |
}; |
||
198 |
|||
199 |
//============================================================================== |
||
200 |
1 |
void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size, |
|
201 |
bool verbose) { |
||
202 |
1 |
std::vector<TStruct> ts; |
|
203 |
1 |
std::vector<BenchTimer> timers; |
|
204 |
|||
205 |
1 |
std::vector<CollisionObject*> env; |
|
206 |
✓✗ | 1 |
generateEnvironments(env, env_scale, env_size); |
207 |
|||
208 |
1 |
std::vector<BroadPhaseCollisionManager*> managers; |
|
209 |
✓✗✓✗ ✓✗ |
1 |
managers.push_back(new NaiveCollisionManager()); |
210 |
✓✗✓✗ ✓✗ |
1 |
managers.push_back(new SSaPCollisionManager()); |
211 |
✓✗✓✗ ✓✗ |
1 |
managers.push_back(new SaPCollisionManager()); |
212 |
✓✗✓✗ ✓✗ |
1 |
managers.push_back(new IntervalTreeCollisionManager()); |
213 |
✓✗✓✗ |
1 |
Vec3f lower_limit, upper_limit; |
214 |
✓✗ | 1 |
SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); |
215 |
FCL_REAL cell_size = |
||
216 |
✓✗ | 1 |
std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, |
217 |
✓✗✓✗ |
1 |
(upper_limit[1] - lower_limit[1]) / 20), |
218 |
✓✗✓✗ ✓✗ |
2 |
(upper_limit[2] - lower_limit[2]) / 20); |
219 |
1 |
managers.push_back( |
|
220 |
✓✗ | 1 |
new SpatialHashingCollisionManager< |
221 |
detail::SparseHashTable<AABB, CollisionObject*, detail::SpatialHash>>( |
||
222 |
✓✗✓✗ |
1 |
cell_size, lower_limit, upper_limit)); |
223 |
#if USE_GOOGLEHASH |
||
224 |
managers.push_back( |
||
225 |
new SpatialHashingCollisionManager<detail::SparseHashTable< |
||
226 |
AABB, CollisionObject*, detail::SpatialHash, GoogleSparseHashTable>>( |
||
227 |
cell_size, lower_limit, upper_limit)); |
||
228 |
managers.push_back( |
||
229 |
new SpatialHashingCollisionManager<detail::SparseHashTable< |
||
230 |
AABB, CollisionObject*, detail::SpatialHash, GoogleDenseHashTable>>( |
||
231 |
cell_size, lower_limit, upper_limit)); |
||
232 |
#endif |
||
233 |
✓✗✓✗ ✓✗ |
1 |
managers.push_back(new DynamicAABBTreeCollisionManager()); |
234 |
✓✗✓✗ ✓✗ |
1 |
managers.push_back(new DynamicAABBTreeArrayCollisionManager()); |
235 |
|||
236 |
{ |
||
237 |
✓✗✓✗ |
1 |
DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager(); |
238 |
1 |
m->tree_init_level = 2; |
|
239 |
✓✗ | 1 |
managers.push_back(m); |
240 |
} |
||
241 |
|||
242 |
{ |
||
243 |
DynamicAABBTreeArrayCollisionManager* m = |
||
244 |
✓✗✓✗ |
1 |
new DynamicAABBTreeArrayCollisionManager(); |
245 |
1 |
m->tree_init_level = 2; |
|
246 |
✓✗ | 1 |
managers.push_back(m); |
247 |
} |
||
248 |
|||
249 |
✓✗ | 1 |
ts.resize(managers.size()); |
250 |
✓✗ | 1 |
timers.resize(managers.size()); |
251 |
|||
252 |
✓✓ | 10 |
for (size_t i = 0; i < managers.size(); ++i) { |
253 |
✓✗ | 9 |
timers[i].start(); |
254 |
✓✗ | 9 |
managers[i]->registerObjects(env); |
255 |
✓✗ | 9 |
timers[i].stop(); |
256 |
✓✗✓✗ |
9 |
ts[i].push_back(timers[i].getElapsedTime()); |
257 |
} |
||
258 |
|||
259 |
✓✓ | 10 |
for (size_t i = 0; i < managers.size(); ++i) { |
260 |
✓✗ | 9 |
timers[i].start(); |
261 |
✓✗ | 9 |
managers[i]->setup(); |
262 |
✓✗ | 9 |
timers[i].stop(); |
263 |
✓✗✓✗ |
9 |
ts[i].push_back(timers[i].getElapsedTime()); |
264 |
} |
||
265 |
|||
266 |
// update the environment |
||
267 |
1 |
FCL_REAL delta_angle_max = |
|
268 |
10 / 360.0 * 2 * boost::math::constants::pi<FCL_REAL>(); |
||
269 |
1 |
FCL_REAL delta_trans_max = 0.01 * env_scale; |
|
270 |
✓✓ | 301 |
for (size_t i = 0; i < env.size(); ++i) { |
271 |
FCL_REAL rand_angle_x = |
||
272 |
300 |
2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; |
|
273 |
FCL_REAL rand_trans_x = |
||
274 |
300 |
2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; |
|
275 |
FCL_REAL rand_angle_y = |
||
276 |
300 |
2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; |
|
277 |
FCL_REAL rand_trans_y = |
||
278 |
300 |
2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; |
|
279 |
FCL_REAL rand_angle_z = |
||
280 |
300 |
2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; |
|
281 |
FCL_REAL rand_trans_z = |
||
282 |
300 |
2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; |
|
283 |
|||
284 |
✓✗✓✗ ✓✗ |
300 |
Matrix3f dR(Eigen::AngleAxisd(rand_angle_x, Vec3f::UnitX()) * |
285 |
✓✗✓✗ ✓✗ |
600 |
Eigen::AngleAxisd(rand_angle_y, Vec3f::UnitY()) * |
286 |
✓✗✓✗ ✓✗ |
600 |
Eigen::AngleAxisd(rand_angle_z, Vec3f::UnitZ())); |
287 |
✓✗ | 300 |
Vec3f dT(rand_trans_x, rand_trans_y, rand_trans_z); |
288 |
|||
289 |
✓✗ | 300 |
Matrix3f R = env[i]->getRotation(); |
290 |
✓✗ | 300 |
Vec3f T = env[i]->getTranslation(); |
291 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
300 |
env[i]->setTransform(dR * R, dR * T + dT); |
292 |
✓✗ | 300 |
env[i]->computeAABB(); |
293 |
} |
||
294 |
|||
295 |
✓✓ | 10 |
for (size_t i = 0; i < managers.size(); ++i) { |
296 |
✓✗ | 9 |
timers[i].start(); |
297 |
✓✗ | 9 |
managers[i]->update(); |
298 |
✓✗ | 9 |
timers[i].stop(); |
299 |
✓✗✓✗ |
9 |
ts[i].push_back(timers[i].getElapsedTime()); |
300 |
} |
||
301 |
|||
302 |
✓✗ | 1 |
std::vector<CollisionDataForUniquenessChecking> self_data(managers.size()); |
303 |
|||
304 |
✓✓ | 10 |
for (size_t i = 0; i < managers.size(); ++i) { |
305 |
18 |
CollisionFunctionForUniquenessChecking callback; |
|
306 |
✓✗ | 9 |
timers[i].start(); |
307 |
✓✗ | 9 |
managers[i]->collide(&callback); |
308 |
✓✗ | 9 |
timers[i].stop(); |
309 |
✓✗✓✗ |
9 |
ts[i].push_back(timers[i].getElapsedTime()); |
310 |
} |
||
311 |
|||
312 |
✓✓✓✗ |
301 |
for (auto obj : env) delete obj; |
313 |
|||
314 |
✓✗ | 1 |
if (!verbose) return; |
315 |
|||
316 |
std::cout.setf(std::ios_base::left, std::ios_base::adjustfield); |
||
317 |
int w = 7; |
||
318 |
|||
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; |
||
325 |
|||
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; |
||
330 |
|||
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; |
||
335 |
|||
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; |
||
340 |
|||
341 |
std::cout << "collision time" << std::endl; |
||
342 |
for (size_t i = 0; i < ts.size(); ++i) { |
||
343 |
FCL_REAL tmp = 0; |
||
344 |
for (size_t j = 4; j < ts[i].records.size(); ++j) tmp += ts[i].records[j]; |
||
345 |
std::cout << std::setw(w) << tmp << " "; |
||
346 |
} |
||
347 |
std::cout << std::endl; |
||
348 |
|||
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; |
||
354 |
} |
||
355 |
|||
356 |
12 |
void broad_phase_update_collision_test(FCL_REAL env_scale, std::size_t env_size, |
|
357 |
std::size_t query_size, |
||
358 |
std::size_t num_max_contacts, |
||
359 |
bool exhaustive, bool use_mesh) { |
||
360 |
24 |
std::vector<TStruct> ts; |
|
361 |
24 |
std::vector<BenchTimer> timers; |
|
362 |
|||
363 |
24 |
std::vector<CollisionObject*> env; |
|
364 |
✓✓ | 12 |
if (use_mesh) |
365 |
✓✗ | 6 |
generateEnvironmentsMesh(env, env_scale, env_size); |
366 |
else |
||
367 |
✓✗ | 6 |
generateEnvironments(env, env_scale, env_size); |
368 |
|||
369 |
24 |
std::vector<CollisionObject*> query; |
|
370 |
✓✓ | 12 |
if (use_mesh) |
371 |
✓✗ | 6 |
generateEnvironmentsMesh(query, env_scale, query_size); |
372 |
else |
||
373 |
✓✗ | 6 |
generateEnvironments(query, env_scale, query_size); |
374 |
|||
375 |
24 |
std::vector<BroadPhaseCollisionManager*> managers; |
|
376 |
|||
377 |
✓✗✓✗ ✓✗ |
12 |
managers.push_back(new NaiveCollisionManager()); |
378 |
✓✗✓✗ ✓✗ |
12 |
managers.push_back(new SSaPCollisionManager()); |
379 |
|||
380 |
✓✗✓✗ ✓✗ |
12 |
managers.push_back(new SaPCollisionManager()); |
381 |
✓✗✓✗ ✓✗ |
12 |
managers.push_back(new IntervalTreeCollisionManager()); |
382 |
|||
383 |
✓✗✓✗ |
12 |
Vec3f lower_limit, upper_limit; |
384 |
✓✗ | 12 |
SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); |
385 |
FCL_REAL cell_size = |
||
386 |
✓✗ | 12 |
std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, |
387 |
✓✗✓✗ |
12 |
(upper_limit[1] - lower_limit[1]) / 20), |
388 |
✓✗✓✗ ✓✗ |
24 |
(upper_limit[2] - lower_limit[2]) / 20); |
389 |
// managers.push_back(new SpatialHashingCollisionManager(cell_size, |
||
390 |
// lower_limit, upper_limit)); |
||
391 |
12 |
managers.push_back( |
|
392 |
✓✗ | 12 |
new SpatialHashingCollisionManager< |
393 |
detail::SparseHashTable<AABB, CollisionObject*, detail::SpatialHash>>( |
||
394 |
✓✗✓✗ |
12 |
cell_size, lower_limit, upper_limit)); |
395 |
#if USE_GOOGLEHASH |
||
396 |
managers.push_back( |
||
397 |
new SpatialHashingCollisionManager<detail::SparseHashTable< |
||
398 |
AABB, CollisionObject*, detail::SpatialHash, GoogleSparseHashTable>>( |
||
399 |
cell_size, lower_limit, upper_limit)); |
||
400 |
managers.push_back( |
||
401 |
new SpatialHashingCollisionManager<detail::SparseHashTable< |
||
402 |
AABB, CollisionObject*, detail::SpatialHash, GoogleDenseHashTable>>( |
||
403 |
cell_size, lower_limit, upper_limit)); |
||
404 |
#endif |
||
405 |
✓✗✓✗ ✓✗ |
12 |
managers.push_back(new DynamicAABBTreeCollisionManager()); |
406 |
✓✗✓✗ ✓✗ |
12 |
managers.push_back(new DynamicAABBTreeArrayCollisionManager()); |
407 |
|||
408 |
{ |
||
409 |
✓✗✓✗ |
12 |
DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager(); |
410 |
12 |
m->tree_init_level = 2; |
|
411 |
✓✗ | 12 |
managers.push_back(m); |
412 |
} |
||
413 |
|||
414 |
{ |
||
415 |
DynamicAABBTreeArrayCollisionManager* m = |
||
416 |
✓✗✓✗ |
12 |
new DynamicAABBTreeArrayCollisionManager(); |
417 |
12 |
m->tree_init_level = 2; |
|
418 |
✓✗ | 12 |
managers.push_back(m); |
419 |
} |
||
420 |
|||
421 |
✓✗ | 12 |
ts.resize(managers.size()); |
422 |
✓✗ | 12 |
timers.resize(managers.size()); |
423 |
|||
424 |
✓✓ | 120 |
for (size_t i = 0; i < managers.size(); ++i) { |
425 |
✓✗ | 108 |
timers[i].start(); |
426 |
✓✗ | 108 |
managers[i]->registerObjects(env); |
427 |
✓✗ | 108 |
timers[i].stop(); |
428 |
✓✗✓✗ |
108 |
ts[i].push_back(timers[i].getElapsedTime()); |
429 |
} |
||
430 |
|||
431 |
✓✓ | 120 |
for (size_t i = 0; i < managers.size(); ++i) { |
432 |
✓✗ | 108 |
timers[i].start(); |
433 |
✓✗ | 108 |
managers[i]->setup(); |
434 |
✓✗ | 108 |
timers[i].stop(); |
435 |
✓✗✓✗ |
108 |
ts[i].push_back(timers[i].getElapsedTime()); |
436 |
} |
||
437 |
|||
438 |
// update the environment |
||
439 |
12 |
FCL_REAL delta_angle_max = |
|
440 |
10 / 360.0 * 2 * boost::math::constants::pi<FCL_REAL>(); |
||
441 |
12 |
FCL_REAL delta_trans_max = 0.01 * env_scale; |
|
442 |
✓✓ | 1056 |
for (size_t i = 0; i < env.size(); ++i) { |
443 |
FCL_REAL rand_angle_x = |
||
444 |
1044 |
2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; |
|
445 |
FCL_REAL rand_trans_x = |
||
446 |
1044 |
2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; |
|
447 |
FCL_REAL rand_angle_y = |
||
448 |
1044 |
2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; |
|
449 |
FCL_REAL rand_trans_y = |
||
450 |
1044 |
2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; |
|
451 |
FCL_REAL rand_angle_z = |
||
452 |
1044 |
2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; |
|
453 |
FCL_REAL rand_trans_z = |
||
454 |
1044 |
2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; |
|
455 |
|||
456 |
✓✗✓✗ ✓✗ |
1044 |
Matrix3f dR(Eigen::AngleAxisd(rand_angle_x, Vec3f::UnitX()) * |
457 |
✓✗✓✗ ✓✗ |
2088 |
Eigen::AngleAxisd(rand_angle_y, Vec3f::UnitY()) * |
458 |
✓✗✓✗ ✓✗ |
2088 |
Eigen::AngleAxisd(rand_angle_z, Vec3f::UnitZ())); |
459 |
✓✗ | 1044 |
Vec3f dT(rand_trans_x, rand_trans_y, rand_trans_z); |
460 |
|||
461 |
✓✗ | 1044 |
Matrix3f R = env[i]->getRotation(); |
462 |
✓✗ | 1044 |
Vec3f T = env[i]->getTranslation(); |
463 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1044 |
env[i]->setTransform(dR * R, dR * T + dT); |
464 |
✓✗ | 1044 |
env[i]->computeAABB(); |
465 |
} |
||
466 |
|||
467 |
✓✓ | 120 |
for (size_t i = 0; i < managers.size(); ++i) { |
468 |
✓✗ | 108 |
timers[i].start(); |
469 |
✓✗ | 108 |
managers[i]->update(); |
470 |
✓✗ | 108 |
timers[i].stop(); |
471 |
✓✗✓✗ |
108 |
ts[i].push_back(timers[i].getElapsedTime()); |
472 |
} |
||
473 |
|||
474 |
✓✗ | 24 |
std::vector<CollisionData> self_data(managers.size()); |
475 |
✓✓ | 120 |
for (size_t i = 0; i < managers.size(); ++i) { |
476 |
✓✓ | 108 |
if (exhaustive) |
477 |
36 |
self_data[i].request.num_max_contacts = 100000; |
|
478 |
else |
||
479 |
72 |
self_data[i].request.num_max_contacts = num_max_contacts; |
|
480 |
} |
||
481 |
|||
482 |
✓✓ | 120 |
for (size_t i = 0; i < managers.size(); ++i) { |
483 |
✓✗ | 216 |
CollisionCallBackDefault callback; |
484 |
✓✗ | 108 |
timers[i].start(); |
485 |
✓✗ | 108 |
managers[i]->collide(&callback); |
486 |
✓✗ | 108 |
timers[i].stop(); |
487 |
✓✗✓✗ |
108 |
ts[i].push_back(timers[i].getElapsedTime()); |
488 |
} |
||
489 |
|||
490 |
✓✓ | 120 |
for (size_t i = 0; i < managers.size(); ++i) |
491 |
✓✗✓✗ |
108 |
std::cout << self_data[i].result.numContacts() << " "; |
492 |
✓✗ | 12 |
std::cout << std::endl; |
493 |
|||
494 |
✓✓ | 12 |
if (exhaustive) { |
495 |
✓✓ | 36 |
for (size_t i = 1; i < managers.size(); ++i) |
496 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
32 |
BOOST_CHECK(self_data[i].result.numContacts() == |
497 |
self_data[0].result.numContacts()); |
||
498 |
} else { |
||
499 |
✓✗ | 16 |
std::vector<bool> self_res(managers.size()); |
500 |
✓✓ | 80 |
for (size_t i = 0; i < self_res.size(); ++i) |
501 |
72 |
self_res[i] = (self_data[i].result.numContacts() > 0); |
|
502 |
|||
503 |
✓✓ | 72 |
for (size_t i = 1; i < self_res.size(); ++i) |
504 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
64 |
BOOST_CHECK(self_res[0] == self_res[i]); |
505 |
|||
506 |
✓✓ | 72 |
for (size_t i = 1; i < managers.size(); ++i) |
507 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
64 |
BOOST_CHECK(self_data[i].result.numContacts() == |
508 |
self_data[0].result.numContacts()); |
||
509 |
} |
||
510 |
|||
511 |
✓✓ | 1884 |
for (size_t i = 0; i < query.size(); ++i) { |
512 |
✓✗ | 3744 |
std::vector<CollisionCallBackDefault> query_callbacks(managers.size()); |
513 |
|||
514 |
✓✓ | 18720 |
for (size_t j = 0; j < query_callbacks.size(); ++j) { |
515 |
✓✓ | 16848 |
if (exhaustive) |
516 |
5616 |
query_callbacks[j].data.request.num_max_contacts = 100000; |
|
517 |
else |
||
518 |
11232 |
query_callbacks[j].data.request.num_max_contacts = num_max_contacts; |
|
519 |
} |
||
520 |
|||
521 |
✓✓ | 18720 |
for (size_t j = 0; j < query_callbacks.size(); ++j) { |
522 |
✓✗ | 16848 |
timers[j].start(); |
523 |
✓✗ | 16848 |
managers[j]->collide(query[i], &query_callbacks[j]); |
524 |
✓✗ | 16848 |
timers[j].stop(); |
525 |
✓✗✓✗ |
16848 |
ts[j].push_back(timers[j].getElapsedTime()); |
526 |
} |
||
527 |
|||
528 |
// for(size_t j = 0; j < managers.size(); ++j) |
||
529 |
// std::cout << query_callbacks[j].result.numContacts() << " "; |
||
530 |
// std::cout << std::endl; |
||
531 |
|||
532 |
✓✓ | 1872 |
if (exhaustive) { |
533 |
✓✓ | 5616 |
for (size_t j = 1; j < managers.size(); ++j) |
534 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
4992 |
BOOST_CHECK(query_callbacks[j].data.result.numContacts() == |
535 |
query_callbacks[0].data.result.numContacts()); |
||
536 |
} else { |
||
537 |
✓✗ | 2496 |
std::vector<bool> query_res(managers.size()); |
538 |
✓✓ | 12480 |
for (size_t j = 0; j < query_res.size(); ++j) |
539 |
11232 |
query_res[j] = (query_callbacks[j].data.result.numContacts() > 0); |
|
540 |
✓✓ | 11232 |
for (size_t j = 1; j < query_res.size(); ++j) |
541 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
9984 |
BOOST_CHECK(query_res[0] == query_res[j]); |
542 |
|||
543 |
✓✓ | 11232 |
for (size_t j = 1; j < managers.size(); ++j) |
544 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
9984 |
BOOST_CHECK(query_callbacks[j].data.result.numContacts() == |
545 |
query_callbacks[0].data.result.numContacts()); |
||
546 |
} |
||
547 |
} |
||
548 |
|||
549 |
✓✓✓✗ |
1056 |
for (size_t i = 0; i < env.size(); ++i) delete env[i]; |
550 |
✓✓✓✗ |
1884 |
for (size_t i = 0; i < query.size(); ++i) delete query[i]; |
551 |
|||
552 |
✓✓✓✗ |
120 |
for (size_t i = 0; i < managers.size(); ++i) delete managers[i]; |
553 |
|||
554 |
12 |
std::cout.setf(std::ios_base::left, std::ios_base::adjustfield); |
|
555 |
12 |
int w = 7; |
|
556 |
|||
557 |
✓✗✓✗ |
12 |
std::cout << "collision timing summary" << std::endl; |
558 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
12 |
std::cout << env_size << " objs, " << query_size << " queries" << std::endl; |
559 |
✓✗✓✗ |
12 |
std::cout << "register time" << std::endl; |
560 |
✓✓ | 120 |
for (size_t i = 0; i < ts.size(); ++i) |
561 |
✓✗✓✗ ✓✗ |
108 |
std::cout << std::setw(w) << ts[i].records[0] << " "; |
562 |
✓✗ | 12 |
std::cout << std::endl; |
563 |
|||
564 |
✓✗✓✗ |
12 |
std::cout << "setup time" << std::endl; |
565 |
✓✓ | 120 |
for (size_t i = 0; i < ts.size(); ++i) |
566 |
✓✗✓✗ ✓✗ |
108 |
std::cout << std::setw(w) << ts[i].records[1] << " "; |
567 |
✓✗ | 12 |
std::cout << std::endl; |
568 |
|||
569 |
✓✗✓✗ |
12 |
std::cout << "update time" << std::endl; |
570 |
✓✓ | 120 |
for (size_t i = 0; i < ts.size(); ++i) |
571 |
✓✗✓✗ ✓✗ |
108 |
std::cout << std::setw(w) << ts[i].records[2] << " "; |
572 |
✓✗ | 12 |
std::cout << std::endl; |
573 |
|||
574 |
✓✗✓✗ |
12 |
std::cout << "self collision time" << std::endl; |
575 |
✓✓ | 120 |
for (size_t i = 0; i < ts.size(); ++i) |
576 |
✓✗✓✗ ✓✗ |
108 |
std::cout << std::setw(w) << ts[i].records[3] << " "; |
577 |
✓✗ | 12 |
std::cout << std::endl; |
578 |
|||
579 |
✓✗✓✗ |
12 |
std::cout << "collision time" << std::endl; |
580 |
✓✓ | 120 |
for (size_t i = 0; i < ts.size(); ++i) { |
581 |
108 |
FCL_REAL tmp = 0; |
|
582 |
✓✓ | 16956 |
for (size_t j = 4; j < ts[i].records.size(); ++j) tmp += ts[i].records[j]; |
583 |
✓✗✓✗ ✓✗ |
108 |
std::cout << std::setw(w) << tmp << " "; |
584 |
} |
||
585 |
✓✗ | 12 |
std::cout << std::endl; |
586 |
|||
587 |
✓✗✓✗ |
12 |
std::cout << "overall time" << std::endl; |
588 |
✓✓ | 120 |
for (size_t i = 0; i < ts.size(); ++i) |
589 |
✓✗✓✗ ✓✗ |
108 |
std::cout << std::setw(w) << ts[i].overall_time << " "; |
590 |
✓✗ | 12 |
std::cout << std::endl; |
591 |
✓✗ | 12 |
std::cout << std::endl; |
592 |
12 |
} |
| Generated by: GCOVR (Version 4.2) |