| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
/* |
||
2 |
* Software License Agreement (BSD License) |
||
3 |
* |
||
4 |
* Copyright (c) 2022, 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 Willow Garage, Inc. 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 |
#define BOOST_TEST_MODULE FCL_SECURITY_MARGIN |
||
36 |
#include <boost/test/included/unit_test.hpp> |
||
37 |
|||
38 |
#include <cmath> |
||
39 |
#include <iostream> |
||
40 |
#include <hpp/fcl/distance.h> |
||
41 |
#include <hpp/fcl/math/transform.h> |
||
42 |
#include <hpp/fcl/collision.h> |
||
43 |
#include <hpp/fcl/collision_object.h> |
||
44 |
#include <hpp/fcl/shape/geometric_shapes.h> |
||
45 |
#include <hpp/fcl/shape/geometric_shapes_utility.h> |
||
46 |
#include <hpp/fcl/shape/geometric_shape_to_BVH_model.h> |
||
47 |
|||
48 |
#include "utility.h" |
||
49 |
|||
50 |
using namespace hpp::fcl; |
||
51 |
using hpp::fcl::CollisionGeometryPtr_t; |
||
52 |
using hpp::fcl::CollisionObject; |
||
53 |
using hpp::fcl::CollisionRequest; |
||
54 |
using hpp::fcl::CollisionResult; |
||
55 |
using hpp::fcl::DistanceRequest; |
||
56 |
using hpp::fcl::DistanceResult; |
||
57 |
using hpp::fcl::Transform3f; |
||
58 |
using hpp::fcl::Vec3f; |
||
59 |
|||
60 |
#define MATH_SQUARED(x) (x * x) |
||
61 |
|||
62 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(aabb_aabb) { |
63 |
✓✗✓✗ ✓✗ |
4 |
CollisionGeometryPtr_t b1(new hpp::fcl::Box(1, 1, 1)); |
64 |
✓✗✓✗ ✓✗ |
4 |
CollisionGeometryPtr_t b2(new hpp::fcl::Box(1, 1, 1)); |
65 |
|||
66 |
✓✗ | 2 |
const Transform3f tf1; |
67 |
✓✗✓✗ |
2 |
const Transform3f tf2_collision(Vec3f(0, 1, 1)); |
68 |
✓✗ | 4 |
hpp::fcl::Box s1(1, 1, 1); |
69 |
✓✗ | 4 |
hpp::fcl::Box s2(1, 1, 1); |
70 |
2 |
const double tol = 1e-8; |
|
71 |
|||
72 |
✓✗✓✗ |
2 |
AABB bv1, bv2; |
73 |
✓✗✓✗ |
2 |
computeBV(s1, Transform3f(), bv1); |
74 |
✓✗✓✗ |
2 |
computeBV(s2, Transform3f(), bv2); |
75 |
|||
76 |
// No security margin - collision |
||
77 |
{ |
||
78 |
✓✗ | 2 |
CollisionRequest collisionRequest(CONTACT, 1); |
79 |
✓✗ | 2 |
AABB bv2_transformed; |
80 |
✓✗ | 2 |
computeBV(s2, tf2_collision, bv2_transformed); |
81 |
FCL_REAL sqrDistLowerBound; |
||
82 |
bool res = |
||
83 |
✓✗ | 2 |
bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); |
84 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(res); |
85 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(sqrDistLowerBound, 0, tol); |
86 |
} |
||
87 |
|||
88 |
// No security margin - no collision |
||
89 |
{ |
||
90 |
✓✗ | 2 |
CollisionRequest collisionRequest(CONTACT, 1); |
91 |
2 |
const double distance = 0.01; |
|
92 |
Transform3f tf2_no_collision( |
||
93 |
✓✗✓✗ ✓✗✓✗ |
2 |
Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance))); |
94 |
✓✗ | 2 |
AABB bv2_transformed; |
95 |
✓✗ | 2 |
computeBV(s2, tf2_no_collision, bv2_transformed); |
96 |
FCL_REAL sqrDistLowerBound; |
||
97 |
bool res = |
||
98 |
✓✗ | 2 |
bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); |
99 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(!res); |
100 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(sqrDistLowerBound, MATH_SQUARED(distance), tol); |
101 |
} |
||
102 |
|||
103 |
// Security margin - collision |
||
104 |
{ |
||
105 |
✓✗ | 2 |
CollisionRequest collisionRequest(CONTACT, 1); |
106 |
2 |
const double distance = 0.01; |
|
107 |
2 |
collisionRequest.security_margin = distance; |
|
108 |
Transform3f tf2_no_collision( |
||
109 |
✓✗✓✗ ✓✗✓✗ |
2 |
Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance))); |
110 |
✓✗ | 2 |
AABB bv2_transformed; |
111 |
✓✗ | 2 |
computeBV(s2, tf2_no_collision, bv2_transformed); |
112 |
FCL_REAL sqrDistLowerBound; |
||
113 |
bool res = |
||
114 |
✓✗ | 2 |
bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); |
115 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(res); |
116 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_SMALL(sqrDistLowerBound, tol); |
117 |
} |
||
118 |
|||
119 |
// Negative security margin - collion because the two boxes are in contact |
||
120 |
{ |
||
121 |
✓✗ | 2 |
CollisionRequest collisionRequest(CONTACT, 1); |
122 |
2 |
const double distance = -0.01; |
|
123 |
2 |
collisionRequest.security_margin = distance; |
|
124 |
const Transform3f tf2( |
||
125 |
✓✗✓✗ ✓✗✓✗ |
2 |
Vec3f(tf2_collision.getTranslation() + Vec3f(0, distance, distance))); |
126 |
✓✗ | 2 |
AABB bv2_transformed; |
127 |
✓✗ | 2 |
computeBV(s2, tf2, bv2_transformed); |
128 |
FCL_REAL sqrDistLowerBound; |
||
129 |
bool res = |
||
130 |
✓✗ | 2 |
bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); |
131 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(res); |
132 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_SMALL(sqrDistLowerBound, tol); |
133 |
} |
||
134 |
|||
135 |
// Negative security margin - no collision |
||
136 |
{ |
||
137 |
✓✗ | 2 |
CollisionRequest collisionRequest(CONTACT, 1); |
138 |
2 |
const double distance = -0.01; |
|
139 |
2 |
collisionRequest.security_margin = distance; |
|
140 |
✓✗ | 2 |
AABB bv2_transformed; |
141 |
✓✗ | 2 |
computeBV(s2, tf2_collision, bv2_transformed); |
142 |
FCL_REAL sqrDistLowerBound; |
||
143 |
bool res = |
||
144 |
✓✗ | 2 |
bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); |
145 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(!res); |
146 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE( |
147 |
sqrDistLowerBound, |
||
148 |
MATH_SQUARED((std::sqrt(2) * collisionRequest.security_margin)), tol); |
||
149 |
} |
||
150 |
2 |
} |
|
151 |
|||
152 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(aabb_aabb_degenerated_cases) { |
153 |
✓✗✓✗ ✓✗ |
4 |
CollisionGeometryPtr_t b1(new hpp::fcl::Box(1, 1, 1)); |
154 |
✓✗✓✗ ✓✗ |
4 |
CollisionGeometryPtr_t b2(new hpp::fcl::Box(1, 1, 1)); |
155 |
|||
156 |
✓✗ | 2 |
const Transform3f tf1; |
157 |
✓✗✓✗ |
2 |
const Transform3f tf2_collision(Vec3f(0, 0, 0)); |
158 |
✓✗ | 4 |
hpp::fcl::Box s1(1, 1, 1); |
159 |
✓✗ | 4 |
hpp::fcl::Box s2(1, 1, 1); |
160 |
|||
161 |
✓✗✓✗ |
2 |
AABB bv1, bv2; |
162 |
✓✗✓✗ |
2 |
computeBV(s1, Transform3f(), bv1); |
163 |
✓✗✓✗ |
2 |
computeBV(s2, Transform3f(), bv2); |
164 |
|||
165 |
// The two AABB are collocated |
||
166 |
{ |
||
167 |
✓✗ | 2 |
CollisionRequest collisionRequest(CONTACT, 1); |
168 |
2 |
const double distance = -2.; |
|
169 |
2 |
collisionRequest.security_margin = distance; |
|
170 |
✓✗ | 2 |
AABB bv2_transformed; |
171 |
✓✗ | 2 |
computeBV(s2, tf2_collision, bv2_transformed); |
172 |
FCL_REAL sqrDistLowerBound; |
||
173 |
bool res = |
||
174 |
✓✗ | 2 |
bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); |
175 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(!res); |
176 |
} |
||
177 |
|||
178 |
✓✗ | 2 |
const AABB bv3; |
179 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(!bv1.overlap(bv3)); |
180 |
2 |
} |
|
181 |
|||
182 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(sphere_sphere) { |
183 |
✓✗✓✗ ✓✗ |
4 |
CollisionGeometryPtr_t s1(new hpp::fcl::Sphere(1)); |
184 |
✓✗✓✗ ✓✗ |
4 |
CollisionGeometryPtr_t s2(new hpp::fcl::Sphere(2)); |
185 |
|||
186 |
✓✗ | 2 |
const Transform3f tf1; |
187 |
✓✗✓✗ |
2 |
const Transform3f tf2_collision(Vec3f(0, 0, 3)); |
188 |
|||
189 |
// No security margin - collision |
||
190 |
{ |
||
191 |
✓✗ | 2 |
CollisionRequest collisionRequest(CONTACT, 1); |
192 |
✓✗ | 4 |
CollisionResult collisionResult; |
193 |
✓✗ | 2 |
collide(s1.get(), tf1, s2.get(), tf2_collision, collisionRequest, |
194 |
collisionResult); |
||
195 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(collisionResult.isCollision()); |
196 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, 0, 1e-8); |
197 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_SMALL(-collisionResult.getContact(0).penetration_depth, 1e-8); |
198 |
} |
||
199 |
|||
200 |
// No security margin - no collision |
||
201 |
{ |
||
202 |
✓✗ | 2 |
CollisionRequest collisionRequest(CONTACT, 1); |
203 |
✓✗ | 4 |
CollisionResult collisionResult; |
204 |
2 |
const double distance = 0.01; |
|
205 |
Transform3f tf2_no_collision( |
||
206 |
✓✗✓✗ ✓✗✓✗ |
2 |
Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance))); |
207 |
✓✗ | 2 |
collide(s1.get(), tf1, s2.get(), tf2_no_collision, collisionRequest, |
208 |
collisionResult); |
||
209 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(!collisionResult.isCollision()); |
210 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, distance, 1e-8); |
211 |
} |
||
212 |
|||
213 |
// Positive security margin - collision |
||
214 |
{ |
||
215 |
✓✗ | 2 |
CollisionRequest collisionRequest(CONTACT, 1); |
216 |
✓✗ | 4 |
CollisionResult collisionResult; |
217 |
2 |
const double distance = 0.01; |
|
218 |
2 |
collisionRequest.security_margin = distance; |
|
219 |
Transform3f tf2_no_collision( |
||
220 |
✓✗✓✗ ✓✗✓✗ |
2 |
Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance))); |
221 |
✓✗ | 2 |
collide(s1.get(), tf1, s2.get(), tf2_no_collision, collisionRequest, |
222 |
collisionResult); |
||
223 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(collisionResult.isCollision()); |
224 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, 0, 1e-8); |
225 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(-collisionResult.getContact(0).penetration_depth, |
226 |
distance, 1e-8); |
||
227 |
} |
||
228 |
|||
229 |
// Negative security margin - collion because the two spheres are in contact |
||
230 |
{ |
||
231 |
✓✗ | 2 |
CollisionRequest collisionRequest(CONTACT, 1); |
232 |
✓✗ | 4 |
CollisionResult collisionResult; |
233 |
2 |
const double distance = -0.01; |
|
234 |
2 |
collisionRequest.security_margin = distance; |
|
235 |
Transform3f tf2( |
||
236 |
✓✗✓✗ ✓✗✓✗ |
2 |
Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance))); |
237 |
✓✗ | 2 |
collide(s1.get(), tf1, s2.get(), tf2, collisionRequest, collisionResult); |
238 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(collisionResult.isCollision()); |
239 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, 1e-8); |
240 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(-collisionResult.getContact(0).penetration_depth, |
241 |
distance, 1e-8); |
||
242 |
} |
||
243 |
|||
244 |
// Negative security margin - no collision |
||
245 |
{ |
||
246 |
✓✗ | 2 |
CollisionRequest collisionRequest(CONTACT, 1); |
247 |
✓✗ | 4 |
CollisionResult collisionResult; |
248 |
2 |
collisionRequest.security_margin = -0.01; |
|
249 |
✓✗ | 2 |
collide(s1.get(), tf1, s2.get(), tf2_collision, collisionRequest, |
250 |
collisionResult); |
||
251 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(!collisionResult.isCollision()); |
252 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, |
253 |
-collisionRequest.security_margin, 1e-8); |
||
254 |
} |
||
255 |
2 |
} |
|
256 |
|||
257 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(capsule_capsule) { |
258 |
✓✗✓✗ ✓✗ |
4 |
CollisionGeometryPtr_t c1(new hpp::fcl::Capsule(0.5, 1.)); |
259 |
✓✗✓✗ ✓✗ |
4 |
CollisionGeometryPtr_t c2(new hpp::fcl::Capsule(0.5, 1.)); |
260 |
|||
261 |
✓✗ | 2 |
const Transform3f tf1; |
262 |
✓✗✓✗ |
2 |
const Transform3f tf2_collision(Vec3f(0, 1., 0)); |
263 |
|||
264 |
// No security margin - collision |
||
265 |
{ |
||
266 |
✓✗ | 2 |
CollisionRequest collisionRequest(CONTACT, 1); |
267 |
✓✗ | 4 |
CollisionResult collisionResult; |
268 |
✓✗ | 2 |
collide(c1.get(), tf1, c2.get(), tf2_collision, collisionRequest, |
269 |
collisionResult); |
||
270 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(collisionResult.isCollision()); |
271 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, 1e-8); |
272 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_SMALL(-collisionResult.getContact(0).penetration_depth, 1e-8); |
273 |
} |
||
274 |
|||
275 |
// No security margin - no collision |
||
276 |
{ |
||
277 |
✓✗ | 2 |
CollisionRequest collisionRequest(CONTACT, 1); |
278 |
✓✗ | 4 |
CollisionResult collisionResult; |
279 |
2 |
const double distance = 0.01; |
|
280 |
Transform3f tf2_no_collision( |
||
281 |
✓✗✓✗ ✓✗✓✗ |
2 |
Vec3f(tf2_collision.getTranslation() + Vec3f(0, distance, 0))); |
282 |
✓✗ | 2 |
collide(c1.get(), tf1, c2.get(), tf2_no_collision, collisionRequest, |
283 |
collisionResult); |
||
284 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(!collisionResult.isCollision()); |
285 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, distance, 1e-8); |
286 |
} |
||
287 |
|||
288 |
// Positive security margin - collision |
||
289 |
{ |
||
290 |
✓✗ | 2 |
CollisionRequest collisionRequest(CONTACT, 1); |
291 |
✓✗ | 4 |
CollisionResult collisionResult; |
292 |
2 |
const double distance = 0.01; |
|
293 |
2 |
collisionRequest.security_margin = distance; |
|
294 |
Transform3f tf2_no_collision( |
||
295 |
✓✗✓✗ ✓✗✓✗ |
2 |
Vec3f(tf2_collision.getTranslation() + Vec3f(0, distance, 0))); |
296 |
✓✗ | 2 |
collide(c1.get(), tf1, c2.get(), tf2_no_collision, collisionRequest, |
297 |
collisionResult); |
||
298 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(collisionResult.isCollision()); |
299 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, 1e-8); |
300 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(-collisionResult.getContact(0).penetration_depth, |
301 |
distance, 1e-8); |
||
302 |
} |
||
303 |
|||
304 |
// Negative security margin - collion because the two capsules are in contact |
||
305 |
{ |
||
306 |
✓✗ | 2 |
CollisionRequest collisionRequest(CONTACT, 1); |
307 |
✓✗ | 4 |
CollisionResult collisionResult; |
308 |
2 |
const double distance = -0.01; |
|
309 |
2 |
collisionRequest.security_margin = distance; |
|
310 |
Transform3f tf2( |
||
311 |
✓✗✓✗ ✓✗✓✗ |
2 |
Vec3f(tf2_collision.getTranslation() + Vec3f(0, distance, 0))); |
312 |
✓✗ | 2 |
collide(c1.get(), tf1, c2.get(), tf2, collisionRequest, collisionResult); |
313 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(collisionResult.isCollision()); |
314 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, 1e-8); |
315 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(-collisionResult.getContact(0).penetration_depth, |
316 |
distance, 1e-8); |
||
317 |
} |
||
318 |
|||
319 |
// Negative security margin - no collision |
||
320 |
{ |
||
321 |
✓✗ | 2 |
CollisionRequest collisionRequest(CONTACT, 1); |
322 |
✓✗ | 4 |
CollisionResult collisionResult; |
323 |
2 |
collisionRequest.security_margin = -0.01; |
|
324 |
✓✗ | 2 |
collide(c1.get(), tf1, c2.get(), tf2_collision, collisionRequest, |
325 |
collisionResult); |
||
326 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(!collisionResult.isCollision()); |
327 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, 0.01, 1e-8); |
328 |
} |
||
329 |
2 |
} |
|
330 |
|||
331 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(box_box) { |
332 |
✓✗✓✗ ✓✗ |
4 |
CollisionGeometryPtr_t b1(new hpp::fcl::Box(1, 1, 1)); |
333 |
✓✗✓✗ ✓✗ |
4 |
CollisionGeometryPtr_t b2(new hpp::fcl::Box(1, 1, 1)); |
334 |
|||
335 |
✓✗ | 2 |
const Transform3f tf1; |
336 |
✓✗✓✗ |
2 |
const Transform3f tf2_collision(Vec3f(0, 1, 1)); |
337 |
|||
338 |
2 |
const double tol = 1e-3; |
|
339 |
|||
340 |
// No security margin - collision |
||
341 |
{ |
||
342 |
✓✗ | 2 |
CollisionRequest collisionRequest(CONTACT, 1); |
343 |
✓✗ | 4 |
CollisionResult collisionResult; |
344 |
✓✗ | 2 |
collide(b1.get(), tf1, b2.get(), tf2_collision, collisionRequest, |
345 |
collisionResult); |
||
346 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(collisionResult.isCollision()); |
347 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, tol); |
348 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_SMALL(-collisionResult.getContact(0).penetration_depth, 1e-8); |
349 |
} |
||
350 |
|||
351 |
// No security margin - no collision |
||
352 |
{ |
||
353 |
✓✗ | 2 |
CollisionRequest collisionRequest(CONTACT, 1); |
354 |
2 |
const double distance = 0.01; |
|
355 |
const Transform3f tf2_no_collision( |
||
356 |
✓✗✓✗ ✓✗✓✗ |
2 |
(tf2_collision.getTranslation() + Vec3f(0, 0, distance)).eval()); |
357 |
|||
358 |
✓✗ | 4 |
CollisionResult collisionResult; |
359 |
✓✗ | 2 |
collide(b1.get(), tf1, b2.get(), tf2_no_collision, collisionRequest, |
360 |
collisionResult); |
||
361 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(!collisionResult.isCollision()); |
362 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, distance, tol); |
363 |
} |
||
364 |
|||
365 |
// Positive security margin - collision |
||
366 |
{ |
||
367 |
✓✗ | 2 |
CollisionRequest collisionRequest(CONTACT, 1); |
368 |
2 |
const double distance = 0.01; |
|
369 |
2 |
collisionRequest.security_margin = distance; |
|
370 |
✓✗ | 4 |
CollisionResult collisionResult; |
371 |
✓✗ | 2 |
collide(b1.get(), tf1, b2.get(), tf2_collision, collisionRequest, |
372 |
collisionResult); |
||
373 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(collisionResult.isCollision()); |
374 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, |
375 |
-collisionRequest.security_margin, tol); |
||
376 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_SMALL(-collisionResult.getContact(0).penetration_depth, 1e-8); |
377 |
} |
||
378 |
|||
379 |
// Negative security margin - no collision |
||
380 |
{ |
||
381 |
✓✗ | 2 |
CollisionRequest collisionRequest(CONTACT, 1); |
382 |
2 |
collisionRequest.security_margin = -0.01; |
|
383 |
✓✗ | 4 |
CollisionResult collisionResult; |
384 |
✓✗ | 2 |
collide(b1.get(), tf1, b2.get(), tf2_collision, collisionRequest, |
385 |
collisionResult); |
||
386 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(!collisionResult.isCollision()); |
387 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, |
388 |
-collisionRequest.security_margin, tol); |
||
389 |
} |
||
390 |
|||
391 |
// Negative security margin - collision |
||
392 |
{ |
||
393 |
✓✗ | 2 |
CollisionRequest collisionRequest(CONTACT, 1); |
394 |
2 |
const FCL_REAL distance = -0.01; |
|
395 |
2 |
collisionRequest.security_margin = distance; |
|
396 |
✓✗ | 4 |
CollisionResult collisionResult; |
397 |
|||
398 |
✓✗ | 2 |
const Transform3f tf2((tf2_collision.getTranslation() + |
399 |
✓✗ | 2 |
Vec3f(0, collisionRequest.security_margin, |
400 |
collisionRequest.security_margin)) |
||
401 |
✓✗✓✗ |
2 |
.eval()); |
402 |
✓✗ | 2 |
collide(b1.get(), tf1, b2.get(), tf2, collisionRequest, collisionResult); |
403 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(collisionResult.isCollision()); |
404 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, tol); |
405 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(-collisionResult.getContact(0).penetration_depth, |
406 |
distance, tol); |
||
407 |
} |
||
408 |
2 |
} |
|
409 |
|||
410 |
template <typename ShapeType1, typename ShapeType2> |
||
411 |
4 |
void test_shape_shape(const ShapeType1& shape1, const Transform3f& tf1, |
|
412 |
const ShapeType2& shape2, |
||
413 |
const Transform3f& tf2_collision, const FCL_REAL tol) { |
||
414 |
// No security margin - collision |
||
415 |
{ |
||
416 |
✓✗ | 4 |
CollisionRequest collisionRequest(CONTACT, 1); |
417 |
✓✗ | 8 |
CollisionResult collisionResult; |
418 |
✓✗ | 4 |
collide(&shape1, tf1, &shape2, tf2_collision, collisionRequest, |
419 |
collisionResult); |
||
420 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
4 |
BOOST_CHECK(collisionResult.isCollision()); |
421 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
4 |
BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, tol); |
422 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
4 |
BOOST_CHECK_SMALL(-collisionResult.getContact(0).penetration_depth, 1e-8); |
423 |
} |
||
424 |
|||
425 |
// No security margin - no collision |
||
426 |
{ |
||
427 |
✓✗ | 4 |
CollisionRequest collisionRequest(CONTACT, 1); |
428 |
4 |
const double distance = 0.01; |
|
429 |
✓✗✓✗ ✓✗✓✗ |
8 |
const Transform3f tf2_no_collision( |
430 |
4 |
(tf2_collision.getTranslation() + Vec3f(0, 0, distance)).eval()); |
|
431 |
|||
432 |
✓✗ | 8 |
CollisionResult collisionResult; |
433 |
✓✗ | 4 |
collide(&shape1, tf1, &shape2, tf2_no_collision, collisionRequest, |
434 |
collisionResult); |
||
435 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
4 |
BOOST_CHECK(!collisionResult.isCollision()); |
436 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
4 |
BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, distance, tol); |
437 |
} |
||
438 |
|||
439 |
// Positive security margin - collision |
||
440 |
{ |
||
441 |
✓✗ | 4 |
CollisionRequest collisionRequest(CONTACT, 1); |
442 |
4 |
const double distance = 0.01; |
|
443 |
4 |
collisionRequest.security_margin = distance; |
|
444 |
✓✗ | 8 |
CollisionResult collisionResult; |
445 |
✓✗ | 4 |
collide(&shape1, tf1, &shape2, tf2_collision, collisionRequest, |
446 |
collisionResult); |
||
447 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
4 |
BOOST_CHECK(collisionResult.isCollision()); |
448 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
4 |
BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, |
449 |
-collisionRequest.security_margin, tol); |
||
450 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
4 |
BOOST_CHECK_SMALL(-collisionResult.getContact(0).penetration_depth, 1e-8); |
451 |
} |
||
452 |
|||
453 |
// Negative security margin - no collision |
||
454 |
{ |
||
455 |
✓✗ | 4 |
CollisionRequest collisionRequest(CONTACT, 1); |
456 |
4 |
collisionRequest.security_margin = -0.01; |
|
457 |
✓✗ | 8 |
CollisionResult collisionResult; |
458 |
✓✗ | 4 |
collide(&shape1, tf1, &shape2, tf2_collision, collisionRequest, |
459 |
collisionResult); |
||
460 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
4 |
BOOST_CHECK(!collisionResult.isCollision()); |
461 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
4 |
BOOST_CHECK_CLOSE( |
462 |
collisionResult.distance_lower_bound, |
||
463 |
(collisionRequest.security_margin * tf2_collision.getTranslation()) |
||
464 |
.norm(), |
||
465 |
tol); |
||
466 |
} |
||
467 |
|||
468 |
// Negative security margin - collision |
||
469 |
{ |
||
470 |
✓✗ | 4 |
CollisionRequest collisionRequest(CONTACT, 1); |
471 |
4 |
const FCL_REAL distance = -0.01; |
|
472 |
4 |
collisionRequest.security_margin = distance; |
|
473 |
✓✗ | 8 |
CollisionResult collisionResult; |
474 |
|||
475 |
✓✗✓✗ ✓✗✓✗ |
4 |
const Transform3f tf2((tf2_collision.getTranslation() + |
476 |
Vec3f(0, collisionRequest.security_margin, |
||
477 |
collisionRequest.security_margin)) |
||
478 |
.eval()); |
||
479 |
✓✗ | 4 |
collide(&shape1, tf1, &shape2, tf2, collisionRequest, collisionResult); |
480 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
4 |
BOOST_CHECK(collisionResult.isCollision()); |
481 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
4 |
BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, tol); |
482 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
4 |
BOOST_CHECK_CLOSE(-collisionResult.getContact(0).penetration_depth, |
483 |
distance, tol); |
||
484 |
} |
||
485 |
4 |
} |
|
486 |
|||
487 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(sphere_box) { |
488 |
✓✗✓✗ |
2 |
Box* box_ptr = new hpp::fcl::Box(1, 1, 1); |
489 |
✓✗ | 4 |
CollisionGeometryPtr_t b1(box_ptr); |
490 |
✓✗ | 4 |
BVHModel<OBBRSS> box_bvh_model = BVHModel<OBBRSS>(); |
491 |
✓✗✓✗ |
2 |
generateBVHModel(box_bvh_model, *box_ptr, Transform3f()); |
492 |
✓✗ | 2 |
box_bvh_model.buildConvexRepresentation(false); |
493 |
2 |
ConvexBase& box_convex = *box_bvh_model.convex.get(); |
|
494 |
✓✗✓✗ ✓✗ |
4 |
CollisionGeometryPtr_t s2(new hpp::fcl::Sphere(0.5)); |
495 |
|||
496 |
✓✗ | 2 |
const Transform3f tf1; |
497 |
✓✗✓✗ |
2 |
const Transform3f tf2_collision(Vec3f(0, 0, 1)); |
498 |
|||
499 |
2 |
const double tol = 1e-6; |
|
500 |
|||
501 |
✓✗ | 2 |
test_shape_shape(*b1.get(), tf1, *s2.get(), tf2_collision, tol); |
502 |
✓✗ | 2 |
test_shape_shape(box_convex, tf1, *s2.get(), tf2_collision, tol); |
503 |
2 |
} |
| Generated by: GCOVR (Version 4.2) |