MattiaMontanari / openGJK

Fast and reliable implementation of the Gilbert-Johnson-Keerthi (GJK) algorithm for C, C#, Go, Matlab and Python
https://www.mattiamontanari.com/opengjk/
GNU General Public License v3.0
135 stars 37 forks source link

Is this a numerical issue? #3

Closed ruiqini closed 4 years ago

ruiqini commented 4 years ago

Thanks for sharing code. I tried some examples but found a issue. Please test these 2 polygons A: 0.795121 -0.727851 0.0 -0.178424 -0.989183 0.0 -0.412644 -0.770664 0.0 0.566564 0.548772 0.0 B: -0.211223 -0.511346 0.0 -0.347973 0.45872 0.0 0.277308 0.969689 0.0 I change the data in example1_c, and output is Distance : 0.508965 Here is the matlab plot of these polygons. However there is a intersection. Could you tell me if this is a numerical issue? Thank you for your time. untitled

MattiaMontanari commented 4 years ago

Thanks for reporting this bug. Yes, it's a numerical issue that the current version cannot handle automatically but I'll soon publish another one which does. I can only provide a quick fix for now..

The proper way to handle these cases is to provide a better initial guess to the GJK. Basically you have to change the lines below to set a "better" initial simplex and initial search direction. By "better" a mean closer to the solution - if that's not clear enough let me know.

https://github.com/MattiaMontanari/openGJK/blob/15cd434e4b8713a8d9535813efb916006b8a9bc9/lib/src/openGJK.c#L774-L782

A really dirty fix that will do for this particular case is the following:

  /* Initialise  */
  s->nvrtx = 1;
  for ( i = 0; i < 3; i++)
  {
    v[i] = bd1.coord[1][i] - bd2.coord[0][i];
    bd1.s[i] = bd1.coord[0][i];
    bd2.s[i] = bd2.coord[0][i];
    s->vrtx[0][i] = v[i];
  }

By changing line 778 you can initialise the serch differently. This it will work for this particular case, but not for all cases (and assumes body 1 has at least 2 points). If I run this I get the following output:

image

ruiqini commented 4 years ago

Thanks for your response. Maybe it is good to use barycentric coordinate to justify if the origin is in the simplex?

MattiaMontanari commented 4 years ago

Thanks for your response. Maybe it is good to use barycentric coordinate to justify if the origin is in the simplex?

Yes, that's the idea but cancellation error makes the check "if the origin is in the simplex" difficult to handle in cases like yours.

MattiaMontanari commented 4 years ago

... it was actually a much simpler bug that I thuogh. Nothing to do with numerical issues! This is fixed now, thanks for reporting!

ruiqini commented 4 years ago

Thanks for your help!