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 Joseph Mirabel */ |
||
37 |
|||
38 |
#include <boost/mpl/vector.hpp> |
||
39 |
|||
40 |
#define BOOST_TEST_MODULE FCL_COLLISION |
||
41 |
#include <boost/test/included/unit_test.hpp> |
||
42 |
|||
43 |
#include <fstream> |
||
44 |
#include <boost/assign/list_of.hpp> |
||
45 |
|||
46 |
#include <hpp/fcl/collision.h> |
||
47 |
#include <hpp/fcl/BV/BV.h> |
||
48 |
#include <hpp/fcl/shape/geometric_shapes.h> |
||
49 |
#include <hpp/fcl/narrowphase/narrowphase.h> |
||
50 |
#include <hpp/fcl/mesh_loader/assimp.h> |
||
51 |
|||
52 |
#include <hpp/fcl/internal/traversal_node_bvhs.h> |
||
53 |
#include <hpp/fcl/internal/traversal_node_setup.h> |
||
54 |
#include "../src/collision_node.h" |
||
55 |
#include <hpp/fcl/internal/BV_splitter.h> |
||
56 |
|||
57 |
#include <hpp/fcl/timings.h> |
||
58 |
|||
59 |
#include "utility.h" |
||
60 |
#include "fcl_resources/config.h" |
||
61 |
|||
62 |
using namespace hpp::fcl; |
||
63 |
namespace utf = boost::unit_test::framework; |
||
64 |
|||
65 |
int num_max_contacts = (std::numeric_limits<int>::max)(); |
||
66 |
|||
67 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(OBB_Box_test) { |
68 |
2 |
FCL_REAL r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; |
|
69 |
4 |
std::vector<Transform3f> rotate_transform; |
|
70 |
✓✗ | 2 |
generateRandomTransforms(r_extents, rotate_transform, 1); |
71 |
|||
72 |
✓✗ | 2 |
AABB aabb1; |
73 |
✓✗ | 2 |
aabb1.min_ = Vec3f(-600, -600, -600); |
74 |
✓✗ | 2 |
aabb1.max_ = Vec3f(600, 600, 600); |
75 |
|||
76 |
✓✗ | 2 |
OBB obb1; |
77 |
✓✗ | 2 |
convertBV(aabb1, rotate_transform[0], obb1); |
78 |
✓✗ | 4 |
Box box1; |
79 |
✓✗ | 2 |
Transform3f box1_tf; |
80 |
✓✗ | 2 |
constructBox(aabb1, rotate_transform[0], box1, box1_tf); |
81 |
|||
82 |
2 |
FCL_REAL extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; |
|
83 |
2 |
std::size_t n = 1000; |
|
84 |
|||
85 |
4 |
std::vector<Transform3f> transforms; |
|
86 |
✓✗ | 2 |
generateRandomTransforms(extents, transforms, n); |
87 |
|||
88 |
✓✓ | 2002 |
for (std::size_t i = 0; i < transforms.size(); ++i) { |
89 |
✓✗ | 2000 |
AABB aabb; |
90 |
✓✗✓✗ |
2000 |
aabb.min_ = aabb1.min_ * 0.5; |
91 |
✓✗✓✗ |
2000 |
aabb.max_ = aabb1.max_ * 0.5; |
92 |
|||
93 |
✓✗ | 2000 |
OBB obb2; |
94 |
✓✗ | 2000 |
convertBV(aabb, transforms[i], obb2); |
95 |
|||
96 |
✓✗ | 4000 |
Box box2; |
97 |
✓✗ | 2000 |
Transform3f box2_tf; |
98 |
✓✗ | 2000 |
constructBox(aabb, transforms[i], box2, box2_tf); |
99 |
|||
100 |
✓✗ | 2000 |
GJKSolver solver; |
101 |
|||
102 |
FCL_REAL distance; |
||
103 |
✓✗ | 2000 |
bool overlap_obb = obb1.overlap(obb2); |
104 |
✓✗ | 2000 |
bool overlap_box = solver.shapeIntersect(box1, box1_tf, box2, box2_tf, |
105 |
distance, false, NULL, NULL); |
||
106 |
|||
107 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2000 |
BOOST_CHECK(overlap_obb == overlap_box); |
108 |
} |
||
109 |
2 |
} |
|
110 |
|||
111 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(OBB_shape_test) { |
112 |
2 |
FCL_REAL r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; |
|
113 |
4 |
std::vector<Transform3f> rotate_transform; |
|
114 |
✓✗ | 2 |
generateRandomTransforms(r_extents, rotate_transform, 1); |
115 |
|||
116 |
✓✗ | 2 |
AABB aabb1; |
117 |
✓✗ | 2 |
aabb1.min_ = Vec3f(-600, -600, -600); |
118 |
✓✗ | 2 |
aabb1.max_ = Vec3f(600, 600, 600); |
119 |
|||
120 |
✓✗ | 2 |
OBB obb1; |
121 |
✓✗ | 2 |
convertBV(aabb1, rotate_transform[0], obb1); |
122 |
✓✗ | 4 |
Box box1; |
123 |
✓✗ | 2 |
Transform3f box1_tf; |
124 |
✓✗ | 2 |
constructBox(aabb1, rotate_transform[0], box1, box1_tf); |
125 |
|||
126 |
2 |
FCL_REAL extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; |
|
127 |
2 |
std::size_t n = 1000; |
|
128 |
|||
129 |
4 |
std::vector<Transform3f> transforms; |
|
130 |
✓✗ | 2 |
generateRandomTransforms(extents, transforms, n); |
131 |
|||
132 |
✓✓ | 2002 |
for (std::size_t i = 0; i < transforms.size(); ++i) { |
133 |
✓✗✓✗ |
2000 |
FCL_REAL len = (aabb1.max_[0] - aabb1.min_[0]) * 0.5; |
134 |
✓✗ | 2000 |
OBB obb2; |
135 |
✓✗ | 2000 |
GJKSolver solver; |
136 |
FCL_REAL distance; |
||
137 |
|||
138 |
{ |
||
139 |
✓✗ | 4000 |
Sphere sphere(len); |
140 |
✓✗ | 2000 |
computeBV(sphere, transforms[i], obb2); |
141 |
|||
142 |
✓✗ | 2000 |
bool overlap_obb = obb1.overlap(obb2); |
143 |
✓✗ | 2000 |
bool overlap_sphere = solver.shapeIntersect( |
144 |
2000 |
box1, box1_tf, sphere, transforms[i], distance, false, NULL, NULL); |
|
145 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2000 |
BOOST_CHECK(overlap_obb >= overlap_sphere); |
146 |
} |
||
147 |
|||
148 |
{ |
||
149 |
✓✗ | 4000 |
Capsule capsule(len, 2 * len); |
150 |
✓✗ | 2000 |
computeBV(capsule, transforms[i], obb2); |
151 |
|||
152 |
✓✗ | 2000 |
bool overlap_obb = obb1.overlap(obb2); |
153 |
✓✗ | 2000 |
bool overlap_capsule = solver.shapeIntersect( |
154 |
2000 |
box1, box1_tf, capsule, transforms[i], distance, false, NULL, NULL); |
|
155 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2000 |
BOOST_CHECK(overlap_obb >= overlap_capsule); |
156 |
} |
||
157 |
|||
158 |
{ |
||
159 |
✓✗ | 4000 |
Cone cone(len, 2 * len); |
160 |
✓✗ | 2000 |
computeBV(cone, transforms[i], obb2); |
161 |
|||
162 |
✓✗ | 2000 |
bool overlap_obb = obb1.overlap(obb2); |
163 |
✓✗ | 2000 |
bool overlap_cone = solver.shapeIntersect( |
164 |
2000 |
box1, box1_tf, cone, transforms[i], distance, false, NULL, NULL); |
|
165 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2000 |
BOOST_CHECK(overlap_obb >= overlap_cone); |
166 |
} |
||
167 |
|||
168 |
{ |
||
169 |
✓✗ | 4000 |
Cylinder cylinder(len, 2 * len); |
170 |
✓✗ | 2000 |
computeBV(cylinder, transforms[i], obb2); |
171 |
|||
172 |
✓✗ | 2000 |
bool overlap_obb = obb1.overlap(obb2); |
173 |
✓✗ | 2000 |
bool overlap_cylinder = solver.shapeIntersect( |
174 |
2000 |
box1, box1_tf, cylinder, transforms[i], distance, false, NULL, NULL); |
|
175 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2000 |
BOOST_CHECK(overlap_obb >= overlap_cylinder); |
176 |
} |
||
177 |
} |
||
178 |
2 |
} |
|
179 |
|||
180 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(OBB_AABB_test) { |
181 |
2 |
FCL_REAL extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; |
|
182 |
2 |
std::size_t n = 1000; |
|
183 |
|||
184 |
4 |
std::vector<Transform3f> transforms; |
|
185 |
✓✗ | 2 |
generateRandomTransforms(extents, transforms, n); |
186 |
|||
187 |
✓✗ | 2 |
AABB aabb1; |
188 |
✓✗ | 2 |
aabb1.min_ = Vec3f(-600, -600, -600); |
189 |
✓✗ | 2 |
aabb1.max_ = Vec3f(600, 600, 600); |
190 |
|||
191 |
✓✗ | 2 |
OBB obb1; |
192 |
✓✗✓✗ |
2 |
convertBV(aabb1, Transform3f(), obb1); |
193 |
|||
194 |
✓✓ | 2002 |
for (std::size_t i = 0; i < transforms.size(); ++i) { |
195 |
✓✗ | 2000 |
AABB aabb; |
196 |
✓✗✓✗ |
2000 |
aabb.min_ = aabb1.min_ * 0.5; |
197 |
✓✗✓✗ |
2000 |
aabb.max_ = aabb1.max_ * 0.5; |
198 |
|||
199 |
✓✗ | 2000 |
AABB aabb2 = translate(aabb, transforms[i].getTranslation()); |
200 |
|||
201 |
✓✗ | 2000 |
OBB obb2; |
202 |
✓✗✓✗ |
2000 |
convertBV(aabb, Transform3f(transforms[i].getTranslation()), obb2); |
203 |
|||
204 |
✓✗ | 2000 |
bool overlap_aabb = aabb1.overlap(aabb2); |
205 |
✓✗ | 2000 |
bool overlap_obb = obb1.overlap(obb2); |
206 |
✗✓ | 2000 |
if (overlap_aabb != overlap_obb) { |
207 |
std::cout << aabb1.min_ << " " << aabb1.max_ << std::endl; |
||
208 |
std::cout << aabb2.min_ << " " << aabb2.max_ << std::endl; |
||
209 |
std::cout << obb1.To << " " << obb1.extent << " " << obb1.axes |
||
210 |
<< std::endl; |
||
211 |
std::cout << obb2.To << " " << obb2.extent << " " << obb2.axes |
||
212 |
<< std::endl; |
||
213 |
} |
||
214 |
|||
215 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2000 |
BOOST_CHECK(overlap_aabb == overlap_obb); |
216 |
} |
||
217 |
✓✗ | 2 |
std::cout << std::endl; |
218 |
2 |
} |
|
219 |
|||
220 |
std::ostream* bench_stream = NULL; |
||
221 |
bool bs_nl = true; |
||
222 |
bool bs_hp = false; |
||
223 |
#define BENCHMARK(stream) \ |
||
224 |
if (bench_stream != NULL) { \ |
||
225 |
*bench_stream << (bs_nl ? "" : ", ") << stream; \ |
||
226 |
bs_nl = false; \ |
||
227 |
} |
||
228 |
#define BENCHMARK_HEADER(stream) \ |
||
229 |
if (!bs_hp) { \ |
||
230 |
BENCHMARK(stream) \ |
||
231 |
} |
||
232 |
#define BENCHMARK_NEXT() \ |
||
233 |
if (bench_stream != NULL && !bs_nl) { \ |
||
234 |
*bench_stream << '\n'; \ |
||
235 |
bs_nl = true; \ |
||
236 |
bs_hp = true; \ |
||
237 |
} |
||
238 |
|||
239 |
typedef std::vector<Contact> Contacts_t; |
||
240 |
typedef boost::mpl::vector<OBB, RSS, KDOP<24>, KDOP<18>, KDOP<16>, kIOS, OBBRSS> |
||
241 |
BVs_t; |
||
242 |
std::vector<SplitMethodType> splitMethods = boost::assign::list_of( |
||
243 |
SPLIT_METHOD_MEAN)(SPLIT_METHOD_MEDIAN)(SPLIT_METHOD_BV_CENTER); |
||
244 |
|||
245 |
#define BV_STR_SPECIALIZATION(bv) \ |
||
246 |
template <> \ |
||
247 |
const char* str<bv>() { \ |
||
248 |
return #bv; \ |
||
249 |
} |
||
250 |
template <typename BV> |
||
251 |
const char* str(); |
||
252 |
18 |
BV_STR_SPECIALIZATION(AABB) |
|
253 |
33 |
BV_STR_SPECIALIZATION(OBB) |
|
254 |
36 |
BV_STR_SPECIALIZATION(RSS) |
|
255 |
12 |
BV_STR_SPECIALIZATION(KDOP<24>) |
|
256 |
12 |
BV_STR_SPECIALIZATION(KDOP<18>) |
|
257 |
12 |
BV_STR_SPECIALIZATION(KDOP<16>) |
|
258 |
36 |
BV_STR_SPECIALIZATION(kIOS) |
|
259 |
36 |
BV_STR_SPECIALIZATION(OBBRSS) |
|
260 |
|||
261 |
template <typename T> |
||
262 |
struct wrap {}; |
||
263 |
|||
264 |
struct base_traits { |
||
265 |
enum { IS_IMPLEMENTED = true }; |
||
266 |
}; |
||
267 |
|||
268 |
enum { |
||
269 |
Oriented = true, |
||
270 |
NonOriented = false, |
||
271 |
Recursive = true, |
||
272 |
NonRecursive = false |
||
273 |
}; |
||
274 |
|||
275 |
template <typename BV, bool Oriented, bool recursive> |
||
276 |
struct traits : base_traits {}; |
||
277 |
|||
278 |
template <short N, bool recursive> |
||
279 |
struct traits<KDOP<N>, Oriented, recursive> : base_traits { |
||
280 |
enum { IS_IMPLEMENTED = false }; |
||
281 |
}; |
||
282 |
|||
283 |
struct mesh_mesh_run_test { |
||
284 |
3 |
mesh_mesh_run_test(const std::vector<Transform3f>& _transforms, |
|
285 |
const CollisionRequest _request) |
||
286 |
3 |
: transforms(_transforms), |
|
287 |
request(_request), |
||
288 |
enable_statistics(false), |
||
289 |
benchmark(false), |
||
290 |
isInit(false), |
||
291 |
3 |
indent(0) {} |
|
292 |
|||
293 |
const std::vector<Transform3f>& transforms; |
||
294 |
const CollisionRequest request; |
||
295 |
bool enable_statistics, benchmark; |
||
296 |
std::vector<Contacts_t> contacts; |
||
297 |
std::vector<Contacts_t> contacts_ref; |
||
298 |
bool isInit; |
||
299 |
|||
300 |
int indent; |
||
301 |
|||
302 |
354 |
const char* getindent() { |
|
303 |
✗✓ | 354 |
assert(indent < 9); |
304 |
static const char* t[] = {"", |
||
305 |
"\t", |
||
306 |
"\t\t", |
||
307 |
"\t\t\t", |
||
308 |
"\t\t\t\t", |
||
309 |
"\t\t\t\t\t", |
||
310 |
"\t\t\t\t\t\t", |
||
311 |
"\t\t\t\t\t\t\t", |
||
312 |
"\t\t\t\t\t\t\t\t"}; |
||
313 |
354 |
return t[indent]; |
|
314 |
} |
||
315 |
|||
316 |
template <typename BV> |
||
317 |
138 |
void query(const std::vector<Transform3f>& transforms, |
|
318 |
SplitMethodType splitMethod, const CollisionRequest request, |
||
319 |
std::vector<Contacts_t>& contacts) { |
||
320 |
✓✓✓✓ ✓✗✓✗ ✓✗ |
138 |
BENCHMARK_HEADER("BV"); |
321 |
✓✓✓✓ ✗✓✓✗ ✓✗ |
138 |
BENCHMARK_HEADER("oriented"); |
322 |
✓✓✓✓ ✗✓✓✗ ✓✗ |
138 |
BENCHMARK_HEADER("Split method"); |
323 |
✓✓ | 138 |
if (enable_statistics) { |
324 |
✓✗✗✓ ✗✗✗✗ ✗✗ |
42 |
BENCHMARK_HEADER("num_bv_tests"); |
325 |
✓✗✗✓ ✗✗✗✗ ✗✗ |
42 |
BENCHMARK_HEADER("num_leaf_tests"); |
326 |
} |
||
327 |
✓✓✓✓ ✗✓✓✗ ✓✗ |
138 |
BENCHMARK_HEADER("numContacts"); |
328 |
✓✓✓✓ ✗✓✓✗ ✓✗ |
138 |
BENCHMARK_HEADER("distance_lower_bound"); |
329 |
✓✓✓✓ ✗✓✓✗ ✓✗ |
138 |
BENCHMARK_HEADER("time"); |
330 |
✓✓✓✓ ✓✗ |
138 |
BENCHMARK_NEXT(); |
331 |
|||
332 |
typedef BVHModel<BV> BVH_t; |
||
333 |
typedef shared_ptr<BVH_t> BVHPtr_t; |
||
334 |
|||
335 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
276 |
BVHPtr_t model1(new BVH_t), model2(new BVH_t); |
336 |
✓✗✓✗ ✓✗ |
138 |
model1->bv_splitter.reset(new BVSplitter<BV>(splitMethod)); |
337 |
✓✗✓✗ ✓✗ |
138 |
model2->bv_splitter.reset(new BVSplitter<BV>(splitMethod)); |
338 |
|||
339 |
✓✗✓✗ ✓✗✓✗ |
138 |
loadPolyhedronFromResource(TEST_RESOURCES_DIR "/env.obj", Vec3f::Ones(), |
340 |
model1); |
||
341 |
✓✗✓✗ ✓✗✓✗ |
138 |
loadPolyhedronFromResource(TEST_RESOURCES_DIR "/rob.obj", Vec3f::Ones(), |
342 |
model2); |
||
343 |
|||
344 |
✓✗ | 138 |
Timer timer(false); |
345 |
✓✗ | 138 |
const Transform3f tf2; |
346 |
138 |
const std::size_t N = transforms.size(); |
|
347 |
|||
348 |
✓✗ | 138 |
contacts.resize(3 * N); |
349 |
|||
350 |
if (traits<BV, Oriented, Recursive>::IS_IMPLEMENTED) { |
||
351 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
84 |
BOOST_TEST_MESSAGE(getindent() << "BV: " << str<BV>() << " oriented"); |
352 |
84 |
++indent; |
|
353 |
|||
354 |
✓✓ | 108 |
for (std::size_t i = 0; i < transforms.size(); ++i) { |
355 |
24 |
const Transform3f& tf1 = transforms[i]; |
|
356 |
24 |
timer.start(); |
|
357 |
|||
358 |
✓✗ | 48 |
CollisionResult local_result; |
359 |
✓✗ | 48 |
MeshCollisionTraversalNode<BV, 0> node(request); |
360 |
24 |
node.enable_statistics = enable_statistics; |
|
361 |
|||
362 |
bool success = |
||
363 |
✓✗ | 24 |
initialize(node, *model1, tf1, *model2, tf2, local_result); |
364 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
24 |
BOOST_REQUIRE(success); |
365 |
|||
366 |
✓✗ | 24 |
collide(&node, request, local_result); |
367 |
|||
368 |
✓✗ | 24 |
timer.stop(); |
369 |
|||
370 |
✗✓✗✗ ✗✗✗✗ |
24 |
BENCHMARK(str<BV>()); |
371 |
✗✓✗✗ ✗✗✗✗ |
24 |
BENCHMARK(1); |
372 |
✗✓✗✗ ✗✗✗✗ |
24 |
BENCHMARK(splitMethod); |
373 |
✓✗ | 24 |
if (enable_statistics) { |
374 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
24 |
BOOST_TEST_MESSAGE(getindent() << "statistics: " << node.num_bv_tests |
375 |
<< " " << node.num_leaf_tests); |
||
376 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
24 |
BOOST_TEST_MESSAGE(getindent() |
377 |
<< "nb contacts: " << local_result.numContacts()); |
||
378 |
✗✓✗✗ ✗✗✗✗ |
24 |
BENCHMARK(node.num_bv_tests); |
379 |
✗✓✗✗ ✗✗✗✗ |
24 |
BENCHMARK(node.num_leaf_tests); |
380 |
} |
||
381 |
✗✓✗✗ ✗✗✗✗ |
24 |
BENCHMARK(local_result.numContacts()); |
382 |
✗✓✗✗ ✗✗✗✗ |
24 |
BENCHMARK(local_result.distance_lower_bound); |
383 |
✗✓✗✗ ✗✗✗✗ ✗✗ |
24 |
BENCHMARK(timer.duration().count()); |
384 |
✗✓✗✗ ✗✗ |
24 |
BENCHMARK_NEXT(); |
385 |
|||
386 |
✗✓ | 24 |
if (local_result.numContacts() > 0) { |
387 |
local_result.getContacts(contacts[i]); |
||
388 |
std::sort(contacts[i].begin(), contacts[i].end()); |
||
389 |
} |
||
390 |
} |
||
391 |
84 |
--indent; |
|
392 |
} |
||
393 |
|||
394 |
if (traits<BV, NonOriented, Recursive>::IS_IMPLEMENTED) { |
||
395 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
138 |
BOOST_TEST_MESSAGE(getindent() << "BV: " << str<BV>()); |
396 |
138 |
++indent; |
|
397 |
|||
398 |
✓✓ | 180 |
for (std::size_t i = 0; i < transforms.size(); ++i) { |
399 |
✓✗ | 42 |
const Transform3f tf1 = transforms[i]; |
400 |
|||
401 |
42 |
timer.start(); |
|
402 |
✓✗ | 84 |
CollisionResult local_result; |
403 |
✓✗ | 84 |
MeshCollisionTraversalNode<BV, RelativeTransformationIsIdentity> node( |
404 |
request); |
||
405 |
42 |
node.enable_statistics = enable_statistics; |
|
406 |
|||
407 |
✓✗✓✗ |
42 |
BVH_t* model1_tmp = new BVH_t(*model1); |
408 |
✓✗ | 42 |
Transform3f tf1_tmp = tf1; |
409 |
✓✗✓✗ |
42 |
BVH_t* model2_tmp = new BVH_t(*model2); |
410 |
✓✗ | 42 |
Transform3f tf2_tmp = tf2; |
411 |
|||
412 |
✓✗ | 42 |
bool success = initialize(node, *model1_tmp, tf1_tmp, *model2_tmp, |
413 |
tf2_tmp, local_result, true, true); |
||
414 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
42 |
BOOST_REQUIRE(success); |
415 |
|||
416 |
✓✗ | 42 |
collide(&node, request, local_result); |
417 |
✓✗ | 42 |
delete model1_tmp; |
418 |
✓✗ | 42 |
delete model2_tmp; |
419 |
|||
420 |
✓✗ | 42 |
timer.stop(); |
421 |
✗✓✗✗ ✗✗✗✗ |
42 |
BENCHMARK(str<BV>()); |
422 |
✗✓✗✗ ✗✗✗✗ |
42 |
BENCHMARK(2); |
423 |
✗✓✗✗ ✗✗✗✗ |
42 |
BENCHMARK(splitMethod); |
424 |
✓✗ | 42 |
if (enable_statistics) { |
425 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
42 |
BOOST_TEST_MESSAGE(getindent() << "statistics: " << node.num_bv_tests |
426 |
<< " " << node.num_leaf_tests); |
||
427 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
42 |
BOOST_TEST_MESSAGE(getindent() |
428 |
<< "nb contacts: " << local_result.numContacts()); |
||
429 |
✗✓✗✗ ✗✗✗✗ |
42 |
BENCHMARK(node.num_bv_tests); |
430 |
✗✓✗✗ ✗✗✗✗ |
42 |
BENCHMARK(node.num_leaf_tests); |
431 |
} |
||
432 |
✗✓✗✗ ✗✗✗✗ |
42 |
BENCHMARK(local_result.numContacts()); |
433 |
✗✓✗✗ ✗✗✗✗ |
42 |
BENCHMARK(local_result.distance_lower_bound); |
434 |
✗✓✗✗ ✗✗✗✗ ✗✗ |
42 |
BENCHMARK(timer.duration().count()); |
435 |
✗✓✗✗ ✗✗ |
42 |
BENCHMARK_NEXT(); |
436 |
|||
437 |
✗✓ | 42 |
if (local_result.numContacts() > 0) { |
438 |
local_result.getContacts(contacts[i + N]); |
||
439 |
std::sort(contacts[i + N].begin(), contacts[i + N].end()); |
||
440 |
} |
||
441 |
} |
||
442 |
138 |
--indent; |
|
443 |
} |
||
444 |
|||
445 |
if (traits<BV, Oriented, NonRecursive>::IS_IMPLEMENTED) { |
||
446 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
84 |
BOOST_TEST_MESSAGE(getindent() |
447 |
<< "BV: " << str<BV>() << " oriented non-recursive"); |
||
448 |
84 |
++indent; |
|
449 |
|||
450 |
✓✓ | 108 |
for (std::size_t i = 0; i < transforms.size(); ++i) { |
451 |
24 |
timer.start(); |
|
452 |
✓✗ | 24 |
const Transform3f tf1 = transforms[i]; |
453 |
|||
454 |
✓✗ | 48 |
CollisionResult local_result; |
455 |
✓✗ | 48 |
MeshCollisionTraversalNode<BV, 0> node(request); |
456 |
24 |
node.enable_statistics = enable_statistics; |
|
457 |
|||
458 |
bool success = |
||
459 |
✓✗ | 24 |
initialize(node, *model1, tf1, *model2, tf2, local_result); |
460 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
24 |
BOOST_REQUIRE(success); |
461 |
|||
462 |
✓✗ | 24 |
collide(&node, request, local_result, NULL, false); |
463 |
|||
464 |
✓✗ | 24 |
timer.stop(); |
465 |
✗✓✗✗ ✗✗✗✗ |
24 |
BENCHMARK(str<BV>()); |
466 |
✗✓✗✗ ✗✗✗✗ |
24 |
BENCHMARK(0); |
467 |
✗✓✗✗ ✗✗✗✗ |
24 |
BENCHMARK(splitMethod); |
468 |
✓✗ | 24 |
if (enable_statistics) { |
469 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
24 |
BOOST_TEST_MESSAGE(getindent() << "statistics: " << node.num_bv_tests |
470 |
<< " " << node.num_leaf_tests); |
||
471 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
24 |
BOOST_TEST_MESSAGE(getindent() |
472 |
<< "nb contacts: " << local_result.numContacts()); |
||
473 |
✗✓✗✗ ✗✗✗✗ |
24 |
BENCHMARK(node.num_bv_tests); |
474 |
✗✓✗✗ ✗✗✗✗ |
24 |
BENCHMARK(node.num_leaf_tests); |
475 |
} |
||
476 |
✗✓✗✗ ✗✗✗✗ |
24 |
BENCHMARK(local_result.numContacts()); |
477 |
✗✓✗✗ ✗✗✗✗ |
24 |
BENCHMARK(local_result.distance_lower_bound); |
478 |
✗✓✗✗ ✗✗✗✗ ✗✗ |
24 |
BENCHMARK(timer.duration().count()); |
479 |
✗✓✗✗ ✗✗ |
24 |
BENCHMARK_NEXT(); |
480 |
|||
481 |
✗✓ | 24 |
if (local_result.numContacts() > 0) { |
482 |
local_result.getContacts(contacts[i + 2 * N]); |
||
483 |
std::sort(contacts[i + 2 * N].begin(), contacts[i + 2 * N].end()); |
||
484 |
} |
||
485 |
} |
||
486 |
84 |
--indent; |
|
487 |
} |
||
488 |
138 |
} |
|
489 |
|||
490 |
42 |
void check_contacts(std::size_t i0, std::size_t i1, bool warn) { |
|
491 |
✓✓ | 84 |
for (std::size_t i = i0; i < i1; ++i) { |
492 |
84 |
Contacts_t in_ref_but_not_in_i; |
|
493 |
std::set_difference( |
||
494 |
126 |
contacts_ref[i].begin(), contacts_ref[i].end(), contacts[i].begin(), |
|
495 |
42 |
contacts[i].end(), |
|
496 |
✓✗✓✗ |
210 |
std::inserter(in_ref_but_not_in_i, in_ref_but_not_in_i.begin())); |
497 |
✗✓ | 42 |
if (!in_ref_but_not_in_i.empty()) { |
498 |
for (std::size_t j = 0; j < in_ref_but_not_in_i.size(); ++j) { |
||
499 |
if (warn) { |
||
500 |
BOOST_WARN_MESSAGE( |
||
501 |
false, "Missed contacts: " << in_ref_but_not_in_i[j].b1 << ", " |
||
502 |
<< in_ref_but_not_in_i[j].b2); |
||
503 |
} else { |
||
504 |
BOOST_CHECK_MESSAGE( |
||
505 |
false, "Missed contacts: " << in_ref_but_not_in_i[j].b1 << ", " |
||
506 |
<< in_ref_but_not_in_i[j].b2); |
||
507 |
} |
||
508 |
} |
||
509 |
} |
||
510 |
|||
511 |
84 |
Contacts_t in_i_but_not_in_ref; |
|
512 |
std::set_difference( |
||
513 |
126 |
contacts[i].begin(), contacts[i].end(), contacts_ref[i].begin(), |
|
514 |
42 |
contacts_ref[i].end(), |
|
515 |
✓✗✓✗ |
210 |
std::inserter(in_i_but_not_in_ref, in_i_but_not_in_ref.begin())); |
516 |
|||
517 |
✗✓ | 42 |
if (!in_i_but_not_in_ref.empty()) { |
518 |
for (std::size_t j = 0; j < in_i_but_not_in_ref.size(); ++j) { |
||
519 |
if (warn) { |
||
520 |
BOOST_WARN_MESSAGE( |
||
521 |
false, "False contacts: " << in_i_but_not_in_ref[j].b1 << ", " |
||
522 |
<< in_i_but_not_in_ref[j].b2); |
||
523 |
} else { |
||
524 |
BOOST_CHECK_MESSAGE( |
||
525 |
false, "False contacts: " << in_i_but_not_in_ref[j].b1 << ", " |
||
526 |
<< in_i_but_not_in_ref[j].b2); |
||
527 |
} |
||
528 |
} |
||
529 |
} |
||
530 |
} |
||
531 |
42 |
} |
|
532 |
|||
533 |
template <typename BV> |
||
534 |
132 |
void check() { |
|
535 |
✓✓ | 132 |
if (benchmark) return; |
536 |
40 |
const std::size_t N = transforms.size(); |
|
537 |
|||
538 |
✓✗✓✗ ✓✗✗✓ |
40 |
BOOST_REQUIRE_EQUAL(contacts.size(), 3 * N); |
539 |
✓✗✓✗ ✓✗✗✓ |
40 |
BOOST_REQUIRE_EQUAL(contacts.size(), contacts_ref.size()); |
540 |
|||
541 |
if (traits<BV, Oriented, Recursive>::IS_IMPLEMENTED) { |
||
542 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
22 |
BOOST_TEST_MESSAGE(getindent() << "BV: " << str<BV>() << " oriented"); |
543 |
22 |
++indent; |
|
544 |
22 |
check_contacts(0, N, false); |
|
545 |
22 |
--indent; |
|
546 |
} |
||
547 |
if (traits<BV, NonOriented, Recursive>::IS_IMPLEMENTED) { |
||
548 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
40 |
BOOST_TEST_MESSAGE(getindent() << "BV: " << str<BV>()); |
549 |
40 |
++indent; |
|
550 |
40 |
check_contacts(N, 2 * N, true); |
|
551 |
40 |
--indent; |
|
552 |
} |
||
553 |
if (traits<BV, Oriented, NonRecursive>::IS_IMPLEMENTED) { |
||
554 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
22 |
BOOST_TEST_MESSAGE(getindent() |
555 |
<< "BV: " << str<BV>() << " oriented non-recursive"); |
||
556 |
22 |
++indent; |
|
557 |
22 |
check_contacts(2 * N, 3 * N, false); |
|
558 |
22 |
--indent; |
|
559 |
} |
||
560 |
} |
||
561 |
|||
562 |
template <typename BV> |
||
563 |
46 |
void operator()(wrap<BV>) { |
|
564 |
✓✓ | 184 |
for (std::size_t i = 0; i < splitMethods.size(); ++i) { |
565 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
138 |
BOOST_TEST_MESSAGE(getindent() << "splitMethod: " << splitMethods[i]); |
566 |
138 |
++indent; |
|
567 |
✓✗ | 138 |
query<BV>(transforms, splitMethods[i], request, |
568 |
✓✓ | 138 |
(isInit ? contacts : contacts_ref)); |
569 |
✓✓ | 138 |
if (isInit) check<BV>(); |
570 |
138 |
isInit = true; |
|
571 |
138 |
--indent; |
|
572 |
} |
||
573 |
46 |
} |
|
574 |
}; |
||
575 |
|||
576 |
// This test |
||
577 |
// 1. load two objects "env.obj" and "rob.obj" from directory |
||
578 |
// fcl_resources, |
||
579 |
// 2. generates n random transformation and for each of them denote tf, |
||
580 |
// 2.1 performs a collision test where object 1 is in pose tf. All |
||
581 |
// the contacts are stored in vector global_pairs. |
||
582 |
// 2.2 performs a series of collision tests with the same object and |
||
583 |
// the same poses using various methods and various types of bounding |
||
584 |
// volumes. Each time the contacts are stored in vector global_pairs_now. |
||
585 |
// |
||
586 |
// The methods used to test collision are |
||
587 |
// - collide_Test that calls function collide with tf for object1 pose and |
||
588 |
// identity for the second object pose, |
||
589 |
// - collide_Test2 that moves all vertices of object1 in pose tf and that |
||
590 |
// calls function collide with identity for both object poses, |
||
591 |
// |
||
592 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(mesh_mesh) { |
593 |
4 |
std::vector<Transform3f> transforms; |
|
594 |
2 |
FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; |
|
595 |
#ifndef NDEBUG // if debug mode |
||
596 |
2 |
std::size_t n = 1; |
|
597 |
#else |
||
598 |
std::size_t n = 10; |
||
599 |
#endif |
||
600 |
✓✗✓✗ ✓✗ |
2 |
n = getNbRun(utf::master_test_suite().argc, utf::master_test_suite().argv, n); |
601 |
|||
602 |
✓✗ | 2 |
generateRandomTransforms(extents, transforms, n); |
603 |
|||
604 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
6 |
Eigen::IOFormat f(Eigen::FullPrecision, 0, ", ", ",", "", "", "(", ")"); |
605 |
✓✓ | 4 |
for (std::size_t i = 0; i < transforms.size(); ++i) { |
606 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
BOOST_TEST_MESSAGE( |
607 |
"q" << i << "=" << transforms[i].getTranslation().format(f) << "+" |
||
608 |
<< transforms[i].getQuatRotation().coeffs().format(f)); |
||
609 |
} |
||
610 |
|||
611 |
// Request all contacts and check that all methods give the same result. |
||
612 |
mesh_mesh_run_test runner( |
||
613 |
✓✗✓✗ |
2 |
transforms, CollisionRequest(CONTACT, (size_t)num_max_contacts)); |
614 |
2 |
runner.enable_statistics = true; |
|
615 |
✓✗✓✗ |
2 |
boost::mpl::for_each<BVs_t, wrap<boost::mpl::placeholders::_1> >(runner); |
616 |
2 |
} |
|
617 |
|||
618 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(mesh_mesh_benchmark) { |
619 |
4 |
std::vector<Transform3f> transforms; |
|
620 |
2 |
FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; |
|
621 |
#ifndef NDEBUG |
||
622 |
2 |
std::size_t n = 0; |
|
623 |
#else |
||
624 |
std::size_t n = 10; |
||
625 |
#endif |
||
626 |
✓✗✓✗ ✓✗ |
2 |
n = getNbRun(utf::master_test_suite().argc, utf::master_test_suite().argv, n); |
627 |
|||
628 |
✓✗ | 2 |
generateRandomTransforms(extents, transforms, n); |
629 |
|||
630 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
6 |
Eigen::IOFormat f(Eigen::FullPrecision, 0, ", ", ",", "", "", "(", ")"); |
631 |
✗✓ | 2 |
for (std::size_t i = 0; i < transforms.size(); ++i) { |
632 |
BOOST_TEST_MESSAGE( |
||
633 |
"q" << i << "=" << transforms[i].getTranslation().format(f) << "+" |
||
634 |
<< transforms[i].getQuatRotation().coeffs().format(f)); |
||
635 |
} |
||
636 |
|||
637 |
// Request all contacts and check that all methods give the same result. |
||
638 |
typedef boost::mpl::vector<OBB, RSS, AABB, KDOP<24>, KDOP<18>, KDOP<16>, kIOS, |
||
639 |
OBBRSS> |
||
640 |
BVs_t; |
||
641 |
|||
642 |
✓✗ | 4 |
std::ofstream ofs("./collision.benchmark.csv", std::ofstream::out); |
643 |
2 |
bench_stream = &ofs; |
|
644 |
|||
645 |
// without lower bound. |
||
646 |
✓✗✓✗ |
4 |
mesh_mesh_run_test runner1(transforms, CollisionRequest()); |
647 |
2 |
runner1.enable_statistics = false; |
|
648 |
2 |
runner1.benchmark = true; |
|
649 |
✓✗✓✗ |
2 |
boost::mpl::for_each<BVs_t, wrap<boost::mpl::placeholders::_1> >(runner1); |
650 |
|||
651 |
// with lower bound. |
||
652 |
mesh_mesh_run_test runner2(transforms, |
||
653 |
✓✗✓✗ |
4 |
CollisionRequest(DISTANCE_LOWER_BOUND, 1)); |
654 |
2 |
runner2.enable_statistics = false; |
|
655 |
2 |
runner2.benchmark = true; |
|
656 |
✓✗✓✗ |
2 |
boost::mpl::for_each<BVs_t, wrap<boost::mpl::placeholders::_1> >(runner2); |
657 |
|||
658 |
2 |
bench_stream = NULL; |
|
659 |
✓✗ | 2 |
ofs.close(); |
660 |
2 |
} |
Generated by: GCOVR (Version 4.2) |