Skip to content

Commit c9486af

Browse files
lmontautlmontaut
committed
test/geometric_shapes: fix cone/cylinders assertions
When EPA is used on strictly convex shapes with very large intersections (like two shapes perfectly blended with one another), we can't use the default tolerance of 1e-6; it is way too low. To converge to that precision, EPA would need a lot of iterations, a lot of vertices and a lot of faces. This is expected behavior. The test cases were modified accordingly.
1 parent c89c161 commit c9486af

File tree

1 file changed

+199
-40
lines changed

1 file changed

+199
-40
lines changed

‎test/geometric_shapes.cpp

+199-40
Original file line numberDiff line numberDiff line change
@@ -3164,15 +3164,47 @@ BOOST_AUTO_TEST_CASE(shapeDistance_cylindercylinder) {
31643164
bool res;
31653165
FCL_REAL dist;
31663166

3167-
res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
3168-
closest_p1, closest_p2, normal);
3169-
BOOST_CHECK(dist <= 0);
3170-
BOOST_CHECK(res);
3167+
{
3168+
// The following situations corresponds to the case where the two cylinders
3169+
// are exactly superposed. This is the worst case for EPA which will take
3170+
// forever to converge with default parameters.
3171+
res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
3172+
closest_p1, closest_p2, normal);
3173+
BOOST_CHECK(dist <= 0);
3174+
BOOST_CHECK(!res);
31713175

3172-
res = solver1.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
3173-
closest_p2, normal);
3174-
BOOST_CHECK(dist <= 0);
3175-
BOOST_CHECK(res);
3176+
res = solver1.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
3177+
closest_p2, normal);
3178+
BOOST_CHECK(dist <= 0);
3179+
BOOST_CHECK(!res);
3180+
3181+
// To handle the superposing case, we have to decrease the tolerance of EPA
3182+
// and allow it to work with more vertices and faces.
3183+
size_t epa_max_face_num_backup = solver1.epa_max_face_num;
3184+
size_t epa_max_vertices_num_backup = solver1.epa_max_vertex_num;
3185+
FCL_REAL epa_tolerance_backup = solver1.epa.tolerance;
3186+
size_t epa_max_iterations_backup = solver1.epa.max_iterations;
3187+
solver1.epa_max_face_num = 1000;
3188+
solver1.epa_max_vertex_num = 1000;
3189+
solver1.epa.tolerance = 1e-2;
3190+
solver1.epa.max_iterations = 1000;
3191+
3192+
res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
3193+
closest_p1, closest_p2, normal);
3194+
BOOST_CHECK(dist <= 0);
3195+
BOOST_CHECK(res);
3196+
3197+
res = solver1.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
3198+
closest_p2, normal);
3199+
BOOST_CHECK(dist <= 0);
3200+
BOOST_CHECK(res);
3201+
3202+
// We restore the original values of the EPA parameters
3203+
solver1.epa_max_face_num = epa_max_face_num_backup;
3204+
solver1.epa_max_vertex_num = epa_max_vertices_num_backup;
3205+
solver1.epa.tolerance = epa_tolerance_backup;
3206+
solver1.epa.max_iterations = epa_max_iterations_backup;
3207+
}
31763208

31773209
res = solver1.shapeDistance(s1, Transform3f(), s2,
31783210
Transform3f(Vec3f(10.1, 0, 0)), dist, closest_p1,
@@ -3210,15 +3242,47 @@ BOOST_AUTO_TEST_CASE(shapeDistance_conecone) {
32103242
bool res;
32113243
FCL_REAL dist;
32123244

3213-
res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
3214-
closest_p1, closest_p2, normal);
3215-
BOOST_CHECK(dist <= 0);
3216-
BOOST_CHECK(res);
3245+
{
3246+
// The following situations corresponds to the case where the two cones
3247+
// are exactly superposed. This is the worst case for EPA which will take
3248+
// forever to converge with default parameters.
3249+
res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
3250+
closest_p1, closest_p2, normal);
3251+
BOOST_CHECK(dist <= 0);
3252+
BOOST_CHECK(!res);
32173253

3218-
res = solver1.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
3219-
closest_p2, normal);
3220-
BOOST_CHECK(dist <= 0);
3221-
BOOST_CHECK(res);
3254+
res = solver1.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
3255+
closest_p2, normal);
3256+
BOOST_CHECK(dist <= 0);
3257+
BOOST_CHECK(!res);
3258+
3259+
// To handle the superposing case, we have to decrease the tolerance of EPA
3260+
// and allow it to work with more vertices and faces.
3261+
size_t epa_max_face_num_backup = solver1.epa_max_face_num;
3262+
size_t epa_max_vertices_num_backup = solver1.epa_max_vertex_num;
3263+
FCL_REAL epa_tolerance_backup = solver1.epa.tolerance;
3264+
size_t epa_max_iterations_backup = solver1.epa.max_iterations;
3265+
solver1.epa_max_face_num = 1000;
3266+
solver1.epa_max_vertex_num = 1000;
3267+
solver1.epa.tolerance = 1e-2;
3268+
solver1.epa.max_iterations = 1000;
3269+
3270+
res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
3271+
closest_p1, closest_p2, normal);
3272+
BOOST_CHECK(dist <= 0);
3273+
BOOST_CHECK(res);
3274+
3275+
res = solver1.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
3276+
closest_p2, normal);
3277+
BOOST_CHECK(dist <= 0);
3278+
BOOST_CHECK(res);
3279+
3280+
// We restore the original values of the EPA parameters
3281+
solver1.epa_max_face_num = epa_max_face_num_backup;
3282+
solver1.epa_max_vertex_num = epa_max_vertices_num_backup;
3283+
solver1.epa.tolerance = epa_tolerance_backup;
3284+
solver1.epa.max_iterations = epa_max_iterations_backup;
3285+
}
32223286

32233287
res = solver1.shapeDistance(s1, Transform3f(), s2,
32243288
Transform3f(Vec3f(10.1, 0, 0)), dist, closest_p1,
@@ -3256,15 +3320,47 @@ BOOST_AUTO_TEST_CASE(shapeDistance_conecylinder) {
32563320
bool res;
32573321
FCL_REAL dist;
32583322

3259-
res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
3260-
closest_p1, closest_p2, normal);
3261-
BOOST_CHECK(dist <= 0);
3262-
BOOST_CHECK(res);
3323+
{
3324+
// The following situations corresponds to the case where the two cones
3325+
// are exactly superposed. This is the worst case for EPA which will take
3326+
// forever to converge with default parameters.
3327+
res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
3328+
closest_p1, closest_p2, normal);
3329+
BOOST_CHECK(dist <= 0);
3330+
BOOST_CHECK(!res);
32633331

3264-
res = solver1.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
3265-
closest_p2, normal);
3266-
BOOST_CHECK(dist <= 0);
3267-
BOOST_CHECK(res);
3332+
res = solver1.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
3333+
closest_p2, normal);
3334+
BOOST_CHECK(dist <= 0);
3335+
BOOST_CHECK(!res);
3336+
3337+
// To handle the superposing case, we have to decrease the tolerance of EPA
3338+
// and allow it to work with more vertices and faces.
3339+
size_t epa_max_face_num_backup = solver1.epa_max_face_num;
3340+
size_t epa_max_vertices_num_backup = solver1.epa_max_vertex_num;
3341+
FCL_REAL epa_tolerance_backup = solver1.epa.tolerance;
3342+
size_t epa_max_iterations_backup = solver1.epa.max_iterations;
3343+
solver1.epa_max_face_num = 1000;
3344+
solver1.epa_max_vertex_num = 1000;
3345+
solver1.epa.tolerance = 1e-2;
3346+
solver1.epa.max_iterations = 1000;
3347+
3348+
res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
3349+
closest_p1, closest_p2, normal);
3350+
BOOST_CHECK(dist <= 0);
3351+
BOOST_CHECK(res);
3352+
3353+
res = solver1.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
3354+
closest_p2, normal);
3355+
BOOST_CHECK(dist <= 0);
3356+
BOOST_CHECK(res);
3357+
3358+
// We restore the original values of the EPA parameters
3359+
solver1.epa_max_face_num = epa_max_face_num_backup;
3360+
solver1.epa_max_vertex_num = epa_max_vertices_num_backup;
3361+
solver1.epa.tolerance = epa_tolerance_backup;
3362+
solver1.epa.max_iterations = epa_max_iterations_backup;
3363+
}
32683364

32693365
res = solver1.shapeDistance(s1, Transform3f(), s2,
32703366
Transform3f(Vec3f(10.1, 0, 0)), dist, closest_p1,
@@ -4088,16 +4184,47 @@ BOOST_AUTO_TEST_CASE(cylindercylinder) {
40884184
bool res;
40894185
FCL_REAL dist;
40904186

4091-
res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
4092-
closest_p1, closest_p2, normal);
4093-
BOOST_CHECK(dist <= 0);
4094-
BOOST_CHECK(res);
4187+
{
4188+
// The following situations corresponds to the case where the two cylinders
4189+
// are exactly superposed. This is the worst case for EPA which will take
4190+
// forever to converge with default parameters.
4191+
res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
4192+
closest_p1, closest_p2, normal);
4193+
BOOST_CHECK(dist <= 0);
4194+
BOOST_CHECK(!res);
40954195

4096-
res = solver2.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
4097-
closest_p2, normal);
4098-
BOOST_CHECK(dist <= 0);
4099-
BOOST_CHECK(res);
4196+
res = solver1.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
4197+
closest_p2, normal);
4198+
BOOST_CHECK(dist <= 0);
4199+
BOOST_CHECK(!res);
4200+
4201+
// To handle the superposing case, we have to decrease the tolerance of EPA
4202+
// and allow it to work with more vertices and faces.
4203+
size_t epa_max_face_num_backup = solver1.epa_max_face_num;
4204+
size_t epa_max_vertices_num_backup = solver1.epa_max_vertex_num;
4205+
FCL_REAL epa_tolerance_backup = solver1.epa.tolerance;
4206+
size_t epa_max_iterations_backup = solver1.epa.max_iterations;
4207+
solver1.epa_max_face_num = 1000;
4208+
solver1.epa_max_vertex_num = 1000;
4209+
solver1.epa.tolerance = 1e-2;
4210+
solver1.epa.max_iterations = 1000;
4211+
4212+
res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
4213+
closest_p1, closest_p2, normal);
4214+
BOOST_CHECK(dist <= 0);
4215+
BOOST_CHECK(res);
41004216

4217+
res = solver1.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
4218+
closest_p2, normal);
4219+
BOOST_CHECK(dist <= 0);
4220+
BOOST_CHECK(res);
4221+
4222+
// We restore the original values of the EPA parameters
4223+
solver1.epa_max_face_num = epa_max_face_num_backup;
4224+
solver1.epa_max_vertex_num = epa_max_vertices_num_backup;
4225+
solver1.epa.tolerance = epa_tolerance_backup;
4226+
solver1.epa.max_iterations = epa_max_iterations_backup;
4227+
}
41014228
res = solver2.shapeDistance(s1, Transform3f(), s2,
41024229
Transform3f(Vec3f(10.1, 0, 0)), dist, closest_p1,
41034230
closest_p2, normal);
@@ -4134,15 +4261,47 @@ BOOST_AUTO_TEST_CASE(conecone) {
41344261
bool res;
41354262
FCL_REAL dist;
41364263

4137-
res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
4138-
closest_p1, closest_p2, normal);
4139-
BOOST_CHECK(dist <= 0);
4140-
BOOST_CHECK(res);
4264+
{
4265+
// The following situations corresponds to the case where the two cylinders
4266+
// are exactly superposed. This is the worst case for EPA which will take
4267+
// forever to converge with default parameters.
4268+
res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
4269+
closest_p1, closest_p2, normal);
4270+
BOOST_CHECK(dist <= 0);
4271+
BOOST_CHECK(!res);
41414272

4142-
res = solver2.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
4143-
closest_p2, normal);
4144-
BOOST_CHECK(dist <= 0);
4145-
BOOST_CHECK(res);
4273+
res = solver1.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
4274+
closest_p2, normal);
4275+
BOOST_CHECK(dist <= 0);
4276+
BOOST_CHECK(!res);
4277+
4278+
// To handle the superposing case, we have to decrease the tolerance of EPA
4279+
// and allow it to work with more vertices and faces.
4280+
size_t epa_max_face_num_backup = solver1.epa_max_face_num;
4281+
size_t epa_max_vertices_num_backup = solver1.epa_max_vertex_num;
4282+
FCL_REAL epa_tolerance_backup = solver1.epa.tolerance;
4283+
size_t epa_max_iterations_backup = solver1.epa.max_iterations;
4284+
solver1.epa_max_face_num = 1000;
4285+
solver1.epa_max_vertex_num = 1000;
4286+
solver1.epa.tolerance = 1e-2;
4287+
solver1.epa.max_iterations = 1000;
4288+
4289+
res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), dist,
4290+
closest_p1, closest_p2, normal);
4291+
BOOST_CHECK(dist <= 0);
4292+
BOOST_CHECK(res);
4293+
4294+
res = solver1.shapeDistance(s1, transform, s2, transform, dist, closest_p1,
4295+
closest_p2, normal);
4296+
BOOST_CHECK(dist <= 0);
4297+
BOOST_CHECK(res);
4298+
4299+
// We restore the original values of the EPA parameters
4300+
solver1.epa_max_face_num = epa_max_face_num_backup;
4301+
solver1.epa_max_vertex_num = epa_max_vertices_num_backup;
4302+
solver1.epa.tolerance = epa_tolerance_backup;
4303+
solver1.epa.max_iterations = epa_max_iterations_backup;
4304+
}
41464305

41474306
res = solver2.shapeDistance(s1, Transform3f(), s2,
41484307
Transform3f(Vec3f(10.1, 0, 0)), dist, closest_p1,

0 commit comments

Comments
 (0)