diff --git a/example2_mex/runme.m b/example2_mex/runme.m index d5fcfe1..7a3a6ef 100644 --- a/example2_mex/runme.m +++ b/example2_mex/runme.m @@ -34,10 +34,10 @@ end % TRY COMPILING MEX FILE fprintf('Compiling mex function... ') try -mex('../lib/src/openGJK.c',... % Source of openGJK +mex(fullfile('..','lib','src','openGJK.c'),... % Source of openGJK '-largeArrayDims', ... % Support large arrays optflug, ... % Compiler flag for debug/optimisation - '-I../lib/include',... % Folder to header files + fullfile('-I..','lib','include'),... % Folder to header files '-outdir', pwd,... % Ouput directory for writing mex function '-output', 'openGJK',... % Name of ouput mex file '-DMATLABDOESMEXSTUFF',... % Define variable for mex function in source files @@ -50,6 +50,7 @@ mex('../lib/src/openGJK.c',... % Source of openGJK catch % Build failed, refer to documentation fprintf('\n\n ERROR DETECTED! Mex file cannot be compiled.\n') + fprintf('\tThoubleshooting: chance your current folder to ..\openGJK\1_src\example2_mex') fprintf('\tFor more information, see ') fprintf('this documentation page.\n\n') return diff --git a/lib/src/openGJK.c b/lib/src/openGJK.c index bbcca75..9d57841 100644 --- a/lib/src/openGJK.c +++ b/lib/src/openGJK.c @@ -141,10 +141,7 @@ void S1D( struct simplex * s) { FacetsTest[ 0 ] = SAMESIGN( nu_test [ indexI ] , det_ap ); FacetsTest[ 1 ] = SAMESIGN( nu_test [ indexI ] , det_pb ); - /* Compare signed lengths and compute barycentric coordinates */ - if ( FacetsTest[ 0 ] + FacetsTest[ 1 ] == 2){ - /* The origin projection P is between A and B */ s->lambdas[0] = det_ap * inv_detM; s->lambdas[1] = 1 - s->lambdas[0]; s->wids[0] = 0; @@ -152,20 +149,15 @@ void S1D( struct simplex * s) { s->nvrtx = 2; } else if ( FacetsTest[ 0 ] == 0 ) { - - /* The origin project P is beyond A */ s->lambdas[0] = 1; s->wids[0] = 0; s->nvrtx = 1; - /* fold back the values stored for the indices into the original point arrays, and the transformed - coordinates, so that these are ready for subsequent calls. */ for ( i = 0; i < 3; ++i) { s->vrtx[0][i] = s->vrtx[1][i]; } } else { - /* RARE The origin project P is behind B */ s->lambdas[0] = 1; s->wids[0] = 1; s->nvrtx = 1; @@ -197,20 +189,20 @@ static void S2D( struct simplex * s) { sb[3-1], sc[3-1]; - double nu_max = 0, + double nu_max = 0, inv_detM = 0, nnorm_sqrd = 0, nnnorm = 0, dotNA; - int i = 0, + int i = 0, FacetsTest[3], k, l, j; - int indexI = 1, - indexJ[2] = {-1}, + int indexI = 1, + indexJ[2] = {0, 2}, stemp[3]; for ( i=0 ; i<3; ++i ) { @@ -221,7 +213,6 @@ static void S2D( struct simplex * s) { s31[i] = c[i] - a[i]; } - /* Find best axis for projection */ k = 1; l = 2; for (i = 0; i < 3 ; ++i) { nu_test[i] = pow(-1.0,i) * (b[k]*c[l] + a[k]*b[l] + c[k]*a[l] - b[k]*a[l] - c[k]*b[l] - a[k]*c[l]); @@ -264,8 +255,6 @@ static void S2D( struct simplex * s) { nu_max = nu_test[ indexI ]; - /* Project origin onto the 2D simplex - plane */ - /* ... compute the normal vector*/ k = 1; l = 2; for ( i = 0; i < 3; ++i ) { n[i] = s21[k] * s31[l] - s21[l] * s31[k]; @@ -278,8 +267,6 @@ static void S2D( struct simplex * s) { } dotNA = dotprod(n,a); - - /* ... project but discard the components of indexI */ pp[0] = dotNA * n[ indexJ[0] ]; pp[1] = dotNA * n[ indexJ[1] ]; @@ -317,9 +304,6 @@ static void S2D( struct simplex * s) { /* Test if sign of ABC is equal to the signes of the auxiliary simplices */ for ( int m = 0; m < 3 ; ++m) { - /* i = 0 -> OBC - most likely to be 1. - * i = 1 -> AOC - * i = 2 -> ABO */ FacetsTest[ m ] = SAMESIGN( nu_max , B[ m ] ); } @@ -339,7 +323,6 @@ static void S2D( struct simplex * s) { } S1D(&sTmp); - /* ... test segment AC */ S1D(s); for (j = 0; j < 3; ++j) { @@ -352,7 +335,6 @@ static void S2D( struct simplex * s) { } if( dotprod(v,v) < dotprod(vtmp,vtmp) ){ - /* Keep simplex. Need to update sID only*/ for (i = 1; i < s->nvrtx ; ++i) { s->wids[i] = s->wids[i] + 1; } @@ -370,7 +352,6 @@ static void S2D( struct simplex * s) { } } else if ( (FacetsTest[0] + FacetsTest[1] + FacetsTest[2] ) == 3 ) { - /* Compare signed lengths and compute barycentric coordinates */ /* The origin projections lays onto the triangle */ inv_detM = 1 / nu_max; s->lambdas[0] = B[2] * inv_detM; @@ -402,15 +383,11 @@ static void S2D( struct simplex * s) { for (i = 1; i < s->nvrtx ; ++i) { s->wids[i] = s->wids[i] + 1; } - } else { - /* RARE - The origin projection P faces the segment BC */ s->nvrtx = 2; S1D(s); - } - } @@ -499,7 +476,7 @@ static void S3D( struct simplex * s) { { for (i = 0; i < 4; i++) { - FacetsTest[i] = 0; /* Any other case. Test ABC, ABD, ACD */ + FacetsTest[i] = 0; } } @@ -542,9 +519,7 @@ static void S3D( struct simplex * s) { sTmp.vrtx[2-k][j] = s->vrtx[ vtxid ][j]; } } - /* ... and call S2D itself */ S2D(&sTmp); - /* ... compute aux squared distance */ for (j = 0; j < 3; ++j) { vtmp[j] = 0; for (l = 0; l < sTmp.nvrtx ; ++l) { @@ -600,9 +575,7 @@ static void S3D( struct simplex * s) { sTmp.vrtx[1][i] = s->vrtx[ 1 ][i]; sTmp.vrtx[2][i] = s->vrtx[ 3 ][i]; } - /* ... and call S2D itself */ S2D(&sTmp); - /* ... compute aux squared distance */ for (j = 0; j < 3; ++j) { vtmp[j] = 0; for (i = 0; i < sTmp.nvrtx ; ++i) { @@ -614,7 +587,6 @@ static void S3D( struct simplex * s) { firstaux = 0; } if ( FacetsTest[2] == 0 ){ - /* ... Test facet ABD */ if ( Flag_sAuxused == 0 ){ for ( i = 0; i < 3; ++i) { @@ -622,9 +594,7 @@ static void S3D( struct simplex * s) { sTmp.vrtx[1][i] = s->vrtx[ 2 ][i]; sTmp.vrtx[2][i] = s->vrtx[ 3 ][i]; } - /* ... and call S2D itself */ S2D(&sTmp); - /* ... compute aux squared distance */ for (j = 0; j < 3; ++j) { vtmp[j] = 0; for (i = 0; i < sTmp.nvrtx ; ++i) { @@ -660,7 +630,6 @@ static void S3D( struct simplex * s) { secondaux = 2; } /* Do a loop and compare current outcomes */ - /* ... computer aux squared distance */ for (j = 0; j < 3; ++j) { v[j] = 0; for (i = 0; i < s->nvrtx ; ++i) { @@ -670,7 +639,6 @@ static void S3D( struct simplex * s) { if( dotprod(v,v) < sqdist_tmp){ /* Keep simplex. Need to update sID only*/ for (i = 0; i < s->nvrtx ; ++i) { - /* Assume that vertex a is always included in sID. */ s->wids[ s->nvrtx - 1 - i] = TrianglesToTest[ secondaux +( s->wids[i] *3) ] ; } } @@ -691,8 +659,6 @@ static void S3D( struct simplex * s) { else if ( FacetsTest[1] + FacetsTest[2] + FacetsTest[3] == 2 ){ /* Only one facet is facing the origin */ if ( FacetsTest[1] == 0 ){ - /* The origin projection P faces the facet ACD */ - /* ... thus, remove the vertex B from the simplex. */ s->nvrtx = 3; for ( i = 0; i < 3; ++i) { s->vrtx[0][i] = s->vrtx[ 0 ][i]; @@ -700,8 +666,6 @@ static void S3D( struct simplex * s) { s->vrtx[2][i] = s->vrtx[ 3 ][i]; } S2D(s); - - /* Keep simplex. Need to update sID only*/ for (i = 0; i < s->nvrtx ; ++i) { s->wids[i] = s->wids[i]; } @@ -737,16 +701,13 @@ static void S3D( struct simplex * s) { } } else { - /* * RARE * - The origin projection P faces the facet BCD */ s->nvrtx = 3; for ( i = 0; i < 3; ++i) { s->vrtx[0][i] = s->vrtx[ 0 ][i]; s->vrtx[1][i] = s->vrtx[ 1 ][i]; s->vrtx[2][i] = s->vrtx[ 2 ][i]; } - /* ... and call S2D itself */ S2D(s); - /* Keep simplex. Need to update sID only*/ for (i = 0; i < s->nvrtx ; ++i) { /* Assume that vertex a is always included in sID. */ s->wids[i] = s->wids[i] + 1; @@ -840,7 +801,7 @@ double gjk ( struct bd bd1, struct bd bd2, struct simplex *s) { } -#if 1 +#if DEBUG mexPrintf ("Num points A = %i \n",bd1.numpoints ); mexPrintf ("Num points B = %i \n",bd2.numpoints ); for ( i = 0; i < bd1.numpoints; ++i) { @@ -944,17 +905,10 @@ void mexFunction( int nlhs, mxArray *plhs[], /**************** PARSE INPUTS AND OUTPUTS **********************/ /*----------------------------------------------------------------*/ /* Examine input (right-hand-side) arguments. */ - mexPrintf("\nThere are %d input arguments.", nrhs); - for (i=0; i