From 90bd15ffef42cf31d0c6e48bf2f95179877cec83 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Fri, 30 Aug 2019 15:48:16 +0200
Subject: [PATCH] [GJK] Make closest points more robust.
---
include/hpp/fcl/narrowphase/gjk.h | 2 +-
include/hpp/fcl/narrowphase/narrowphase.h | 29 ++++--------
src/narrowphase/gjk.cpp | 56 ++++++++++++++++++++---
src/narrowphase/narrowphase.cpp | 2 +-
4 files changed, 62 insertions(+), 27 deletions(-)
diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h
index 08904301..ade07eba 100644
--- a/include/hpp/fcl/narrowphase/gjk.h
+++ b/include/hpp/fcl/narrowphase/gjk.h
@@ -169,7 +169,7 @@ struct GJK
/// Get the closest points on each object.
/// \return true on success
- bool getClosestPoints (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1) const;
+ static bool getClosestPoints (const Simplex& simplex, Vec3f& w0, Vec3f& w1);
/// @brief get the guess from current simplex
Vec3f getGuessFromSimplex() const;
diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h
index ffb28f2a..b2df87f8 100644
--- a/include/hpp/fcl/narrowphase/narrowphase.h
+++ b/include/hpp/fcl/narrowphase/narrowphase.h
@@ -77,11 +77,8 @@ namespace fcl
details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
if(epa_status != details::EPA::Failed)
{
- Vec3f w0 (Vec3f::Zero());
- for(size_t i = 0; i < epa.result.rank; ++i)
- {
- w0 += epa.result.vertex[i]->w0 * epa.result.coefficient[i];
- }
+ Vec3f w0, w1;
+ details::GJK::getClosestPoints (epa.result, 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));
@@ -127,11 +124,8 @@ namespace fcl
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 (Vec3f::Zero());
- for(short i = 0; i < epa.result.rank; ++i)
- {
- w0 += epa.result.vertex[i]->w0 * epa.result.coefficient[i];
- }
+ Vec3f w0, w1;
+ details::GJK::getClosestPoints (epa.result, w0, w1);
distance = -epa.depth;
normal = -epa.normal;
p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
@@ -143,7 +137,7 @@ namespace fcl
{
col = false;
- gjk.getClosestPoints (shape, p1, p2);
+ details::GJK::getClosestPoints (*gjk.getSimplex(), 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());
@@ -188,7 +182,7 @@ namespace fcl
// TODO: understand why GJK fails between cylinder and box
assert (distance * distance < sqrt (eps));
Vec3f w0, w1;
- gjk.getClosestPoints (shape, w0, w1);
+ details::GJK::getClosestPoints (*gjk.getSimplex(), w0, w1);
distance = 0;
p1 = p2 = tf1.transform (.5* (w0 + w1));
normal = Vec3f (0,0,0);
@@ -196,7 +190,7 @@ namespace fcl
}
else if(gjk_status == details::GJK::Valid)
{
- gjk.getClosestPoints (shape, p1, p2);
+ details::GJK::getClosestPoints (*gjk.getSimplex(), 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());
@@ -216,11 +210,8 @@ namespace fcl
details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
if(epa_status != details::EPA::Failed)
{
- Vec3f w0 (Vec3f::Zero());
- for(short i = 0; i < epa.result.rank; ++i)
- {
- w0 += epa.result.vertex[i]->w0 * epa.result.coefficient[i];
- }
+ Vec3f w0, w1;
+ details::GJK::getClosestPoints (epa.result, w0, w1);
assert (epa.depth >= -eps);
distance = std::min (0., -epa.depth);
normal = tf2.getRotation() * epa.normal;
@@ -229,7 +220,7 @@ namespace fcl
}
else
{
- gjk.getClosestPoints (shape, p1, p2);
+ details::GJK::getClosestPoints (*gjk.getSimplex(), p1, p2);
distance = 0;
p1 = tf1.transform (p1);
diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp
index d1b4cade..adc5b521 100644
--- a/src/narrowphase/gjk.cpp
+++ b/src/narrowphase/gjk.cpp
@@ -308,15 +308,59 @@ Vec3f GJK::getGuessFromSimplex() const
return ray;
}
-bool GJK::getClosestPoints (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1) const
+bool GJK::getClosestPoints (const Simplex& simplex, Vec3f& w0, Vec3f& w1)
{
+ SimplexV* const* vs = simplex.vertex;
+
+ for (short i = 0; i < simplex.rank; ++i) {
+ assert (vs[i]->w.isApprox (vs[i]->w0 - vs[i]->w1));
+ }
+
+ Project::ProjectResult projection;
+ switch (simplex.rank) {
+ case 1:
+ w0 = vs[0]->w0;
+ w1 = vs[0]->w1;
+ return true;
+ case 2:
+ {
+ const Vec3f& a = vs[0]->w, a0 = vs[0]->w0, a1 = vs[0]->w1,
+ b = vs[1]->w, b0 = vs[1]->w0, b1 = vs[1]->w1;
+ FCL_REAL la, lb;
+ Vec3f N (b - a);
+ la = N.dot(-a);
+ if (la <= 0) {
+ w0 = a0;
+ w1 = a1;
+ } else {
+ lb = N.squaredNorm();
+ if (la > lb) {
+ w0 = b0;
+ w1 = b1;
+ } else {
+ lb = la / lb;
+ la = 1 - lb;
+ w0 = la * a0 + lb * b0;
+ w1 = la * a1 + lb * b1;
+ }
+ }
+ }
+ return true;
+ case 3:
+ // TODO avoid the reprojection
+ projection = Project::projectTriangleOrigin (vs[0]->w, vs[1]->w, vs[2]->w);
+ break;
+ case 4: // We are in collision.
+ projection = Project::projectTetrahedraOrigin (vs[0]->w, vs[1]->w, vs[2]->w, vs[3]->w);
+ break;
+ default:
+ throw std::logic_error ("The simplex rank must be in [ 1, 4 ]");
+ }
w0.setZero();
w1.setZero();
- for(short i = 0; i < getSimplex()->rank; ++i)
- {
- FCL_REAL p = getSimplex()->coefficient[i];
- w0 += getSimplex()->vertex[i]->w0 * p;
- w1 += getSimplex()->vertex[i]->w1 * p;
+ for (short i = 0; i < simplex.rank; ++i) {
+ w0 += projection.parameterization[i] * vs[i]->w0;
+ w1 += projection.parameterization[i] * vs[i]->w1;
}
return true;
}
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index 7ab2035c..943c4dd7 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -644,7 +644,7 @@ bool GJKSolver_indep::shapeDistance<Capsule, Capsule>
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
- gjk.getClosestPoints (shape, p1, p2);
+ details::GJK::getClosestPoints (*gjk.getSimplex(), p1, p2);
if((gjk_status == details::GJK::Valid) ||
(gjk_status == details::GJK::Failed))
--
GitLab