turtlebasket
/
openGJK
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Bugfix merge

fixes-turtlebasket
Mattia Montanari 2023-05-23 21:57:52 +02:00
parent 47df2d76ea 5b6a2c456a
commit 4b2d30e1ca
1 changed files with 12 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
openGJK.c
View File

@ -635,20 +635,17 @@ subalgorithm(gkSimplex* s, gkFloat* v) {
gkFloat gkFloat
compute_minimum_distance(gkPolytope bd1, gkPolytope bd2, gkSimplex* restrict s) { compute_minimum_distance(gkPolytope bd1, gkPolytope bd2, gkSimplex* restrict s) {
unsigned int k = 0; /**< Iteration counter */ unsigned int k = 0; /**< Iteration counter */
unsigned int i; /**< General purpose counter */ const int mk = 25; /**< Maximum number of GJK iterations */
const int mk = 25; /**< Maximum number of iterations of the GJK algorithm */ const gkFloat eps_rel = eps_rel22; /**< Tolerance on relative */
gkFloat v[3]; /**< Search direction */ const gkFloat eps_tot = eps_tot22; /**< Tolerance on absolute distance */
gkFloat vminus[3]; /**< Search direction * -1 */
gkFloat w[3]; /**< Vertex on CSO boundary given by the difference of support
functions on both bodies */
const gkFloat eps_rel = eps_rel22; /**< Tolerance on relative */
const gkFloat eps_rel2 = eps_rel * eps_rel; const gkFloat eps_rel2 = eps_rel * eps_rel;
const gkFloat eps_tot = eps_tot22; unsigned int i;
gkFloat exeedtol_rel; /**< Test for 1st exit condition */ gkFloat w[3];
int absTestin; gkFloat v[3];
gkFloat vminus[3];
gkFloat norm2Wmax = 0; gkFloat norm2Wmax = 0;
gkFloat tesnorm;
/* Initialise search direction */ /* Initialise search direction */
v[0] = bd1.coord[0][0] - bd2.coord[0][0]; v[0] = bd1.coord[0][0] - bd2.coord[0][0];
@ -687,7 +684,7 @@ compute_minimum_distance(gkPolytope bd1, gkPolytope bd2, gkSimplex* restrict s)
/* Test first exit condition (new point already in simplex/can't move /* Test first exit condition (new point already in simplex/can't move
* further) */ * further) */
exeedtol_rel = (norm2(v) - dotProduct(v, w)); gkFloat exeedtol_rel = (norm2(v) - dotProduct(v, w));
if (exeedtol_rel <= (eps_rel * norm2(v)) || exeedtol_rel < eps_tot22) { if (exeedtol_rel <= (eps_rel * norm2(v)) || exeedtol_rel < eps_tot22) {
break; break;
} }
@ -708,14 +705,13 @@ compute_minimum_distance(gkPolytope bd1, gkPolytope bd2, gkSimplex* restrict s)
/* Test */ /* Test */
for (int jj = 0; jj < s->nvrtx; jj++) { for (int jj = 0; jj < s->nvrtx; jj++) {
tesnorm = norm2(s->vrtx[jj]); gkFloat tesnorm = norm2(s->vrtx[jj]);
if (tesnorm > norm2Wmax) { if (tesnorm > norm2Wmax) {
norm2Wmax = tesnorm; norm2Wmax = tesnorm;
} }
} }
absTestin = (norm2(v) <= (eps_tot * eps_tot * norm2Wmax)); if ((norm2(v) <= (eps_tot * eps_tot * norm2Wmax))) {
if (absTestin) {
break; break;
} }