GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
/* |
||
2 |
* Software License Agreement (BSD License) |
||
3 |
* |
||
4 |
* Copyright (c) 2021, INRIA |
||
5 |
* All rights reserved. |
||
6 |
* |
||
7 |
* Redistribution and use in source and binary forms, with or without |
||
8 |
* modification, are permitted provided that the following conditions |
||
9 |
* are met: |
||
10 |
* |
||
11 |
* * Redistributions of source code must retain the above copyright |
||
12 |
* notice, this list of conditions and the following disclaimer. |
||
13 |
* * Redistributions in binary form must reproduce the above |
||
14 |
* copyright notice, this list of conditions and the following |
||
15 |
* disclaimer in the documentation and/or other materials provided |
||
16 |
* with the distribution. |
||
17 |
* * Neither the name of Open Source Robotics Foundation nor the names of its |
||
18 |
* contributors may be used to endorse or promote products derived |
||
19 |
* from this software without specific prior written permission. |
||
20 |
* |
||
21 |
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||
22 |
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||
23 |
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||
24 |
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||
25 |
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||
26 |
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||
27 |
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
||
28 |
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
||
29 |
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||
30 |
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||
31 |
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||
32 |
* POSSIBILITY OF SUCH DAMAGE. |
||
33 |
*/ |
||
34 |
|||
35 |
/** \author Justin Carpentier */ |
||
36 |
|||
37 |
#define BOOST_TEST_MODULE FCL_HEIGHT_FIELDS |
||
38 |
#include <boost/test/included/unit_test.hpp> |
||
39 |
#include <boost/filesystem.hpp> |
||
40 |
|||
41 |
#include "fcl_resources/config.h" |
||
42 |
|||
43 |
#include <hpp/fcl/collision.h> |
||
44 |
#include <hpp/fcl/hfield.h> |
||
45 |
#include <hpp/fcl/math/transform.h> |
||
46 |
#include <hpp/fcl/shape/geometric_shapes.h> |
||
47 |
#include <hpp/fcl/mesh_loader/assimp.h> |
||
48 |
#include <hpp/fcl/mesh_loader/loader.h> |
||
49 |
|||
50 |
#include <hpp/fcl/collision.h> |
||
51 |
|||
52 |
#include "utility.h" |
||
53 |
#include <iostream> |
||
54 |
|||
55 |
using namespace hpp::fcl; |
||
56 |
|||
57 |
template <typename BV> |
||
58 |
12 |
void test_constant_hfields(const Eigen::DenseIndex nx, |
|
59 |
const Eigen::DenseIndex ny, |
||
60 |
const FCL_REAL min_altitude, |
||
61 |
const FCL_REAL max_altitude) { |
||
62 |
12 |
const FCL_REAL x_dim = 1., y_dim = 2.; |
|
63 |
✓✗✓✗ |
24 |
const MatrixXf heights = MatrixXf::Constant(ny, nx, max_altitude); |
64 |
|||
65 |
✓✗ | 24 |
HeightField<BV> hfield(x_dim, y_dim, heights, min_altitude); |
66 |
|||
67 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
12 |
BOOST_CHECK(hfield.getXDim() == x_dim); |
68 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
12 |
BOOST_CHECK(hfield.getYDim() == y_dim); |
69 |
|||
70 |
12 |
const VecXf& x_grid = hfield.getXGrid(); |
|
71 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
12 |
BOOST_CHECK(x_grid[0] == -x_dim / 2.); |
72 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
12 |
BOOST_CHECK(x_grid[nx - 1] == x_dim / 2.); |
73 |
|||
74 |
12 |
const VecXf& y_grid = hfield.getYGrid(); |
|
75 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
12 |
BOOST_CHECK(y_grid[0] == y_dim / 2.); |
76 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
12 |
BOOST_CHECK(y_grid[ny - 1] == -y_dim / 2.); |
77 |
|||
78 |
// Test local AABB |
||
79 |
✓✗ | 12 |
hfield.computeLocalAABB(); |
80 |
|||
81 |
✓✓ | 500 |
for (Eigen::DenseIndex i = 0; i < nx; ++i) { |
82 |
✓✓ | 40664 |
for (Eigen::DenseIndex j = 0; j < ny; ++j) { |
83 |
✓✗✓✗ ✓✗✓✗ |
40176 |
Vec3f point(x_grid[i], y_grid[j], heights(j, i)); |
84 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
40176 |
BOOST_CHECK(hfield.aabb_local.contain(point)); |
85 |
} |
||
86 |
} |
||
87 |
|||
88 |
// Test clone |
||
89 |
{ |
||
90 |
✓✗ | 12 |
HeightField<BV>* hfield_clone = hfield.clone(); |
91 |
✓✗ | 12 |
hfield_clone->computeLocalAABB(); |
92 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
12 |
BOOST_CHECK(*hfield_clone == hfield); |
93 |
|||
94 |
✓✗ | 12 |
delete hfield_clone; |
95 |
} |
||
96 |
|||
97 |
// Build equivalent object |
||
98 |
✓✗ | 24 |
const Box equivalent_box(x_dim, y_dim, max_altitude - min_altitude); |
99 |
✓✗✓✗ |
12 |
const Transform3f box_placement( |
100 |
✓✗ | 12 |
Matrix3f::Identity(), Vec3f(0., 0., (max_altitude + min_altitude) / 2.)); |
101 |
|||
102 |
// Test collision |
||
103 |
✓✗ | 24 |
const Sphere sphere(1.); |
104 |
✓✓✓✗ ✓✗✗✗ |
12 |
static const Transform3f IdTransform; |
105 |
|||
106 |
✓✗✓✗ ✓✗ |
24 |
const Box box(Vec3f::Ones()); |
107 |
|||
108 |
✓✗✓✗ |
12 |
Transform3f M_sphere, M_box; |
109 |
|||
110 |
// No collision case |
||
111 |
{ |
||
112 |
12 |
const FCL_REAL eps_no_collision = +0.1 * (max_altitude - min_altitude); |
|
113 |
✓✗ | 12 |
M_sphere.setTranslation( |
114 |
✓✗ | 12 |
Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); |
115 |
✓✗ | 12 |
M_box.setTranslation( |
116 |
✓✗✓✗ |
12 |
Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); |
117 |
✓✗ | 12 |
CollisionRequest request; |
118 |
|||
119 |
✓✗ | 24 |
CollisionResult result; |
120 |
✓✗ | 12 |
collide(&hfield, IdTransform, &sphere, M_sphere, request, result); |
121 |
|||
122 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
12 |
BOOST_CHECK(!result.isCollision()); |
123 |
|||
124 |
✓✗ | 24 |
CollisionResult result_check_sphere; |
125 |
✓✗✓✗ |
12 |
collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, |
126 |
request, result_check_sphere); |
||
127 |
|||
128 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
12 |
BOOST_CHECK(!result_check_sphere.isCollision()); |
129 |
|||
130 |
✓✗ | 24 |
CollisionResult result_check_box; |
131 |
✓✗✓✗ |
12 |
collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, |
132 |
result_check_box); |
||
133 |
|||
134 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
12 |
BOOST_CHECK(!result_check_box.isCollision()); |
135 |
} |
||
136 |
|||
137 |
// Collision case |
||
138 |
{ |
||
139 |
12 |
const FCL_REAL eps_collision = -0.1 * (max_altitude - min_altitude); |
|
140 |
✓✗ | 12 |
M_sphere.setTranslation( |
141 |
✓✗ | 12 |
Vec3f(0., 0., max_altitude + sphere.radius + eps_collision)); |
142 |
✓✗ | 12 |
M_box.setTranslation( |
143 |
✓✗✓✗ |
12 |
Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_collision)); |
144 |
CollisionRequest |
||
145 |
✓✗ | 12 |
request; //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1))); |
146 |
|||
147 |
✓✗ | 24 |
CollisionResult result; |
148 |
✓✗ | 12 |
collide(&hfield, IdTransform, &sphere, M_sphere, request, result); |
149 |
|||
150 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
12 |
BOOST_CHECK(result.isCollision()); |
151 |
|||
152 |
✓✗ | 24 |
CollisionResult result_check; |
153 |
✓✗✓✗ |
12 |
collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, |
154 |
request, result_check); |
||
155 |
|||
156 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
12 |
BOOST_CHECK(result_check.isCollision()); |
157 |
|||
158 |
✓✗ | 24 |
CollisionResult result_check_box; |
159 |
✓✗✓✗ |
12 |
collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, |
160 |
result_check_box); |
||
161 |
|||
162 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
12 |
BOOST_CHECK(result_check_box.isCollision()); |
163 |
} |
||
164 |
|||
165 |
// Update height |
||
166 |
✓✗✓✗ |
12 |
hfield.updateHeights( |
167 |
✓✗ | 12 |
MatrixXf::Constant(ny, nx, max_altitude / 2.)); // We change nothing |
168 |
|||
169 |
// No collision case |
||
170 |
{ |
||
171 |
12 |
const FCL_REAL eps_no_collision = +0.1 * (max_altitude - min_altitude); |
|
172 |
✓✗ | 12 |
M_sphere.setTranslation( |
173 |
✓✗ | 12 |
Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); |
174 |
✓✗ | 12 |
M_box.setTranslation( |
175 |
✓✗✓✗ |
12 |
Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); |
176 |
✓✗ | 12 |
CollisionRequest request; |
177 |
|||
178 |
✓✗ | 24 |
CollisionResult result; |
179 |
✓✗ | 12 |
collide(&hfield, IdTransform, &sphere, M_sphere, request, result); |
180 |
|||
181 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
12 |
BOOST_CHECK(!result.isCollision()); |
182 |
|||
183 |
✓✗ | 24 |
CollisionResult result_check_sphere; |
184 |
✓✗✓✗ |
12 |
collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, |
185 |
request, result_check_sphere); |
||
186 |
|||
187 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
12 |
BOOST_CHECK(!result_check_sphere.isCollision()); |
188 |
|||
189 |
✓✗ | 24 |
CollisionResult result_check_box; |
190 |
✓✗✓✗ |
12 |
collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, |
191 |
result_check_box); |
||
192 |
|||
193 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
12 |
BOOST_CHECK(!result_check_box.isCollision()); |
194 |
} |
||
195 |
|||
196 |
// Collision case |
||
197 |
{ |
||
198 |
12 |
const FCL_REAL eps_collision = -0.1 * (max_altitude - min_altitude); |
|
199 |
✓✗ | 12 |
M_sphere.setTranslation( |
200 |
✓✗ | 12 |
Vec3f(0., 0., max_altitude + sphere.radius + eps_collision)); |
201 |
✓✗ | 12 |
M_box.setTranslation( |
202 |
✓✗✓✗ |
12 |
Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_collision)); |
203 |
CollisionRequest |
||
204 |
✓✗ | 12 |
request; //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1))); |
205 |
|||
206 |
✓✗ | 24 |
CollisionResult result; |
207 |
✓✗ | 12 |
collide(&hfield, IdTransform, &sphere, M_sphere, request, result); |
208 |
|||
209 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
12 |
BOOST_CHECK(!result.isCollision()); |
210 |
|||
211 |
✓✗ | 24 |
CollisionResult result_check; |
212 |
✓✗✓✗ |
12 |
collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, |
213 |
request, result_check); |
||
214 |
|||
215 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
12 |
BOOST_CHECK(result_check.isCollision()); |
216 |
|||
217 |
✓✗ | 24 |
CollisionResult result_check_box; |
218 |
✓✗✓✗ |
12 |
collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, |
219 |
result_check_box); |
||
220 |
|||
221 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
12 |
BOOST_CHECK(result_check_box.isCollision()); |
222 |
} |
||
223 |
|||
224 |
// Restore height |
||
225 |
✓✗✓✗ ✓✗ |
12 |
hfield.updateHeights( |
226 |
MatrixXf::Constant(ny, nx, max_altitude)); // We change nothing |
||
227 |
|||
228 |
// Collision case |
||
229 |
{ |
||
230 |
12 |
const FCL_REAL eps_collision = -0.1 * (max_altitude - min_altitude); |
|
231 |
✓✗ | 12 |
M_sphere.setTranslation( |
232 |
✓✗ | 12 |
Vec3f(0., 0., max_altitude + sphere.radius + eps_collision)); |
233 |
✓✗ | 12 |
M_box.setTranslation( |
234 |
✓✗✓✗ |
12 |
Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_collision)); |
235 |
CollisionRequest |
||
236 |
✓✗ | 12 |
request; //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1))); |
237 |
|||
238 |
✓✗ | 24 |
CollisionResult result; |
239 |
✓✗ | 12 |
collide(&hfield, IdTransform, &sphere, M_sphere, request, result); |
240 |
|||
241 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
12 |
BOOST_CHECK(result.isCollision()); |
242 |
|||
243 |
✓✗ | 24 |
CollisionResult result_check; |
244 |
✓✗✓✗ |
12 |
collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, |
245 |
request, result_check); |
||
246 |
|||
247 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
12 |
BOOST_CHECK(result_check.isCollision()); |
248 |
|||
249 |
✓✗ | 24 |
CollisionResult result_check_box; |
250 |
✓✗✓✗ |
12 |
collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, |
251 |
result_check_box); |
||
252 |
|||
253 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
12 |
BOOST_CHECK(result_check_box.isCollision()); |
254 |
} |
||
255 |
12 |
} |
|
256 |
|||
257 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(building_constant_hfields) { |
258 |
2 |
const FCL_REAL max_altitude = 1., min_altitude = 0.; |
|
259 |
|||
260 |
2 |
test_constant_hfields<OBBRSS>(2, 2, min_altitude, |
|
261 |
max_altitude); // Simple case |
||
262 |
2 |
test_constant_hfields<OBBRSS>(20, 2, min_altitude, max_altitude); |
|
263 |
2 |
test_constant_hfields<OBBRSS>(100, 100, min_altitude, max_altitude); |
|
264 |
// test_constant_hfields<OBBRSS>(1000,1000,min_altitude,max_altitude); |
||
265 |
|||
266 |
2 |
test_constant_hfields<AABB>(2, 2, min_altitude, max_altitude); // Simple case |
|
267 |
2 |
test_constant_hfields<AABB>(20, 2, min_altitude, max_altitude); |
|
268 |
2 |
test_constant_hfields<AABB>(100, 100, min_altitude, max_altitude); |
|
269 |
2 |
} |
|
270 |
|||
271 |
template <typename BV> |
||
272 |
1 |
void test_negative_security_margin(const Eigen::DenseIndex nx, |
|
273 |
const Eigen::DenseIndex ny, |
||
274 |
const FCL_REAL min_altitude, |
||
275 |
const FCL_REAL max_altitude) { |
||
276 |
1 |
const FCL_REAL x_dim = 1., y_dim = 2.; |
|
277 |
✓✗✓✗ |
2 |
const MatrixXf heights = MatrixXf::Constant(ny, nx, max_altitude); |
278 |
|||
279 |
✓✗ | 2 |
HeightField<BV> hfield(x_dim, y_dim, heights, min_altitude); |
280 |
|||
281 |
// Build equivalent object |
||
282 |
✓✗ | 2 |
const Box equivalent_box(x_dim, y_dim, max_altitude - min_altitude); |
283 |
✓✗✓✗ |
1 |
const Transform3f box_placement( |
284 |
✓✗ | 1 |
Matrix3f::Identity(), Vec3f(0., 0., (max_altitude + min_altitude) / 2.)); |
285 |
|||
286 |
// Test collision |
||
287 |
✓✗ | 2 |
const Sphere sphere(1.); |
288 |
✓✗✓✗ ✓✗✗✗ |
1 |
static const Transform3f IdTransform; |
289 |
|||
290 |
✓✗✓✗ ✓✗ |
2 |
const Box box(Vec3f::Ones()); |
291 |
|||
292 |
✓✗✓✗ |
1 |
Transform3f M_sphere, M_box; |
293 |
|||
294 |
// No collision case |
||
295 |
{ |
||
296 |
1 |
const FCL_REAL eps_no_collision = +0.1 * (max_altitude - min_altitude); |
|
297 |
✓✗ | 1 |
M_sphere.setTranslation( |
298 |
✓✗ | 1 |
Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); |
299 |
✓✗ | 1 |
M_box.setTranslation( |
300 |
✓✗✓✗ |
1 |
Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); |
301 |
✓✗ | 1 |
CollisionRequest request; |
302 |
|||
303 |
✓✗ | 2 |
CollisionResult result; |
304 |
✓✗ | 1 |
collide(&hfield, IdTransform, &sphere, M_sphere, request, result); |
305 |
|||
306 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1 |
BOOST_CHECK(!result.isCollision()); |
307 |
|||
308 |
✓✗ | 2 |
CollisionResult result_check_sphere; |
309 |
✓✗✓✗ |
1 |
collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, |
310 |
request, result_check_sphere); |
||
311 |
|||
312 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1 |
BOOST_CHECK(!result_check_sphere.isCollision()); |
313 |
|||
314 |
✓✗ | 2 |
CollisionResult result_check_box; |
315 |
✓✗✓✗ |
1 |
collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, |
316 |
result_check_box); |
||
317 |
|||
318 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1 |
BOOST_CHECK(!result_check_box.isCollision()); |
319 |
} |
||
320 |
|||
321 |
// Collision case - positive security_margin |
||
322 |
{ |
||
323 |
1 |
const FCL_REAL eps_no_collision = +0.1 * (max_altitude - min_altitude); |
|
324 |
✓✗ | 1 |
M_sphere.setTranslation( |
325 |
✓✗ | 1 |
Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); |
326 |
✓✗ | 1 |
M_box.setTranslation( |
327 |
✓✗✓✗ |
1 |
Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); |
328 |
✓✗ | 1 |
CollisionRequest request; |
329 |
1 |
request.security_margin = eps_no_collision + 1e-6; |
|
330 |
|||
331 |
✓✗ | 2 |
CollisionResult result; |
332 |
✓✗ | 1 |
collide(&hfield, IdTransform, &sphere, M_sphere, request, result); |
333 |
|||
334 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1 |
BOOST_CHECK(result.isCollision()); |
335 |
|||
336 |
✓✗ | 2 |
CollisionResult result_check_sphere; |
337 |
✓✗✓✗ |
1 |
collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, |
338 |
request, result_check_sphere); |
||
339 |
|||
340 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1 |
BOOST_CHECK(result_check_sphere.isCollision()); |
341 |
|||
342 |
✓✗ | 2 |
CollisionResult result_check_box; |
343 |
✓✗✓✗ |
1 |
collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, |
344 |
result_check_box); |
||
345 |
|||
346 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1 |
BOOST_CHECK(result_check_box.isCollision()); |
347 |
} |
||
348 |
|||
349 |
// Collision case |
||
350 |
{ |
||
351 |
1 |
const FCL_REAL eps_no_collision = -0.1 * (max_altitude - min_altitude); |
|
352 |
✓✗ | 1 |
M_sphere.setTranslation( |
353 |
✓✗ | 1 |
Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); |
354 |
✓✗ | 1 |
M_box.setTranslation( |
355 |
✓✗✓✗ |
1 |
Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); |
356 |
✓✗ | 1 |
CollisionRequest request; |
357 |
|||
358 |
✓✗ | 2 |
CollisionResult result; |
359 |
✓✗ | 1 |
collide(&hfield, IdTransform, &sphere, M_sphere, request, result); |
360 |
|||
361 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1 |
BOOST_CHECK(result.isCollision()); |
362 |
|||
363 |
✓✗ | 2 |
CollisionResult result_check_sphere; |
364 |
✓✗✓✗ |
1 |
collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, |
365 |
request, result_check_sphere); |
||
366 |
|||
367 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1 |
BOOST_CHECK(result_check_sphere.isCollision()); |
368 |
|||
369 |
✓✗ | 2 |
CollisionResult result_check_box; |
370 |
✓✗✓✗ |
1 |
collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, |
371 |
result_check_box); |
||
372 |
|||
373 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1 |
BOOST_CHECK(result_check_box.isCollision()); |
374 |
} |
||
375 |
|||
376 |
// No collision case - negative security_margin |
||
377 |
{ |
||
378 |
1 |
const FCL_REAL eps_no_collision = -0.1 * (max_altitude - min_altitude); |
|
379 |
✓✗ | 1 |
M_sphere.setTranslation( |
380 |
✓✗ | 1 |
Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); |
381 |
✓✗ | 1 |
M_box.setTranslation( |
382 |
✓✗✓✗ |
1 |
Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); |
383 |
✓✗ | 1 |
CollisionRequest request; |
384 |
1 |
request.security_margin = eps_no_collision - 1e-4; |
|
385 |
|||
386 |
✓✗ | 2 |
CollisionResult result; |
387 |
✓✗ | 1 |
collide(&hfield, IdTransform, &sphere, M_sphere, request, result); |
388 |
|||
389 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1 |
BOOST_CHECK(!result.isCollision()); |
390 |
|||
391 |
✓✗ | 2 |
CollisionResult result_check_sphere; |
392 |
✓✗✓✗ |
1 |
collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, |
393 |
request, result_check_sphere); |
||
394 |
|||
395 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1 |
BOOST_CHECK(!result_check_sphere.isCollision()); |
396 |
|||
397 |
✓✗ | 2 |
CollisionResult result_check_box; |
398 |
✓✗✓✗ |
1 |
collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, |
399 |
result_check_box); |
||
400 |
|||
401 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
1 |
BOOST_CHECK(!result_check_box.isCollision()); |
402 |
} |
||
403 |
1 |
} |
|
404 |
|||
405 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(negative_security_margin) { |
406 |
2 |
const FCL_REAL max_altitude = 1., min_altitude = 0.; |
|
407 |
|||
408 |
// test_negative_security_margin<OBBRSS>(100, 100, min_altitude, |
||
409 |
// max_altitude); |
||
410 |
2 |
test_negative_security_margin<AABB>(100, 100, min_altitude, max_altitude); |
|
411 |
2 |
} |
|
412 |
|||
413 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(hfield_with_square_hole) { |
414 |
2 |
const Eigen::DenseIndex nx = 100, ny = 100; |
|
415 |
|||
416 |
typedef AABB BV; |
||
417 |
const MatrixXf X = |
||
418 |
✓✗✓✗ ✓✗ |
4 |
Eigen::RowVectorXd::LinSpaced(nx, -1., 1.).replicate(ny, 1); |
419 |
✓✗✓✗ ✓✗ |
4 |
const MatrixXf Y = Eigen::VectorXd::LinSpaced(ny, 1., -1.).replicate(1, nx); |
420 |
|||
421 |
2 |
const FCL_REAL dim_square = 0.5; |
|
422 |
|||
423 |
const Eigen::Array<bool, Eigen::Dynamic, Eigen::Dynamic> hole = |
||
424 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
4 |
(X.array().abs() < dim_square) && (Y.array().abs() < dim_square); |
425 |
|||
426 |
const MatrixXf heights = |
||
427 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
MatrixXf::Ones(ny, nx) - hole.cast<double>().matrix(); |
428 |
|||
429 |
✓✗ | 4 |
const HeightField<BV> hfield(2., 2., heights, -10.); |
430 |
|||
431 |
✓✗ | 4 |
Sphere sphere(0.48); |
432 |
✓✗✓✗ |
2 |
const Transform3f sphere_pos(Vec3f(0., 0., 0.5)); |
433 |
✓✗ | 2 |
const Transform3f hfield_pos; |
434 |
|||
435 |
✓✗ | 2 |
const CollisionRequest request; |
436 |
|||
437 |
✓✗ | 4 |
CollisionResult result; |
438 |
{ |
||
439 |
✓✗ | 2 |
collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); |
440 |
|||
441 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(!result.isCollision()); |
442 |
} |
||
443 |
|||
444 |
2 |
sphere.radius = 0.51; |
|
445 |
|||
446 |
{ |
||
447 |
✓✗ | 4 |
CollisionResult result; |
448 |
✓✗ | 4 |
const Sphere sphere2(0.51); |
449 |
✓✗ | 2 |
collide(&hfield, hfield_pos, &sphere2, sphere_pos, request, result); |
450 |
|||
451 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(result.isCollision()); |
452 |
} |
||
453 |
2 |
} |
|
454 |
|||
455 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(hfield_with_circular_hole) { |
456 |
2 |
const Eigen::DenseIndex nx = 100, ny = 100; |
|
457 |
|||
458 |
// typedef OBBRSS BV; TODO(jcarpent): OBBRSS does not work (compile in Debug |
||
459 |
// mode), as the overlap of OBBRSS is not satisfactory yet. |
||
460 |
typedef AABB BV; |
||
461 |
const MatrixXf X = |
||
462 |
✓✗✓✗ ✓✗ |
4 |
Eigen::RowVectorXd::LinSpaced(nx, -1., 1.).replicate(ny, 1); |
463 |
✓✗✓✗ ✓✗ |
4 |
const MatrixXf Y = Eigen::VectorXd::LinSpaced(ny, 1., -1.).replicate(1, nx); |
464 |
|||
465 |
2 |
const FCL_REAL dim_hole = 1; |
|
466 |
|||
467 |
const Eigen::Array<bool, Eigen::Dynamic, Eigen::Dynamic> hole = |
||
468 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
(X.array().square() + Y.array().square() <= dim_hole); |
469 |
|||
470 |
const MatrixXf heights = |
||
471 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
MatrixXf::Ones(ny, nx) - hole.cast<double>().matrix(); |
472 |
|||
473 |
✓✗ | 4 |
const HeightField<BV> hfield(2., 2., heights, -10.); |
474 |
|||
475 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(hfield.getXGrid()[0] == -1.); |
476 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(hfield.getXGrid()[nx - 1] == +1.); |
477 |
|||
478 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(hfield.getYGrid()[0] == +1.); |
479 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(hfield.getYGrid()[ny - 1] == -1.); |
480 |
|||
481 |
✓✗ | 4 |
Sphere sphere(0.975); |
482 |
✓✗✓✗ |
2 |
const Transform3f sphere_pos(Vec3f(0., 0., 1.)); |
483 |
✓✗ | 2 |
const Transform3f hfield_pos; |
484 |
|||
485 |
{ |
||
486 |
✓✗ | 4 |
CollisionResult result; |
487 |
✓✗ | 2 |
CollisionRequest request; |
488 |
2 |
request.security_margin = 0.; |
|
489 |
✓✗ | 2 |
collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); |
490 |
|||
491 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(!result.isCollision()); |
492 |
} |
||
493 |
|||
494 |
{ |
||
495 |
✓✗ | 4 |
CollisionResult result; |
496 |
✓✗ | 2 |
CollisionRequest request; |
497 |
2 |
request.security_margin = 0.01; |
|
498 |
✓✗ | 2 |
collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); |
499 |
|||
500 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(!result.isCollision()); |
501 |
} |
||
502 |
|||
503 |
{ |
||
504 |
✓✗ | 4 |
CollisionResult result; |
505 |
✓✗ | 2 |
CollisionRequest request; |
506 |
2 |
request.security_margin = 1. - sphere.radius; |
|
507 |
✓✗ | 2 |
collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); |
508 |
|||
509 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(result.isCollision()); |
510 |
} |
||
511 |
|||
512 |
{ |
||
513 |
✓✗ | 4 |
CollisionResult result; |
514 |
✓✗ | 2 |
CollisionRequest request; |
515 |
2 |
request.security_margin = -0.005; |
|
516 |
✓✗ | 2 |
collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); |
517 |
|||
518 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(!result.isCollision()); |
519 |
} |
||
520 |
2 |
} |
Generated by: GCOVR (Version 4.2) |