GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: test/security_margin.cpp Lines: 256 256 100.0 %
Date: 2024-02-09 12:57:42 Branches: 794 1588 50.0 %

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
}