Commit baa37422 authored by Florent Lamiraux's avatar Florent Lamiraux Committed by Florent Lamiraux florent@laas.fr
Browse files

[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.
parent c63c8f68
......@@ -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
......@@ -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));
......
......@@ -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 ()
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment