From baa374228647ff52c8be68be77cdf8bd46227b04 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Fri, 11 Jan 2019 21:01:12 +0100
Subject: [PATCH] [GJK] If gjk fails for two triangles, return no collision.
It seems that GJK fails when the triangles are close to each other but not
colliding,
Add a test in test_fcl_gjk with specific triangles and transforms that make
GJK fail.
---
src/narrowphase/narrowphase.cpp | 29 ++++++-------------
test/test_fcl_gjk.cpp | 49 ++++++++++++++++++++++++++-------
test/test_fcl_gjk.py | 32 ++++++---------------
3 files changed, 57 insertions(+), 53 deletions(-)
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index 7eb28b72..9d00eafe 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -636,18 +636,17 @@ bool GJKSolver_indep::shapeDistance<Capsule, Capsule>
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
- if(gjk_status == details::GJK::Valid)
+ Vec3f w0 (Vec3f::Zero()), w1 (Vec3f::Zero());
+ for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
+ {
+ FCL_REAL p = gjk.getSimplex()->coefficient[i];
+ w0 += shape.support(gjk.getSimplex()->vertex[i]->d, 0) * p;
+ w1 += shape.support(-gjk.getSimplex()->vertex[i]->d, 1) * p;
+ }
+ if((gjk_status == details::GJK::Valid) ||
+ (gjk_status == details::GJK::Failed))
{
- Vec3f w0 (Vec3f::Zero()), w1 (Vec3f::Zero());
- for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
- {
- FCL_REAL p = gjk.getSimplex()->coefficient[i];
- w0 += shape.support(gjk.getSimplex()->vertex[i]->d, 0) * p;
- w1 += shape.support(-gjk.getSimplex()->vertex[i]->d, 1) * p;
- }
-
dist = (w0 - w1).norm();
-
p1 = tf1.transform (w0);
p2 = tf1.transform (w1);
@@ -655,15 +654,7 @@ bool GJKSolver_indep::shapeDistance<Capsule, Capsule>
}
else if (gjk_status == details::GJK::Inside)
{
- Vec3f w0 (Vec3f::Zero()), w1 (Vec3f::Zero());
- for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
- {
- FCL_REAL p = gjk.getSimplex()->coefficient[i];
- w0 += shape.support(gjk.getSimplex()->vertex[i]->d, 0) * p;
- w1 += shape.support(-gjk.getSimplex()->vertex[i]->d, 1) * p;
- }
dist = 0;
-
p1 = tf1.transform (w0);
p2 = tf1.transform (w1);
@@ -675,7 +666,5 @@ bool GJKSolver_indep::shapeDistance<Capsule, Capsule>
}
return false;
}
- abort ();
- return false;
}
} // fcl
diff --git a/test/test_fcl_gjk.cpp b/test/test_fcl_gjk.cpp
index e65b9a44..59d8c0fc 100644
--- a/test/test_fcl_gjk.cpp
+++ b/test/test_fcl_gjk.cpp
@@ -47,6 +47,7 @@
using fcl::GJKSolver_indep;
using fcl::TriangleP;
using fcl::Vec3f;
+using fcl::Quaternion3f;
using fcl::Transform3f;
using fcl::Matrix3f;
using fcl::FCL_REAL;
@@ -83,10 +84,32 @@ BOOST_AUTO_TEST_CASE(distance_triangle_triangle_1)
FCL_REAL eps = 1e-7;
Results_t results (N);
for (std::size_t i=0; i<N; ++i) {
- Vec3f P1 (Vec3f::Random ()), P2 (Vec3f::Random ()), P3 (Vec3f::Random ());
- Vec3f Q1 (Vec3f::Random ()), Q2 (Vec3f::Random ()), Q3 (Vec3f::Random ());
- TriangleP tri1 (P1, P2, P3);
- TriangleP tri2 (Q1, Q2, Q3);
+ Vec3f P1_loc (Vec3f::Random ()), P2_loc (Vec3f::Random ()),
+ P3_loc (Vec3f::Random ());
+ Vec3f Q1_loc (Vec3f::Random ()), Q2_loc (Vec3f::Random ()),
+ Q3_loc (Vec3f::Random ());
+ if (i==0) {
+ P1_loc = Vec3f (0.063996093749999997, -0.15320971679687501,
+ -0.42799999999999999);
+ P2_loc = Vec3f (0.069105957031249998, -0.150722900390625,
+ -0.42999999999999999);
+ P3_loc = Vec3f (0.063996093749999997, -0.15320971679687501,
+ -0.42999999999999999);
+ Q1_loc = Vec3f (-25.655000000000001, -1.2858199462890625,
+ 3.7249809570312502);
+ Q2_loc = Vec3f (-10.926, -1.284259033203125, 3.7281499023437501);
+ Q3_loc = Vec3f (-10.926, -1.2866180419921875, 3.72335400390625);
+ Transform3f tf
+ (Quaternion3f (-0.42437287410898855, -0.26862477561450587,
+ -0.46249645019513175, 0.73064726592483387),
+ Vec3f (-12.824601270753471, -1.6840516940066426, 3.8914453043793844));
+ tf1 = tf;
+ } else {
+ tf1 = Transform3f ();
+ }
+
+ TriangleP tri1 (P1_loc, P2_loc, P3_loc);
+ TriangleP tri2 (Q1_loc, Q2_loc, Q3_loc);
Vec3f normal;
bool res;
@@ -107,14 +130,17 @@ BOOST_AUTO_TEST_CASE(distance_triangle_triangle_1)
res = solver.shapeDistance (tri1, tf1, tri2, tf2, distance, c1, c2,
normal2);
if (!res) {
- std::cerr << "P1 = " << P1.format (tuple) << std::endl;
- std::cerr << "P2 = " << P2.format (tuple) << std::endl;
- std::cerr << "P3 = " << P3.format (tuple) << std::endl;
- std::cerr << "Q1 = " << Q1.format (tuple) << std::endl;
- std::cerr << "Q2 = " << Q2.format (tuple) << std::endl;
- std::cerr << "Q3 = " << Q3.format (tuple) << std::endl;
+ std::cerr << "P1 = " << P1_loc.format (tuple) << std::endl;
+ std::cerr << "P2 = " << P2_loc.format (tuple) << std::endl;
+ std::cerr << "P3 = " << P3_loc.format (tuple) << std::endl;
+ std::cerr << "Q1 = " << Q1_loc.format (tuple) << std::endl;
+ std::cerr << "Q2 = " << Q2_loc.format (tuple) << std::endl;
+ std::cerr << "Q3 = " << Q3_loc.format (tuple) << std::endl;
std::cerr << "p1 = " << c1.format (tuple) << std::endl;
std::cerr << "p2 = " << c2.format (tuple) << std::endl;
+ std::cerr << "tf1 = " << tf1.getTranslation ().format (tuple)
+ << " + " << tf1.getQuatRotation ().coeffs ().format (tuple)
+ << std::endl;
std::cerr << "tf2 = " << tf2.getTranslation ().format (tuple)
<< " + " << tf2.getQuatRotation ().coeffs ().format (tuple)
<< std::endl;
@@ -125,6 +151,9 @@ BOOST_AUTO_TEST_CASE(distance_triangle_triangle_1)
tf2.setIdentity ();
}
// Compute vectors between vertices
+ Vec3f P1 (tf1.transform (P1_loc)), P2 (tf1.transform (P2_loc)),
+ P3 (tf1.transform (P3_loc)), Q1 (tf2.transform (Q1_loc)),
+ Q2 (tf2.transform (Q2_loc)), Q3 (tf2.transform (Q3_loc));
Vec3f u1 (P2 - P1);
Vec3f v1 (P3 - P1);
Vec3f w1 (u1.cross (v1));
diff --git a/test/test_fcl_gjk.py b/test/test_fcl_gjk.py
index a8e96435..b92dac60 100644
--- a/test/test_fcl_gjk.py
+++ b/test/test_fcl_gjk.py
@@ -20,35 +20,21 @@ c.gui.createScene (sceneName)
c.gui.addSceneToWindow (sceneName, wid)
P1 = (-0.6475786872429674, -0.519875255189778, 0.5955961037406681)
-P2 = (0.4653088233737781, 0.313127305970121, 0.934810277044219)
-P3 = (0.2789166910941325, 0.5194696837661181, -0.8130390456938367)
-Q1 = (-0.7301951766620367, 0.04042013969291935, -0.8435357165725602)
-Q2 = (-0.8601872044895716, -0.5906898274974384, -0.07715905321629679)
-Q3 = (0.6393545603562867, 0.1466372567911807, 0.5111616707924576)
-p1 = (0.2238377827027012, 0.2163758468474661, 0.4505435657492071)
-p2 = (0.2238377827027012, 0.2163758468474662, 0.450543565749207)
-tf2 = (-0.1588705771755817, 0.1959319373363798, 0.04007284069698749) + (0, 0, 0, 1)
-normal = (-0.6220181878092853, 0.7671227156255918, 0.1568952300284174)
+P2 = (0.069105957031249998, -0.150722900390625, -0.42999999999999999)
+P3 = (0.063996093749999997, -0.15320971679687501, -0.42999999999999999)
+Q1 = (-25.655000000000001, -1.2858199462890625, 3.7249809570312502)
+Q2 = (-10.926, -1.284259033203125, 3.7281499023437501)
+Q3 = (-10.926, -1.2866180419921875, 3.72335400390625)
+tf1 = (-12.824601270753471, -1.6840516940066426, 3.8914453043793844,
+ -0.26862477561450587, -0.46249645019513175, 0.73064726592483387,
+ -0.42437287410898855)
+tf2 = (0, 0, 0, 0, 0, 0, 1)
c.gui.addTriangleFace ("triangle1", P1, P2, P3, Red)
c.gui.addTriangleFace ("triangle2", Q1, Q2, Q3, Green)
c.gui.addToGroup ('triangle1', sceneName)
c.gui.addToGroup ('triangle2', sceneName)
-c.gui.addSphere ('closest point 1', .02, Red)
-c.gui.addSphere ('closest point 2', .02, Green)
-c.gui.addToGroup ('closest point 1', sceneName)
-c.gui.addToGroup ('closest point 2', sceneName)
-
-c.gui.addLine ('normal', P1, tuple (np.array (P1) + np.array (normal)), Red)
-c.gui.addToGroup ('normal', sceneName)
-
-q1 = p1 + (1,0,0,0)
-q2 = p2 + (1,0,0,0)
-tf1 = (0, 0, 0, 0, 0, 0, 1)
-
-c.gui.applyConfiguration ('closest point 1', q1)
-c.gui.applyConfiguration ('closest point 2', q2)
c.gui.applyConfiguration ('triangle1', tf1)
c.gui.applyConfiguration ('triangle2', tf2)
c.gui.refresh ()
--
GitLab