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-2015, 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 |
#define BOOST_TEST_MODULE FCL_BROADPHASE |
||
39 |
#include <boost/test/included/unit_test.hpp> |
||
40 |
|||
41 |
#include <hpp/fcl/config.hh> |
||
42 |
#include <hpp/fcl/broadphase/broadphase.h> |
||
43 |
#include <hpp/fcl/shape/geometric_shape_to_BVH_model.h> |
||
44 |
#include <hpp/fcl/math/transform.h> |
||
45 |
#include "utility.h" |
||
46 |
|||
47 |
#if USE_GOOGLEHASH |
||
48 |
#include <sparsehash/sparse_hash_map> |
||
49 |
#include <sparsehash/dense_hash_map> |
||
50 |
#include <hash_map> |
||
51 |
#endif |
||
52 |
|||
53 |
#include <boost/math/constants/constants.hpp> |
||
54 |
#include <iostream> |
||
55 |
#include <iomanip> |
||
56 |
|||
57 |
using namespace hpp::fcl; |
||
58 |
using namespace hpp::fcl::detail; |
||
59 |
|||
60 |
/// @brief Generate environment with 3 * n objects for self distance, so we try |
||
61 |
/// to make sure none of them collide with each other. |
||
62 |
void generateSelfDistanceEnvironments(std::vector<CollisionObject*>& env, |
||
63 |
double env_scale, std::size_t n); |
||
64 |
|||
65 |
/// @brief Generate environment with 3 * n objects for self distance, but all in |
||
66 |
/// meshes. |
||
67 |
void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject*>& env, |
||
68 |
double env_scale, std::size_t n); |
||
69 |
|||
70 |
/// @brief test for broad phase distance |
||
71 |
void broad_phase_distance_test(double env_scale, std::size_t env_size, |
||
72 |
std::size_t query_size, bool use_mesh = false); |
||
73 |
|||
74 |
/// @brief test for broad phase self distance |
||
75 |
void broad_phase_self_distance_test(double env_scale, std::size_t env_size, |
||
76 |
bool use_mesh = false); |
||
77 |
|||
78 |
FCL_REAL DELTA = 0.01; |
||
79 |
|||
80 |
#if USE_GOOGLEHASH |
||
81 |
template <typename U, typename V> |
||
82 |
struct GoogleSparseHashTable |
||
83 |
: public google::sparse_hash_map<U, V, std::tr1::hash<size_t>, |
||
84 |
std::equal_to<size_t> > {}; |
||
85 |
|||
86 |
template <typename U, typename V> |
||
87 |
struct GoogleDenseHashTable |
||
88 |
: public google::dense_hash_map<U, V, std::tr1::hash<size_t>, |
||
89 |
std::equal_to<size_t> > { |
||
90 |
GoogleDenseHashTable() |
||
91 |
: google::dense_hash_map<U, V, std::tr1::hash<size_t>, |
||
92 |
std::equal_to<size_t> >() { |
||
93 |
this->set_empty_key(NULL); |
||
94 |
} |
||
95 |
}; |
||
96 |
#endif |
||
97 |
|||
98 |
// TODO(jcarpent): fix this test |
||
99 |
// - test_core_bf_broad_phase_distance |
||
100 |
|||
101 |
/// check broad phase distance |
||
102 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_distance) { |
103 |
#ifndef NDEBUG |
||
104 |
2 |
broad_phase_distance_test(200, 100, 10); |
|
105 |
2 |
broad_phase_distance_test(200, 1000, 10); |
|
106 |
2 |
broad_phase_distance_test(2000, 100, 10); |
|
107 |
2 |
broad_phase_distance_test(2000, 1000, 10); |
|
108 |
#else |
||
109 |
broad_phase_distance_test(200, 100, 100); |
||
110 |
broad_phase_distance_test(200, 1000, 100); |
||
111 |
broad_phase_distance_test(2000, 100, 100); |
||
112 |
broad_phase_distance_test(2000, 1000, 100); |
||
113 |
#endif |
||
114 |
2 |
} |
|
115 |
|||
116 |
/// check broad phase self distance |
||
117 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_self_distance) { |
118 |
2 |
broad_phase_self_distance_test(200, 512); |
|
119 |
2 |
broad_phase_self_distance_test(200, 1000); |
|
120 |
2 |
broad_phase_self_distance_test(200, 5000); |
|
121 |
2 |
} |
|
122 |
|||
123 |
/// check broad phase distance |
||
124 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_distance_mesh) { |
125 |
#ifndef NDEBUG |
||
126 |
2 |
broad_phase_distance_test(200, 10, 10, true); |
|
127 |
2 |
broad_phase_distance_test(200, 100, 10, true); |
|
128 |
2 |
broad_phase_distance_test(2000, 10, 10, true); |
|
129 |
2 |
broad_phase_distance_test(2000, 100, 10, true); |
|
130 |
#else |
||
131 |
broad_phase_distance_test(200, 100, 100, true); |
||
132 |
broad_phase_distance_test(200, 1000, 100, true); |
||
133 |
broad_phase_distance_test(2000, 100, 100, true); |
||
134 |
broad_phase_distance_test(2000, 1000, 100, true); |
||
135 |
#endif |
||
136 |
2 |
} |
|
137 |
|||
138 |
/// check broad phase self distance |
||
139 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_self_distance_mesh) { |
140 |
2 |
broad_phase_self_distance_test(200, 512, true); |
|
141 |
2 |
broad_phase_self_distance_test(200, 1000, true); |
|
142 |
2 |
broad_phase_self_distance_test(200, 5000, true); |
|
143 |
2 |
} |
|
144 |
|||
145 |
3 |
void generateSelfDistanceEnvironments(std::vector<CollisionObject*>& env, |
|
146 |
double env_scale, std::size_t n) { |
||
147 |
3 |
int n_edge = static_cast<int>(std::floor(std::pow(n, 1 / 3.0))); |
|
148 |
|||
149 |
3 |
FCL_REAL step_size = env_scale * 2 / n_edge; |
|
150 |
3 |
FCL_REAL delta_size = step_size * 0.05; |
|
151 |
3 |
FCL_REAL single_size = step_size - 2 * delta_size; |
|
152 |
|||
153 |
3 |
int i = 0; |
|
154 |
✓✓ | 1498 |
for (; i < n_edge * n_edge * n_edge / 4; ++i) { |
155 |
1495 |
int x = i % (n_edge * n_edge); |
|
156 |
1495 |
int y = (i - n_edge * n_edge * x) % n_edge; |
|
157 |
1495 |
int z = i - n_edge * n_edge * x - n_edge * y; |
|
158 |
|||
159 |
✓✗ | 1495 |
Box* box = new Box(single_size, single_size, single_size); |
160 |
✓✗ | 1495 |
env.push_back(new CollisionObject( |
161 |
1495 |
shared_ptr<CollisionGeometry>(box), |
|
162 |
✓✗ | 1495 |
Transform3f(Vec3f( |
163 |
x * step_size + delta_size + 0.5 * single_size - env_scale, |
||
164 |
y * step_size + delta_size + 0.5 * single_size - env_scale, |
||
165 |
✓✗✓✗ ✓✗ |
4485 |
z * step_size + delta_size + 0.5 * single_size - env_scale)))); |
166 |
1495 |
env.back()->collisionGeometry()->computeLocalAABB(); |
|
167 |
} |
||
168 |
|||
169 |
✗✓ | 3 |
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; |
||
173 |
|||
174 |
Sphere* sphere = new Sphere(single_size / 2); |
||
175 |
env.push_back(new CollisionObject( |
||
176 |
shared_ptr<CollisionGeometry>(sphere), |
||
177 |
Transform3f(Vec3f( |
||
178 |
x * step_size + delta_size + 0.5 * single_size - env_scale, |
||
179 |
y * step_size + delta_size + 0.5 * single_size - env_scale, |
||
180 |
z * step_size + delta_size + 0.5 * single_size - env_scale)))); |
||
181 |
env.back()->collisionGeometry()->computeLocalAABB(); |
||
182 |
} |
||
183 |
|||
184 |
✗✓ | 3 |
for (; i < n_edge * n_edge * n_edge / 4; ++i) { |
185 |
int x = i % (n_edge * n_edge); |
||
186 |
int y = (i - n_edge * n_edge * x) % n_edge; |
||
187 |
int z = i - n_edge * n_edge * x - n_edge * y; |
||
188 |
|||
189 |
Cylinder* cylinder = new Cylinder(single_size / 2, single_size); |
||
190 |
env.push_back(new CollisionObject( |
||
191 |
shared_ptr<CollisionGeometry>(cylinder), |
||
192 |
Transform3f(Vec3f( |
||
193 |
x * step_size + delta_size + 0.5 * single_size - env_scale, |
||
194 |
y * step_size + delta_size + 0.5 * single_size - env_scale, |
||
195 |
z * step_size + delta_size + 0.5 * single_size - env_scale)))); |
||
196 |
env.back()->collisionGeometry()->computeLocalAABB(); |
||
197 |
} |
||
198 |
|||
199 |
✗✓ | 3 |
for (; i < n_edge * n_edge * n_edge / 4; ++i) { |
200 |
int x = i % (n_edge * n_edge); |
||
201 |
int y = (i - n_edge * n_edge * x) % n_edge; |
||
202 |
int z = i - n_edge * n_edge * x - n_edge * y; |
||
203 |
|||
204 |
Cone* cone = new Cone(single_size / 2, single_size); |
||
205 |
env.push_back(new CollisionObject( |
||
206 |
shared_ptr<CollisionGeometry>(cone), |
||
207 |
Transform3f(Vec3f( |
||
208 |
x * step_size + delta_size + 0.5 * single_size - env_scale, |
||
209 |
y * step_size + delta_size + 0.5 * single_size - env_scale, |
||
210 |
z * step_size + delta_size + 0.5 * single_size - env_scale)))); |
||
211 |
env.back()->collisionGeometry()->computeLocalAABB(); |
||
212 |
} |
||
213 |
3 |
} |
|
214 |
|||
215 |
3 |
void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject*>& env, |
|
216 |
double env_scale, std::size_t n) { |
||
217 |
3 |
int n_edge = static_cast<int>(std::floor(std::pow(n, 1 / 3.0))); |
|
218 |
|||
219 |
3 |
FCL_REAL step_size = env_scale * 2 / n_edge; |
|
220 |
3 |
FCL_REAL delta_size = step_size * 0.05; |
|
221 |
3 |
FCL_REAL single_size = step_size - 2 * delta_size; |
|
222 |
|||
223 |
3 |
int i = 0; |
|
224 |
✓✓ | 1498 |
for (; i < n_edge * n_edge * n_edge / 4; ++i) { |
225 |
1495 |
int x = i % (n_edge * n_edge); |
|
226 |
1495 |
int y = (i - n_edge * n_edge * x) % n_edge; |
|
227 |
1495 |
int z = i - n_edge * n_edge * x - n_edge * y; |
|
228 |
|||
229 |
✓✗ | 2990 |
Box box(single_size, single_size, single_size); |
230 |
✓✗✓✗ |
1495 |
BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>(); |
231 |
✓✗✓✗ |
1495 |
generateBVHModel(*model, box, Transform3f()); |
232 |
✓✗ | 1495 |
env.push_back(new CollisionObject( |
233 |
✓✗ | 1495 |
shared_ptr<CollisionGeometry>(model), |
234 |
✓✗ | 1495 |
Transform3f(Vec3f( |
235 |
x * step_size + delta_size + 0.5 * single_size - env_scale, |
||
236 |
y * step_size + delta_size + 0.5 * single_size - env_scale, |
||
237 |
✓✗✓✗ ✓✗ |
4485 |
z * step_size + delta_size + 0.5 * single_size - env_scale)))); |
238 |
✓✗ | 1495 |
env.back()->collisionGeometry()->computeLocalAABB(); |
239 |
} |
||
240 |
|||
241 |
✗✓ | 3 |
for (; i < n_edge * n_edge * n_edge / 4; ++i) { |
242 |
int x = i % (n_edge * n_edge); |
||
243 |
int y = (i - n_edge * n_edge * x) % n_edge; |
||
244 |
int z = i - n_edge * n_edge * x - n_edge * y; |
||
245 |
|||
246 |
Sphere sphere(single_size / 2); |
||
247 |
BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>(); |
||
248 |
generateBVHModel(*model, sphere, Transform3f(), 16, 16); |
||
249 |
env.push_back(new CollisionObject( |
||
250 |
shared_ptr<CollisionGeometry>(model), |
||
251 |
Transform3f(Vec3f( |
||
252 |
x * step_size + delta_size + 0.5 * single_size - env_scale, |
||
253 |
y * step_size + delta_size + 0.5 * single_size - env_scale, |
||
254 |
z * step_size + delta_size + 0.5 * single_size - env_scale)))); |
||
255 |
env.back()->collisionGeometry()->computeLocalAABB(); |
||
256 |
} |
||
257 |
|||
258 |
✗✓ | 3 |
for (; i < n_edge * n_edge * n_edge / 4; ++i) { |
259 |
int x = i % (n_edge * n_edge); |
||
260 |
int y = (i - n_edge * n_edge * x) % n_edge; |
||
261 |
int z = i - n_edge * n_edge * x - n_edge * y; |
||
262 |
|||
263 |
Cylinder cylinder(single_size / 2, single_size); |
||
264 |
BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>(); |
||
265 |
generateBVHModel(*model, cylinder, Transform3f(), 16, 16); |
||
266 |
env.push_back(new CollisionObject( |
||
267 |
shared_ptr<CollisionGeometry>(model), |
||
268 |
Transform3f(Vec3f( |
||
269 |
x * step_size + delta_size + 0.5 * single_size - env_scale, |
||
270 |
y * step_size + delta_size + 0.5 * single_size - env_scale, |
||
271 |
z * step_size + delta_size + 0.5 * single_size - env_scale)))); |
||
272 |
env.back()->collisionGeometry()->computeLocalAABB(); |
||
273 |
} |
||
274 |
|||
275 |
✗✓ | 3 |
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; |
||
279 |
|||
280 |
Cone cone(single_size / 2, single_size); |
||
281 |
BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>(); |
||
282 |
generateBVHModel(*model, cone, Transform3f(), 16, 16); |
||
283 |
env.push_back(new CollisionObject( |
||
284 |
shared_ptr<CollisionGeometry>(model), |
||
285 |
Transform3f(Vec3f( |
||
286 |
x * step_size + delta_size + 0.5 * single_size - env_scale, |
||
287 |
y * step_size + delta_size + 0.5 * single_size - env_scale, |
||
288 |
z * step_size + delta_size + 0.5 * single_size - env_scale)))); |
||
289 |
env.back()->collisionGeometry()->computeLocalAABB(); |
||
290 |
} |
||
291 |
3 |
} |
|
292 |
|||
293 |
6 |
void broad_phase_self_distance_test(double env_scale, std::size_t env_size, |
|
294 |
bool use_mesh) { |
||
295 |
12 |
std::vector<TStruct> ts; |
|
296 |
12 |
std::vector<BenchTimer> timers; |
|
297 |
|||
298 |
12 |
std::vector<CollisionObject*> env; |
|
299 |
✓✓ | 6 |
if (use_mesh) |
300 |
✓✗ | 3 |
generateSelfDistanceEnvironmentsMesh(env, env_scale, env_size); |
301 |
else |
||
302 |
✓✗ | 3 |
generateSelfDistanceEnvironments(env, env_scale, env_size); |
303 |
|||
304 |
12 |
std::vector<BroadPhaseCollisionManager*> managers; |
|
305 |
|||
306 |
✓✗✓✗ ✓✗ |
6 |
managers.push_back(new NaiveCollisionManager()); |
307 |
✓✗✓✗ ✓✗ |
6 |
managers.push_back(new SSaPCollisionManager()); |
308 |
✓✗✓✗ ✓✗ |
6 |
managers.push_back(new SaPCollisionManager()); |
309 |
✓✗✓✗ ✓✗ |
6 |
managers.push_back(new IntervalTreeCollisionManager()); |
310 |
|||
311 |
✓✗✓✗ |
6 |
Vec3f lower_limit, upper_limit; |
312 |
✓✗ | 6 |
SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); |
313 |
✓✗ | 6 |
FCL_REAL cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 5, |
314 |
✓✗✓✗ |
6 |
(upper_limit[1] - lower_limit[1]) / 5), |
315 |
✓✗✓✗ ✓✗ |
12 |
(upper_limit[2] - lower_limit[2]) / 5); |
316 |
// managers.push_back(new SpatialHashingCollisionManager<>(cell_size, |
||
317 |
// lower_limit, upper_limit)); |
||
318 |
✓✗ | 6 |
managers.push_back(new SpatialHashingCollisionManager< |
319 |
SparseHashTable<AABB, CollisionObject*, SpatialHash> >( |
||
320 |
✓✗✓✗ |
6 |
cell_size, lower_limit, upper_limit)); |
321 |
#if USE_GOOGLEHASH |
||
322 |
managers.push_back( |
||
323 |
new SpatialHashingCollisionManager<SparseHashTable< |
||
324 |
AABB, CollisionObject*, SpatialHash, GoogleSparseHashTable> >( |
||
325 |
cell_size, lower_limit, upper_limit)); |
||
326 |
managers.push_back( |
||
327 |
new SpatialHashingCollisionManager<SparseHashTable< |
||
328 |
AABB, CollisionObject*, SpatialHash, GoogleDenseHashTable> >( |
||
329 |
cell_size, lower_limit, upper_limit)); |
||
330 |
#endif |
||
331 |
✓✗✓✗ ✓✗ |
6 |
managers.push_back(new DynamicAABBTreeCollisionManager()); |
332 |
✓✗✓✗ ✓✗ |
6 |
managers.push_back(new DynamicAABBTreeArrayCollisionManager()); |
333 |
|||
334 |
{ |
||
335 |
✓✗✓✗ |
6 |
DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager(); |
336 |
6 |
m->tree_init_level = 2; |
|
337 |
✓✗ | 6 |
managers.push_back(m); |
338 |
} |
||
339 |
|||
340 |
{ |
||
341 |
DynamicAABBTreeArrayCollisionManager* m = |
||
342 |
✓✗✓✗ |
6 |
new DynamicAABBTreeArrayCollisionManager(); |
343 |
6 |
m->tree_init_level = 2; |
|
344 |
✓✗ | 6 |
managers.push_back(m); |
345 |
} |
||
346 |
|||
347 |
✓✗ | 6 |
ts.resize(managers.size()); |
348 |
✓✗ | 6 |
timers.resize(managers.size()); |
349 |
|||
350 |
✓✓ | 60 |
for (size_t i = 0; i < managers.size(); ++i) { |
351 |
✓✗ | 54 |
timers[i].start(); |
352 |
✓✗ | 54 |
managers[i]->registerObjects(env); |
353 |
✓✗ | 54 |
timers[i].stop(); |
354 |
✓✗✓✗ |
54 |
ts[i].push_back(timers[i].getElapsedTime()); |
355 |
} |
||
356 |
|||
357 |
✓✓ | 60 |
for (size_t i = 0; i < managers.size(); ++i) { |
358 |
✓✗ | 54 |
timers[i].start(); |
359 |
✓✗ | 54 |
managers[i]->setup(); |
360 |
✓✗ | 54 |
timers[i].stop(); |
361 |
✓✗✓✗ |
54 |
ts[i].push_back(timers[i].getElapsedTime()); |
362 |
} |
||
363 |
|||
364 |
✓✗ | 12 |
std::vector<DistanceCallBackDefault> self_callbacks(managers.size()); |
365 |
|||
366 |
✓✓ | 60 |
for (size_t i = 0; i < self_callbacks.size(); ++i) { |
367 |
✓✗ | 54 |
timers[i].start(); |
368 |
✓✗ | 54 |
managers[i]->distance(&self_callbacks[i]); |
369 |
✓✗ | 54 |
timers[i].stop(); |
370 |
✓✗✓✗ |
54 |
ts[i].push_back(timers[i].getElapsedTime()); |
371 |
// std::cout << self_data[i].result.min_distance << " "; |
||
372 |
} |
||
373 |
// std::cout << std::endl; |
||
374 |
|||
375 |
✓✓ | 54 |
for (size_t i = 1; i < managers.size(); ++i) |
376 |
✓✗✓✗ ✓✗✗✓ ✗✗✓✗ ✓✗✗✓ |
48 |
BOOST_CHECK(fabs(self_callbacks[0].data.result.min_distance - |
377 |
self_callbacks[i].data.result.min_distance) < DELTA || |
||
378 |
fabs(self_callbacks[0].data.result.min_distance - |
||
379 |
self_callbacks[i].data.result.min_distance) / |
||
380 |
fabs(self_callbacks[0].data.result.min_distance) < |
||
381 |
DELTA); |
||
382 |
|||
383 |
✓✓✓✗ |
2996 |
for (size_t i = 0; i < env.size(); ++i) delete env[i]; |
384 |
|||
385 |
✓✓✓✗ |
60 |
for (size_t i = 0; i < managers.size(); ++i) delete managers[i]; |
386 |
|||
387 |
6 |
std::cout.setf(std::ios_base::left, std::ios_base::adjustfield); |
|
388 |
6 |
int w = 7; |
|
389 |
|||
390 |
✓✗✓✗ |
6 |
std::cout << "self distance timing summary" << std::endl; |
391 |
✓✗✓✗ ✓✗ |
6 |
std::cout << env.size() << " objs" << std::endl; |
392 |
✓✗✓✗ |
6 |
std::cout << "register time" << std::endl; |
393 |
✓✓ | 60 |
for (size_t i = 0; i < ts.size(); ++i) |
394 |
✓✗✓✗ ✓✗ |
54 |
std::cout << std::setw(w) << ts[i].records[0] << " "; |
395 |
✓✗ | 6 |
std::cout << std::endl; |
396 |
|||
397 |
✓✗✓✗ |
6 |
std::cout << "setup time" << std::endl; |
398 |
✓✓ | 60 |
for (size_t i = 0; i < ts.size(); ++i) |
399 |
✓✗✓✗ ✓✗ |
54 |
std::cout << std::setw(w) << ts[i].records[1] << " "; |
400 |
✓✗ | 6 |
std::cout << std::endl; |
401 |
|||
402 |
✓✗✓✗ |
6 |
std::cout << "self distance time" << std::endl; |
403 |
✓✓ | 60 |
for (size_t i = 0; i < ts.size(); ++i) |
404 |
✓✗✓✗ ✓✗ |
54 |
std::cout << std::setw(w) << ts[i].records[2] << " "; |
405 |
✓✗ | 6 |
std::cout << std::endl; |
406 |
|||
407 |
✓✗✓✗ |
6 |
std::cout << "overall time" << std::endl; |
408 |
✓✓ | 60 |
for (size_t i = 0; i < ts.size(); ++i) |
409 |
✓✗✓✗ ✓✗ |
54 |
std::cout << std::setw(w) << ts[i].overall_time << " "; |
410 |
✓✗ | 6 |
std::cout << std::endl; |
411 |
✓✗ | 6 |
std::cout << std::endl; |
412 |
6 |
} |
|
413 |
|||
414 |
8 |
void broad_phase_distance_test(double env_scale, std::size_t env_size, |
|
415 |
std::size_t query_size, bool use_mesh) { |
||
416 |
16 |
std::vector<TStruct> ts; |
|
417 |
16 |
std::vector<BenchTimer> timers; |
|
418 |
|||
419 |
16 |
std::vector<CollisionObject*> env; |
|
420 |
✓✓ | 8 |
if (use_mesh) |
421 |
✓✗ | 4 |
generateEnvironmentsMesh(env, env_scale, env_size); |
422 |
else |
||
423 |
✓✗ | 4 |
generateEnvironments(env, env_scale, env_size); |
424 |
|||
425 |
16 |
std::vector<CollisionObject*> query; |
|
426 |
|||
427 |
✓✗✓✗ |
8 |
BroadPhaseCollisionManager* manager = new NaiveCollisionManager(); |
428 |
✓✓✓✗ |
7268 |
for (std::size_t i = 0; i < env.size(); ++i) manager->registerObject(env[i]); |
429 |
✓✗ | 8 |
manager->setup(); |
430 |
|||
431 |
while (1) { |
||
432 |
32 |
std::vector<CollisionObject*> candidates; |
|
433 |
✓✓ | 32 |
if (use_mesh) |
434 |
✓✗ | 4 |
generateEnvironmentsMesh(candidates, env_scale, query_size); |
435 |
else |
||
436 |
✓✗ | 28 |
generateEnvironments(candidates, env_scale, query_size); |
437 |
|||
438 |
✓✓ | 848 |
for (std::size_t i = 0; i < candidates.size(); ++i) { |
439 |
✓✗ | 824 |
CollisionCallBackDefault callback; |
440 |
✓✗ | 824 |
manager->collide(candidates[i], &callback); |
441 |
✓✓ | 824 |
if (callback.data.result.numContacts() == 0) |
442 |
✓✗ | 80 |
query.push_back(candidates[i]); |
443 |
else |
||
444 |
✓✗ | 744 |
delete candidates[i]; |
445 |
✓✓ | 824 |
if (query.size() == query_size) break; |
446 |
} |
||
447 |
|||
448 |
✓✓ | 32 |
if (query.size() == query_size) break; |
449 |
24 |
} |
|
450 |
|||
451 |
✓✗ | 8 |
delete manager; |
452 |
|||
453 |
16 |
std::vector<BroadPhaseCollisionManager*> managers; |
|
454 |
|||
455 |
✓✗✓✗ ✓✗ |
8 |
managers.push_back(new NaiveCollisionManager()); |
456 |
✓✗✓✗ ✓✗ |
8 |
managers.push_back(new SSaPCollisionManager()); |
457 |
✓✗✓✗ ✓✗ |
8 |
managers.push_back(new SaPCollisionManager()); |
458 |
✓✗✓✗ ✓✗ |
8 |
managers.push_back(new IntervalTreeCollisionManager()); |
459 |
|||
460 |
✓✗✓✗ |
8 |
Vec3f lower_limit, upper_limit; |
461 |
✓✗ | 8 |
SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); |
462 |
FCL_REAL cell_size = |
||
463 |
✓✗ | 8 |
std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, |
464 |
✓✗✓✗ |
8 |
(upper_limit[1] - lower_limit[1]) / 20), |
465 |
✓✗✓✗ ✓✗ |
16 |
(upper_limit[2] - lower_limit[2]) / 20); |
466 |
// managers.push_back(new SpatialHashingCollisionManager<>(cell_size, |
||
467 |
// lower_limit, upper_limit)); |
||
468 |
✓✗ | 8 |
managers.push_back(new SpatialHashingCollisionManager< |
469 |
SparseHashTable<AABB, CollisionObject*, SpatialHash> >( |
||
470 |
✓✗✓✗ |
8 |
cell_size, lower_limit, upper_limit)); |
471 |
#if USE_GOOGLEHASH |
||
472 |
managers.push_back( |
||
473 |
new SpatialHashingCollisionManager<SparseHashTable< |
||
474 |
AABB, CollisionObject*, SpatialHash, GoogleSparseHashTable> >( |
||
475 |
cell_size, lower_limit, upper_limit)); |
||
476 |
managers.push_back( |
||
477 |
new SpatialHashingCollisionManager<SparseHashTable< |
||
478 |
AABB, CollisionObject*, SpatialHash, GoogleDenseHashTable> >( |
||
479 |
cell_size, lower_limit, upper_limit)); |
||
480 |
#endif |
||
481 |
✓✗✓✗ ✓✗ |
8 |
managers.push_back(new DynamicAABBTreeCollisionManager()); |
482 |
✓✗✓✗ ✓✗ |
8 |
managers.push_back(new DynamicAABBTreeArrayCollisionManager()); |
483 |
|||
484 |
{ |
||
485 |
✓✗✓✗ |
8 |
DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager(); |
486 |
8 |
m->tree_init_level = 2; |
|
487 |
✓✗ | 8 |
managers.push_back(m); |
488 |
} |
||
489 |
|||
490 |
{ |
||
491 |
DynamicAABBTreeArrayCollisionManager* m = |
||
492 |
✓✗✓✗ |
8 |
new DynamicAABBTreeArrayCollisionManager(); |
493 |
8 |
m->tree_init_level = 2; |
|
494 |
✓✗ | 8 |
managers.push_back(m); |
495 |
} |
||
496 |
|||
497 |
✓✗ | 8 |
ts.resize(managers.size()); |
498 |
✓✗ | 8 |
timers.resize(managers.size()); |
499 |
|||
500 |
✓✓ | 80 |
for (size_t i = 0; i < managers.size(); ++i) { |
501 |
✓✗ | 72 |
timers[i].start(); |
502 |
✓✗ | 72 |
managers[i]->registerObjects(env); |
503 |
✓✗ | 72 |
timers[i].stop(); |
504 |
✓✗✓✗ |
72 |
ts[i].push_back(timers[i].getElapsedTime()); |
505 |
} |
||
506 |
|||
507 |
✓✓ | 80 |
for (size_t i = 0; i < managers.size(); ++i) { |
508 |
✓✗ | 72 |
timers[i].start(); |
509 |
✓✗ | 72 |
managers[i]->setup(); |
510 |
✓✗ | 72 |
timers[i].stop(); |
511 |
✓✗✓✗ |
72 |
ts[i].push_back(timers[i].getElapsedTime()); |
512 |
} |
||
513 |
|||
514 |
✓✓ | 88 |
for (size_t i = 0; i < query.size(); ++i) { |
515 |
✓✗ | 160 |
std::vector<DistanceCallBackDefault> query_callbacks(managers.size()); |
516 |
✓✓ | 800 |
for (size_t j = 0; j < managers.size(); ++j) { |
517 |
✓✗ | 720 |
timers[j].start(); |
518 |
✓✗ | 720 |
managers[j]->distance(query[i], &query_callbacks[j]); |
519 |
✓✗ | 720 |
timers[j].stop(); |
520 |
✓✗✓✗ |
720 |
ts[j].push_back(timers[j].getElapsedTime()); |
521 |
✓✗✓✗ |
720 |
std::cout << query_callbacks[j].data.result.min_distance << " "; |
522 |
} |
||
523 |
✓✗ | 80 |
std::cout << std::endl; |
524 |
|||
525 |
✓✓ | 720 |
for (size_t j = 1; j < managers.size(); ++j) { |
526 |
640 |
bool test = fabs(query_callbacks[0].data.result.min_distance - |
|
527 |
✓✓ | 777 |
query_callbacks[j].data.result.min_distance) < DELTA || |
528 |
137 |
fabs(query_callbacks[0].data.result.min_distance - |
|
529 |
137 |
query_callbacks[j].data.result.min_distance) / |
|
530 |
✓✓ | 137 |
fabs(query_callbacks[0].data.result.min_distance) < |
531 |
640 |
DELTA; |
|
532 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
640 |
BOOST_CHECK(test); |
533 |
|||
534 |
✓✓ | 640 |
if (!test) { |
535 |
134 |
const BroadPhaseCollisionManager& self = *managers[j]; |
|
536 |
✓✗✓✗ ✓✗ |
134 |
std::cout << "j: " << typeid(self).name() << std::endl; |
537 |
✓✗ | 134 |
std::cout << "query_callbacks[0].data.result.min_distance: " |
538 |
✓✗✓✗ |
134 |
<< query_callbacks[0].data.result.min_distance << std::endl; |
539 |
✓✗ | 134 |
std::cout << "query_callbacks[j].data.result.min_distance: " |
540 |
✓✗✓✗ |
134 |
<< query_callbacks[j].data.result.min_distance << std::endl; |
541 |
} |
||
542 |
} |
||
543 |
} |
||
544 |
|||
545 |
✓✓✓✗ |
7268 |
for (std::size_t i = 0; i < env.size(); ++i) delete env[i]; |
546 |
✓✓✓✗ |
88 |
for (std::size_t i = 0; i < query.size(); ++i) delete query[i]; |
547 |
|||
548 |
✓✓✓✗ |
80 |
for (size_t i = 0; i < managers.size(); ++i) delete managers[i]; |
549 |
|||
550 |
8 |
std::cout.setf(std::ios_base::left, std::ios_base::adjustfield); |
|
551 |
8 |
int w = 7; |
|
552 |
|||
553 |
✓✗✓✗ |
8 |
std::cout << "distance timing summary" << std::endl; |
554 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
8 |
std::cout << env_size << " objs, " << query_size << " queries" << std::endl; |
555 |
✓✗✓✗ |
8 |
std::cout << "register time" << std::endl; |
556 |
✓✓ | 80 |
for (size_t i = 0; i < ts.size(); ++i) |
557 |
✓✗✓✗ ✓✗ |
72 |
std::cout << std::setw(w) << ts[i].records[0] << " "; |
558 |
✓✗ | 8 |
std::cout << std::endl; |
559 |
|||
560 |
✓✗✓✗ |
8 |
std::cout << "setup time" << std::endl; |
561 |
✓✓ | 80 |
for (size_t i = 0; i < ts.size(); ++i) |
562 |
✓✗✓✗ ✓✗ |
72 |
std::cout << std::setw(w) << ts[i].records[1] << " "; |
563 |
✓✗ | 8 |
std::cout << std::endl; |
564 |
|||
565 |
✓✗✓✗ |
8 |
std::cout << "distance time" << std::endl; |
566 |
✓✓ | 80 |
for (size_t i = 0; i < ts.size(); ++i) { |
567 |
72 |
FCL_REAL tmp = 0; |
|
568 |
✓✓ | 792 |
for (size_t j = 2; j < ts[i].records.size(); ++j) tmp += ts[i].records[j]; |
569 |
✓✗✓✗ ✓✗ |
72 |
std::cout << std::setw(w) << tmp << " "; |
570 |
} |
||
571 |
✓✗ | 8 |
std::cout << std::endl; |
572 |
|||
573 |
✓✗✓✗ |
8 |
std::cout << "overall time" << std::endl; |
574 |
✓✓ | 80 |
for (size_t i = 0; i < ts.size(); ++i) |
575 |
✓✗✓✗ ✓✗ |
72 |
std::cout << std::setw(w) << ts[i].overall_time << " "; |
576 |
✓✗ | 8 |
std::cout << std::endl; |
577 |
✓✗ | 8 |
std::cout << std::endl; |
578 |
8 |
} |
Generated by: GCOVR (Version 4.2) |