Skip to content

Commit

Permalink
Propagate the changes related to GJK API change.
Browse files Browse the repository at this point in the history
  • Loading branch information
@jmirabel
jmirabel committed Mar 3, 2020
1 parent c79e908 commit 741f38b
Show file tree
Hide file tree
Showing 3 changed files with 70 additions and 59 deletions.
114 changes: 62 additions & 52 deletions include/hpp/fcl/narrowphase/narrowphase.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,27 +68,36 @@ namespace fcl
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();

switch(gjk_status)
{
Vec3f w0, w1;
switch(gjk_status) {
case details::GJK::Inside:
{
if (gjk.hasPenetrationInformation(shape)) {
gjk.getClosestPoints (shape, w0, w1);
if(penetration_depth) *penetration_depth = gjk.distance;
if(normal) *normal = tf1.getRotation() * (w0 - w1).normalized();
if(contact_points) *contact_points = tf1.transform((w0 + w1) / 2);
return true;
} else {
details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
if(epa_status != details::EPA::Failed)
{
Vec3f w0, w1;
epa.getClosestPoints (shape, w0, w1);
if(penetration_depth) *penetration_depth = -epa.depth;
if(normal) *normal = tf2.getRotation() * epa.normal;
if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
return true;
}
else return false;
{
epa.getClosestPoints (shape, w0, w1);
if(penetration_depth) *penetration_depth = -epa.depth;
// TODO The normal computed by GJK in the s1 frame so this should be
// if(normal) *normal = tf1.getRotation() * epa.normal;
if(normal) *normal = tf2.getRotation() * epa.normal;
if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
return true;
}
// TODO EPA failed but we know there is a collision so we should
// return true;
return false;
}
break;
default:
;
}
}

return false;
}
Expand Down Expand Up @@ -119,37 +128,40 @@ namespace fcl
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();

switch(gjk_status)
{
Vec3f w0, w1;
switch(gjk_status) {
case details::GJK::Inside:
{
col = true;
col = true;
if (gjk.hasPenetrationInformation(shape)) {
gjk.getClosestPoints (shape, w0, w1);
distance = gjk.distance;
normal = tf1.getRotation() * (w1 - w0).normalized();
p1 = p2 = tf1.transform((w0 + w1) / 2);
} else {
details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
assert (epa_status != details::EPA::Failed); (void) epa_status;
Vec3f w0, w1;

epa.getClosestPoints (shape, w0, w1);
distance = -epa.depth;
normal = -epa.normal;
p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
assert (distance <= 1e-6);
break;
}
break;
case details::GJK::Valid:
case details::GJK::Failed:
{
col = false;
col = false;

gjk.getClosestPoints (shape, p1, p2);
// TODO On degenerated case, the closest point may be wrong
// (i.e. an object face normal is colinear to gjk.ray
// assert (distance == (w0 - w1).norm());
distance = gjk.distance;
gjk.getClosestPoints (shape, p1, p2);
// TODO On degenerated case, the closest point may be wrong
// (i.e. an object face normal is colinear to gjk.ray
// assert (distance == (w0 - w1).norm());
distance = gjk.distance;

p1 = tf1.transform (p1);
p2 = tf1.transform (p2);
assert (distance > 0);
}
p1 = tf1.transform (p1);
p2 = tf1.transform (p2);
assert (distance > 0);
break;
default:
assert (false && "should not reach type part.");
Expand All @@ -168,7 +180,6 @@ namespace fcl
#ifndef NDEBUG
FCL_REAL eps (sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
#endif
bool compute_normal (true);
Vec3f guess(1, 0, 0);
if(enable_cached_guess) guess = cached_guess;

Expand Down Expand Up @@ -205,29 +216,28 @@ namespace fcl
else
{
assert (gjk_status == details::GJK::Inside);
if (compute_normal)
{
details::EPA epa(epa_max_face_num, epa_max_vertex_num,
epa_max_iterations, epa_tolerance);
details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
if(epa_status != details::EPA::Failed)
{
Vec3f w0, w1;
epa.getClosestPoints (shape, w0, w1);
assert (epa.depth >= -eps);
distance = std::min (0., -epa.depth);
normal = tf2.getRotation() * epa.normal;
p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
}
}
else
if (gjk.hasPenetrationInformation (shape)) {
gjk.getClosestPoints (shape, p1, p2);
distance = gjk.distance;
p1 = tf1.transform (p1);
p2 = tf1.transform (p2);
normal = (p2 - p1).normalized();
} else {
details::EPA epa(epa_max_face_num, epa_max_vertex_num,
epa_max_iterations, epa_tolerance);
details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
if(epa_status != details::EPA::Failed)
{
gjk.getClosestPoints (shape, p1, p2);
distance = 0;

p1 = tf1.transform (p1);
p2 = tf1.transform (p2);
Vec3f w0, w1;
epa.getClosestPoints (shape, w0, w1);
assert (epa.depth >= -eps);
distance = std::min (0., -epa.depth);
// TODO should be
// normal = tf1.getRotation() * epa.normal;
normal = tf2.getRotation() * epa.normal;
p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
}
}
return false;
}
}
Expand Down
11 changes: 6 additions & 5 deletions test/gjk.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -188,10 +188,12 @@ BOOST_AUTO_TEST_CASE(distance_triangle_triangle_1)
M.col (0) = u1; M.col (1) = v1; M.col (2) = w1;
// Compute a1 such that p1 = P1 + a11 u1 + a12 v1 + a13 u1 x v1
a1 = M.inverse() * (p1 - P1);
EIGEN_VECTOR_IS_APPROX(p1, P1 + a1[0] * u1 + a1[1] * v1, eps);
BOOST_CHECK (w2.squaredNorm () > eps*eps);
// Compute a2 such that p2 = Q1 + a21 u2 + a22 v2 + a23 u2 x v2
M.col (0) = u2; M.col (1) = v2; M.col (2) = w2;
a2 = M.inverse() * (p2 - Q1);
EIGEN_VECTOR_IS_APPROX(p2, Q1 + a2[0] * u2 + a2[1] * v2, eps);

// minimal distance and closest points can be considered as a constrained
// optimisation problem:
Expand Down Expand Up @@ -234,9 +236,9 @@ BOOST_AUTO_TEST_CASE(distance_triangle_triangle_1)
grad_g (2,5) = 1; grad_g (3,5) = 1;
// Check that closest points are on triangles planes
// Projection of [P1p1] on line normal to triangle 1 plane is equal to 0
BOOST_CHECK (fabs (a1 [2]) < eps);
BOOST_CHECK_SMALL (a1 [2], eps);
// Projection of [Q1p2] on line normal to triangle 2 plane is equal to 0
BOOST_CHECK (fabs (a2 [2]) < eps);
BOOST_CHECK_SMALL (a2 [2], eps);

/* Check Karush–Kuhn–Tucker conditions
6
Expand Down Expand Up @@ -268,7 +270,8 @@ BOOST_AUTO_TEST_CASE(distance_triangle_triangle_1)
(Mkkt, Eigen::ComputeThinU | Eigen::ComputeThinV);
vector_t c (svd.solve (-grad_f));
for (vector_t::Index j=0; j < c.size (); ++j) {
BOOST_CHECK (c [j] >= -eps);
BOOST_CHECK_MESSAGE (c [j] >= -eps,
"c[" << j << "]{" << c[j] << "} is below " << -eps);
}
}
}
Expand Down Expand Up @@ -394,8 +397,6 @@ BOOST_AUTO_TEST_CASE(triangle_capsule)

// GJK + EPA -> collision
test_gjk_triangle_capsule(Vec3f(-0.5, -0.01, 0), true,
//Vec3f(1., 0, 0),
//Vec3f(1., 0, 0),
Vec3f(0, 1, 0),
Vec3f(0.5, 0, 0));
}
4 changes: 2 additions & 2 deletions test/utility.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,11 +54,11 @@
#endif

#define EIGEN_VECTOR_IS_APPROX(Va, Vb, precision) \
BOOST_CHECK_MESSAGE((Va).isApprox(Vb, precision), \
BOOST_CHECK_MESSAGE(((Va) - (Vb)).isZero(precision), \
"check " #Va ".isApprox(" #Vb ") failed " \
"[\n" << (Va).transpose() << "\n!=\n" << (Vb).transpose() << "\n]")
#define EIGEN_MATRIX_IS_APPROX(Va, Vb, precision) \
BOOST_CHECK_MESSAGE((Va).isApprox(Vb, precision), \
BOOST_CHECK_MESSAGE(((Va) - (Vb)).isZero(precision), \
"check " #Va ".isApprox(" #Vb ") failed " \
"[\n" << (Va) << "\n!=\n" << (Vb) << "\n]")

Expand Down

0 comments on commit 741f38b

Please sign in to comment.