Fix to major bugs in sub-algorithm and exit conditions, and support function improved

example1_c/main.c

@@ -7,7 +7,7 @@
 *       #    # #      #      #   ## #     # #     # #   #                 *
 *        ####  #      ###### #    #  #####   #####  #    #                *
 *                                                                         *
-*           Mattia Montanari    |   University of Oxford 2018             *
+*           Mattia Montanari    |   University of Oxford 2019             *
 * - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
 *                                                                         *
 * This file runs an example to illustrate how to invoke the openGJK lib   *

example2_mex/runme.m

@@ -7,7 +7,7 @@
 %       #    # #      #      #   ## #     # #     # #   #                 %
 %        ####  #      ###### #    #  #####   #####  #    #                %
 %                                                                         %
-%           Mattia Montanari    |   University of Oxford 2018             %
+%           Mattia Montanari    |   University of Oxford 2019             %
 % - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - %
 %                                                                         %
 % This file compiles a mex function from the openGJK library and runs an  %

lib/CMakeLists.txt

@@ -7,7 +7,7 @@
 #       #    # #      #      #   ## #     # #     # #   #                 #
 #        ####  #      ###### #    #  #####   #####  #    #                #
 #                                                                         #
-#           Mattia Montanari    |   University of Oxford 2018             #
+#           Mattia Montanari    |   University of Oxford 2019             #
 # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - #
 
 

lib/include/openGJK/openGJK.h

@@ -7,7 +7,7 @@
 *       #    # #      #      #   ## #     # #     # #   #                 *
 *        ####  #      ###### #    #  #####   #####  #    #                *
 *                                                                         *
-*           Mattia Montanari    |   University of Oxford 2018             *
+*           Mattia Montanari    |   University of Oxford 2019             *
 * - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
 *                                                                         *
 * This is the header file for the openGJK.c file. It defines the openGJK  *

lib/src/openGJK.c

@@ -7,7 +7,8 @@
 *       #    # #      #      #   ## #     # #     # #   #                 *
 *        ####  #      ###### #    #  #####   #####  #    #                *
 *                                                                         *
-*           Mattia Montanari    |   University of Oxford 2018             *
+*                                                                         *
+*           Mattia Montanari    |   University of Oxford 2019             *
 * - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
 *                                                                         *
 * This file implements the GJK algorithm and the Signed Volumes method as *
@@ -28,15 +29,14 @@
 
 #define _CRT_HAS_CXX17 0
 #include <stdio.h>
-
 #include "openGJK/openGJK.h"
 
-/* If defined, uses an advanced method to compute the determinant of matrices. */
+/* If instricuted, uses adaptive floating point arithmetics. */
 #ifdef ADAPTIVEFP
 #include "predicates.h"
 #endif
 
-/* Used for creating a mex function for Matlab.  */
+/* If instricuted, compile a mex function for Matlab.  */
 #ifdef MATLABDOESMEXSTUFF
   #include "mex.h" 
 #else
@@ -47,80 +47,54 @@
   #define _isnan(x) isnan(x)
 #endif
 
+#define dotProduct(a, b) (a[0]*b[0]+a[1]*b[1]+a[2]*b[2])
 
 /**
- * @brief Computes the norm of a vector.
+ * @brief Returns squared vector norm
  */
 double norm2(double *v) {
-  double n2;
+  double n2 = 0;
-  n2 = 0;
+  for (int i = 0; i < 3; ++i) 
-  for (int i = 0; i < 3; ++i) {
     n2 += v[i] * v[i];
-  }
   return n2;
 }
 
 /**
- * @brief Computes the dot product between two vectors.
+ * @brief Finds point of minimum norm of 1-simplex. Robust, but slower, version of algorithm presented in paper.
  */
-inline double dotprod(double *a, double *b) {
+inline static void S1D(struct simplex * s, double *vv) {
-  double dot;
+  int    i = 0;
-  dot = 0;
+  int    indexI = 1;
-  for (int i = 0; i < 3; ++i) {
+  int    FacetsTest[2];  
-    dot += a[i] * b[i];
+  double det_ap = 0;
-  }
+  double det_pb = 0;
-  return dot;
+  double pt = 0;
-}
+  double leng = 0;
-
+  double a[3];
-/**
+  double b[3];
-* @brief Function invoked by the Signed Volumes sub-algorithm for 1-simplex.
+  double t[3];
-*/
+  double nu_fabs[3];
-void S1D( struct simplex * s) {
-
-  double    a[3],
-    b[3],
-    t[3],
-    nu_test[3],
-    nu_fabs[3];
-
-  double    inv_detM = 0,
-    det_ap = 0,
-    det_pb = 0,
-    pt = 0;
-
-  int     i = 0,
-    indexI = 1,
-    FacetsTest[ 2 ];
 
   for (i = 0; i < 3; ++i) {
     b[i] = s->vrtx[0][i];
     a[i] = s->vrtx[1][i];
     t[i] = b[i] - a[i];
-  }
+    leng += t[i];
-
+    nu_fabs[i] = fabs(t[i]);
-  for ( i = 0; i < 3; ++i) {
-    nu_test[i] = a[i] - b[i];
-  }
-  for ( i = 0; i < 3; ++i) {
-    nu_fabs[i] = fabs( nu_test[i] );
   }
   
   if (nu_fabs[0] > nu_fabs[1]) {
-    if (nu_fabs[0] > nu_fabs[2]){
+    if (nu_fabs[0] > nu_fabs[2])
       indexI = 0;
-    }
+    else 
-    else {
       indexI = 2;    
   }
-  }
   else if (nu_fabs[0] < nu_fabs[1]) {
-    if (nu_fabs[1] > nu_fabs[2]){
+    if (nu_fabs[1] > nu_fabs[2])
       indexI = 1;
-    }
+    else
-    else {
       indexI = 2;
   }
-  }
   else if (nu_fabs[0] < nu_fabs[2]) {
     indexI = 2;
   }
@@ -128,83 +102,82 @@ void S1D( struct simplex * s) {
     indexI = 2;
   }
 
-  /* Compute inverse of detM */
-  inv_detM = 1 / nu_test [ indexI ];
-
   /* Project origin onto the 1D simplex - line */
-  pt = dotprod (b,t) / (dotprod(t,t)) * ( a[indexI] - b[indexI] ) + b[indexI];
+  pt = dotProduct(b, t) / (dotProduct(t, t)) * (a[indexI] - b[indexI]) + b[indexI];
 
   /* Compute signed determinants */
   det_ap = a[indexI] - pt;
   det_pb = pt - b[indexI];
 
-  /* Test if sign of ABCD is equal to the signes of the auxiliary simplices */
+  /* Compare signs of AB and auxiliary simplices */
-  FacetsTest[ 0 ] = SAMESIGN( nu_test [ indexI ] , det_ap );
+  FacetsTest[0] = SAMESIGN(t[indexI], -1 * det_ap);
-  FacetsTest[ 1 ] = SAMESIGN( nu_test [ indexI ] , det_pb );
+  FacetsTest[1] = SAMESIGN(t[indexI], -1 * det_pb);
 
   if (FacetsTest[0] + FacetsTest[1] == 2) {
-    s->lambdas[0] = det_ap * inv_detM;
+    /* The origin is between A and B */
+    s->lambdas[0] = det_ap * -1.0 / t[indexI];
     s->lambdas[1] = 1 - s->lambdas[0];
     s->wids[0] = 0;
     s->wids[1] = 1;
     s->nvrtx = 2;
   }
   else if (FacetsTest[0] == 0) {
+    /* The origin is beyond A */
     s->lambdas[0] = 1;
     s->wids[0] = 0;
     s->nvrtx = 1;
-
     for (i = 0; i < 3; ++i) {
       s->vrtx[0][i] = s->vrtx[1][i];
     }
   }
   else {
+    /* The origin is behind B */
     s->lambdas[0] = 1;
     s->wids[0] = 1;
     s->nvrtx = 1;
   }
+   
+  for (int j = 0; j < 3; ++j) {
+    vv[j] = 0;
+    for (i = 0; i < s->nvrtx; ++i) {
+      vv[j] += (s->lambdas[i] * s->vrtx[i][j]);
+    }
+  } 
 }
 
-
-
 /**
-* @brief Function invoked by the Signed Volumes sub-algorithm for 2-simplex.
+ * @brief Finds point of minimum norm of 2-simplex. Robust, but slower, version of algorithm presented in paper.
  */
-static void S2D( struct simplex * s) {
+inline static void S2D(struct simplex * s, double *vv) {
+  int       i;
+  int       k;
+  int       l;
+  int       j;
+  int       indexI = 1;
+  //int       stemp[3];
+  int       FacetsTest[3];
+  int       indexJ[2] = { -1 };
+  double    nu_max = 0;
+  double    inv_detM = 0;
+  double    nnorm_sqrd = 0;
+  double    nnnorm = 0;
+  double    dotNA;
+  double    a[3];
+  double    b[3];
+  double    c[3];
+  double    s21[3];
+  double    s31[3];
+  double    nu_test[3];
+  double    nu_fabs[3];
+  double    B[3];
+  double    n[3];
+  double    v[3];
+  double    vtmp[3];
+  double    pp[3 - 1];
+  double    sa[3 - 1];
+  double    sb[3 - 1];
+  double    sc[3 - 1];
 
-  double    a[3],
-    b[3],
-    c[3],
-    s21[3],
-    s31[3],
-    nu_test[3],
-    nu_fabs[3],
-    B[ 3 ],
-    n[3],
-    v[3],
-    vtmp[3];
-
-
-  double pp[3-1],
-    sa[3-1],
-    sb[3-1],
-    sc[3-1];
-
-  double nu_max = 0,
-    inv_detM = 0,
-    nnorm_sqrd = 0,
-    nnnorm = 0,
-    dotNA;
-
-  int i = 0,
-    FacetsTest[3],
-    k,
-    l,
-    j;
-
-  int indexI = 1,
-    indexJ[2] = {0, 2},
-    stemp[3];
 
   for (i = 0; i < 3; ++i) {
     c[i] = s->vrtx[0][i];
@@ -214,6 +187,7 @@ 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]);
@@ -267,7 +241,8 @@ static void S2D( struct simplex * s) {
     n[i] = n[i] * nnnorm;
   }
 
-  dotNA = dotprod(n,a);
+  dotNA = dotProduct(n, a);
+
   pp[0] = dotNA * n[indexJ[0]];
   pp[1] = dotNA * n[indexJ[1]];
 
@@ -304,12 +279,12 @@ static void S2D( struct simplex * s) {
 #endif
 
   /* Test if sign of ABC is equal to the signes of the auxiliary simplices */
-  for ( int m = 0; m < 3 ; ++m) {
+  for (i = 0; i < 3; ++i) {
-    FacetsTest[ m ] = SAMESIGN( nu_max , B[ m ] );
+    FacetsTest[i] = SAMESIGN(nu_max, B[i]);
   }
 
-  if ( FacetsTest[0] +FacetsTest[1] + FacetsTest[2] == 0  || isnan(n[0]) ){
+  // The nan check was not included originally and will be removed in v2.0
-
+  if (FacetsTest[1] + FacetsTest[2] == 0 || isnan(n[0])) { 
     struct simplex  sTmp;
 
     sTmp.nvrtx = 2;
@@ -323,8 +298,8 @@ static void S2D( struct simplex * s) {
       s->vrtx[1][i] = s->vrtx[2][i];
     }
 
-    S1D(&sTmp);
+    S1D(&sTmp, v);
-    S1D(s);
+    S1D(s, v);
 
     for (j = 0; j < 3; ++j) {
       vtmp[j] = 0;
@@ -335,7 +310,8 @@ static void S2D( struct simplex * s) {
       }
     }
 
-    if( dotprod(v,v) < dotprod(vtmp,vtmp) ){
+    if (dotProduct(v, v) < dotProduct(vtmp, vtmp)) {
+      /* Keep simplex. Need to update sID only*/
       for (i = 1; i < s->nvrtx; ++i) {
         s->wids[i] = s->wids[i] + 1;
       }
@@ -370,60 +346,67 @@ static void S2D( struct simplex * s) {
       s->vrtx[0][i] = s->vrtx[1][i];
       s->vrtx[1][i] = s->vrtx[2][i];
     }
-    S1D(s);
+    S1D(s, v);
   }
   else if (FacetsTest[1] == 0) {
     /* The origin projection P faces the segment AC */
-    stemp[0] = 0;   stemp[1] = 2;
     s->nvrtx = 2;
     for (i = 0; i < 3; ++i) {
-      s->vrtx[0][i] = s->vrtx[ stemp[0] ][i];
+      s->vrtx[0][i] = s->vrtx[0][i];
-      s->vrtx[1][i] = s->vrtx[ stemp[1] ][i];
+      s->vrtx[1][i] = s->vrtx[2][i];
     }
-    S1D(s);
+    S1D(s, v);
     for (i = 1; i < s->nvrtx; ++i) {
       s->wids[i] = s->wids[i] + 1;
     }
   }
   else {
+    /* The origin projection P faces the segment BC */
     s->nvrtx = 2;
-    S1D(s);
+    S1D(s, v);
-  }
+
   }
   
+  for ( j = 0; j < 3; ++j) {
+    vv[j] = 0;
+    for (i = 0; i < s->nvrtx; ++i) {
+      vv[j] += (s->lambdas[i] * s->vrtx[i][j]);
+    }
+  }
+}
 
 /**
-* @brief Function invoked by the Signed Volumes sub-algorithm for 3-simplex.
+ * @brief Finds point of minimum norm of 3-simplex. Robust, but slower, version of algorithm presented in paper.
-*
  */
-static void S3D( struct simplex * s) {
+inline static void S3D(struct simplex * s,  double *vv) {
-
+  int      FacetsTest[4] = { 1,1,1,1 };
+  int      sID[4] = { 0, 0, 0, 0 };
+  int      TrianglesToTest[9] = { 3, 3, 3, 1, 2, 2, 0, 0, 1 };
+  int      i;
+  int      j;
+  int      k;
+  int      l;
+  int      vtxid;
+  int      ndoubts = 0;
+  int      nclosestS = 0;
+  int      firstaux = 0;
+  int      secondaux = 0;
+  int      Flag_sAuxused = 0;
   double   a[3];
   double   b[3];
   double   c[3];
   double   d[3];
-#ifdef ADAPTIVEFP
-  double  o[3] = {0}; 
-#endif
   double   vtmp[3];
   double   v[3];
   double   B[4];
+  double   tmplamda[4] = { 0, 0, 0, 0 };
+  double   tmpscoo1[4][3] = { 0 };
   double   sqdist_tmp = 0;
   double   detM;
   double   inv_detM;
-  double  tmplamda[4] = {0, 0, 0, 0};
+#ifdef ADAPTIVEFP
-  double  tmpscoo1[4][3] = {0};
+  double  o[3] = { 0 };
-  int     i, j, k, l,
+#endif
-          Flag_sAuxused = 0,
-          firstaux = 0,
-          secondaux = 0;
-  int     FacetsTest[ 4 ] = { 1,1,1,1 },
-          sID[4] = {0, 0, 0, 0},
-          nclosestS = 0;
-  int     TrianglesToTest[9] = { 3, 3, 3, 1, 2, 2, 0, 0, 1 };
-  int     ndoubts = 0,
-          vtxid;
-
   for (i = 0; i < 3; ++i) {
     d[i] = s->vrtx[0][i];
     c[i] = s->vrtx[1][i];
@@ -450,44 +433,27 @@ static void S3D( struct simplex * s) {
   if (fabs(detM) < eps)
   {
     if (fabs(B[2]) < eps && fabs(B[3]) < eps)
-    {
       FacetsTest[1] = 0; /* A = B. Test only ACD */
-    }
     else if (fabs(B[1]) < eps && fabs(B[3]) < eps)
-    {
       FacetsTest[2] = 0; /* A = C. Test only ABD */
-    }
     else if (fabs(B[1]) < eps && fabs(B[2]) < eps)
-    {
       FacetsTest[3] = 0; /* A = D. Test only ABC */
-    }
     else if (fabs(B[0]) < eps && fabs(B[3]) < eps)
-    {
       FacetsTest[1] = 0; /* B = C. Test only ACD */
-    }
     else if (fabs(B[0]) < eps && fabs(B[2]) < eps)
-    {
       FacetsTest[1] = 0; /* B = D. Test only ABD */
-    }
     else if (fabs(B[0]) < eps && fabs(B[1]) < eps)
-    {
       FacetsTest[2] = 0; /* C = D. Test only ABC */
-    }
+    else {
-    else
-    {
       for (i = 0; i < 4; i++)
-      {
+        FacetsTest[i] = 0; /* Any other case. Test ABC, ABD, ACD */
-        FacetsTest[i] = 0; 
     }
   }
-
-  }
   else 
   {
-    for (i = 0; i < 4 ; ++i) {
+    for (i = 0; i < 4; ++i) 
       FacetsTest[i] = SAMESIGN(detM, B[i]);
   }
-  }
 
   /* Compare signed volumes and compute barycentric coordinates */
   if (FacetsTest[0] + FacetsTest[1] + FacetsTest[2] + FacetsTest[3] == 4) {
@@ -502,7 +468,6 @@ static void S3D( struct simplex * s) {
     s->wids[2] = 2;
     s->wids[3] = 3;
     s->nvrtx = 4;
-    return;
   }
   else if (FacetsTest[1] + FacetsTest[2] + FacetsTest[3] == 0) {
     /* There are three facets facing the origin  */
@@ -513,14 +478,15 @@ static void S3D( struct simplex * s) {
     for (i = 0; i < ndoubts; ++i) {
       sTmp.nvrtx = 3;
       /* Assign coordinates to simplex */
-
       for (k = 0; k < sTmp.nvrtx; ++k) {
         vtxid = TrianglesToTest[i + (k * 3)];
         for (j = 0; j < 3; ++j) {
           sTmp.vrtx[2 - k][j] = s->vrtx[vtxid][j];
         }
       }
-      S2D(&sTmp);
+      /* ... and call S2D  */
+      S2D(&sTmp, v);
+      /* ... compute aux squared distance */
       for (j = 0; j < 3; ++j) {
         vtmp[j] = 0;
         for (l = 0; l < sTmp.nvrtx; ++l) {
@@ -529,15 +495,15 @@ static void S3D( struct simplex * s) {
       }
 
       if (i == 0) {
-        sqdist_tmp = dotprod(vtmp,vtmp);
+        sqdist_tmp = dotProduct(vtmp, vtmp);
         nclosestS = sTmp.nvrtx;
         for (l = 0; l < nclosestS; ++l) {
           sID[l] = TrianglesToTest[i + (sTmp.wids[l] * 3)];
           tmplamda[l] = sTmp.lambdas[l];
         }
       }
-      else if (dotprod(vtmp,vtmp) < sqdist_tmp){
+      else if (dotProduct(vtmp, vtmp) < sqdist_tmp) {
-        sqdist_tmp = dotprod(vtmp,vtmp);
+        sqdist_tmp = dotProduct(vtmp, vtmp);
         nclosestS = sTmp.nvrtx;
         for (l = 0; l < nclosestS; ++l) {
           sID[l] = TrianglesToTest[i + (sTmp.wids[l] * 3)];
@@ -561,8 +527,6 @@ static void S3D( struct simplex * s) {
       s->lambdas[i] = tmplamda[i];
       s->wids[nclosestS - 1 - i] = sID[i];
     }
-
-    return;
   }
   else if (FacetsTest[1] + FacetsTest[2] + FacetsTest[3] == 1) {
     /* There are two   facets facing the origin, need to find the closest.   */
@@ -576,18 +540,21 @@ static void S3D( struct simplex * s) {
         sTmp.vrtx[1][i] = s->vrtx[1][i];
         sTmp.vrtx[2][i] = s->vrtx[3][i];
       }
-      S2D(&sTmp);
+      /* ... and call S2D  */
+      S2D(&sTmp, v);
+      /* ... compute aux squared distance */
       for (j = 0; j < 3; ++j) {
         vtmp[j] = 0;
         for (i = 0; i < sTmp.nvrtx; ++i) {
           vtmp[j] += sTmp.lambdas[i] * (sTmp.vrtx[i][j]);
         }
       }
-      sqdist_tmp = dotprod(vtmp,vtmp);
+      sqdist_tmp = dotProduct(vtmp, vtmp);
       Flag_sAuxused = 1;
       firstaux = 0;
     }
     if (FacetsTest[2] == 0) {
+      /* ... Test facet ABD  */
       if (Flag_sAuxused == 0) {
 
         for (i = 0; i < 3; ++i) {
@@ -595,14 +562,16 @@ static void S3D( struct simplex * s) {
           sTmp.vrtx[1][i] = s->vrtx[2][i];
           sTmp.vrtx[2][i] = s->vrtx[3][i];
         }
-        S2D(&sTmp);
+        /* ... and call S2D  */
+        S2D(&sTmp, v);
+        /* ... compute aux squared distance */
         for (j = 0; j < 3; ++j) {
           vtmp[j] = 0;
           for (i = 0; i < sTmp.nvrtx; ++i) {
             vtmp[j] += sTmp.lambdas[i] * (sTmp.vrtx[i][j]);
           }
         }
-        sqdist_tmp = dotprod(vtmp,vtmp);
+        sqdist_tmp = dotProduct(vtmp, vtmp);
         firstaux = 1;
       }
       else {
@@ -613,7 +582,7 @@ static void S3D( struct simplex * s) {
           s->vrtx[2][i] = s->vrtx[3][i];
         }
         /* ... and call S2D itself */
-        S2D(s);
+        S2D(s, v);
         secondaux = 1;
       }
     }
@@ -626,25 +595,25 @@ static void S3D( struct simplex * s) {
         s->vrtx[1][i] = s->vrtx[2][i];
         s->vrtx[2][i] = s->vrtx[3][i];
       }
-      /* ... and call S2D itself */
+      /* ... and call S2D  */
-      S2D(s);
+      S2D(s, v);
       secondaux = 2;
     }
-    /* Do a loop and compare current outcomes */
+    /* Compare outcomes */ 
     for (j = 0; j < 3; ++j) {
       v[j] = 0;
       for (i = 0; i < s->nvrtx; ++i) {
         v[j] += s->lambdas[i] * (s->vrtx[i][j]);
       }
     }
-    if( dotprod(v,v) < sqdist_tmp){
+    if (dotProduct(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)];
       }
     }
     else {
-
       s->nvrtx = sTmp.nvrtx;
       for (i = 0; i < s->nvrtx; ++i) {
         for (j = 0; j < 3; ++j) {
@@ -654,40 +623,38 @@ static void S3D( struct simplex * s) {
         s->wids[sTmp.nvrtx - 1 - i] = TrianglesToTest[firstaux + (sTmp.wids[i] * 3)];
       }
     }
-
-    return;
   }
   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 */ 
       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[3][i];
       }
-      S2D(s);
+      S2D(s, v);
+
+      /* Keep simplex. Need to update sID only*/
       for (i = 0; i < s->nvrtx; ++i) {
         s->wids[i] = s->wids[i];
       }
-      return;
     }
     else if (FacetsTest[2] == 0) {
       /* The origin projection P faces the facet ABD */ 
-      /* ... thus, remove the vertex C from the simplex. */
       s->nvrtx = 3;
       for (i = 0; i < 3; ++i) {
         s->vrtx[0][i] = s->vrtx[0][i];
         s->vrtx[1][i] = s->vrtx[2][i];
         s->vrtx[2][i] = s->vrtx[3][i];
       }
-      S2D(s);
+      S2D(s, v);
       /* Keep simplex. Need to update sID only*/
       for (i = 2; i < s->nvrtx; ++i) {
         /* Assume that vertex a is always included in sID. */
         s->wids[i] = s->wids[i] + 1;
       }
-      return;
     }
     else if (FacetsTest[3] == 0) {
       /* The origin projection P faces the facet ABC */
@@ -697,168 +664,159 @@ static void S3D( struct simplex * s) {
         s->vrtx[1][i] = s->vrtx[2][i];
         s->vrtx[2][i] = s->vrtx[3][i];
       }
-      S2D(s);
+      S2D(s, v);
-      return;
     }
   }
   else {
+    /* 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];
     }
-    S2D(s);
+    /* ... and call S2D   */
+    S2D(s, v);
+    /* 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;
     }
-    return;
+  }
+
+
+  for ( j = 0; j < 3; ++j) {
+    vv[j] = 0;
+    for (i = 0; i < s->nvrtx; ++i) {
+      vv[j] += (s->lambdas[i] * s->vrtx[i][j]);
+    }
   }
 }
 
 
-/**
+inline static void support( struct bd *body, double *v ) {
-* @brief Evaluate support function for polytopes.
+  int    better = -1;
-*/
-void support ( struct bd *body, double *v ) {
-
   int    i;
   double s;
   double maxs;
   double *vrt;
  
-  /* Initialise variables */
+  maxs = dotProduct(body->s, v);
-  maxs = dotprod (body->coord[0],v);
-  body->s[0] = body->coord[0][0];
-  body->s[1] = body->coord[0][1];
-  body->s[2] = body->coord[0][2];
 
   for (i = 1; i < body->numpoints; ++i) {
     vrt = body->coord[i];
-    /* Evaluate function */
+    s = dotProduct (vrt, v);
-    s = dotprod (vrt,v);
     if ( s > maxs ){
       maxs = s;
-      body->s[0] = vrt[0];
+      better = i;
-      body->s[1] = vrt[1];
-      body->s[2] = vrt[2];
-    }
     }
   }
 
+  if (better != -1 ){
+	  body->s[0] = body->coord[better][0];
+	  body->s[1] = body->coord[better][1];
+    body->s[2] = body->coord[better][2];
+  }
+}
 
-/**
+inline static void subalgorithm( struct simplex *s, double *v ) {
-* @brief Signed Volumes method.
-*/
-void subalgorithm  ( struct simplex *s ) {
 
   switch ( s->nvrtx ){
     case 4: 
-      S3D( s );
+      S3D( s, v );
       break;
     case 3: 
-      S2D( s );
+      S2D( s, v );
       break;
     case 2: 
-      S1D( s );
+      S1D( s, v );
-      break;
-    default : 
-      s->lambdas[0] = 1.0;
       break;
   }
-
 }
 
-
-/**
-* @brief ~The GJK algorithm
-*
-*/
 double gjk( struct bd bd1, struct bd bd2, struct simplex *s) {
 
+  int    i;                   
   int    k = 0;                /**< Iteration counter            */
-  int i;                    /**< General purpose counter      */
+  int    mk = 50;              /**< Maximum number of iterations of the GJK algorithm */
-  int mk = 100;             /**< Maximum number of iterations of the GJK algorithm */
-  double v[3];              /**< Search direction             */
-  double vminus[3];         /**< Search direction             */
-  double w[3];              /**< Vertex on CSO frontier       */
-  double eps_rel = 1e-10;   /**< Tolerance on relative        */
-  double eps_tot = 1e-13;
-  double dd = -1;           /**< Squared distance             */
-  int maxitreached = 0;     /**< Flag for maximum iterations  */
-  int origininsimplex = 0;  /**< Flag for origin in simples   */
   int    exeedtol_rel = 0;     /**< Flag for 1st exit condition  */
-  int exeedtol_tot = 0;     /**< Flag for 2nd exit condition  */ 
+  double v[3];                 /**< Search direction             */
-  int nullV = 0;            /**< Flag for 3rd exit condition  */ 
+  double vminus[3];            /**< Search direction * -1        */
+  double w[3];                 /**< Vertex on CSO boundary given by the difference of support functions on both bodies */
+  double eps_rel = 1e-5;       /**< Tolerance on relative distance */
+  double eps_tot = 1e-15;      /**< Tolerance on absolute distance */
+  double norm2Wmax = 0;
+  double tesnorm = 0;
 
 #ifdef ADAPTIVEFP
   exactinit();
 #endif
 
-  /* Initialise search direction */
+  double eps_rel2 = eps_rel * eps_rel  ;
-  for ( i = 0; i < 3; ++i) {
+
+  /* Initialise  */
+  s->nvrtx = 1;
+  for ( i = 0; i < 3; i++)
+  {
     v[i] = bd1.coord[0][i] - bd2.coord[0][i];
-    vminus[i] = -v[i];
+    bd1.s[i] = bd1.coord[0][i];
+    bd2.s[i] = bd2.coord[0][i];
+    s->vrtx[0][i] = v[i];
   }
 
   /* Begin GJK iteration */
   do {
-    /* Increment iteration counter */
+
     k++;
 
-    /* Support function on polytope A */
+    /* Update negative search direction */
-    support( &bd1 , vminus );
+    vminus[0] = -v[0];
-    /* Support function on polytope B */
+    vminus[1] = -v[1];
-    support( &bd2 , v );
+    vminus[2] = -v[2];
-    /* Minkowski difference */
-    for ( i = 0; i < 3; ++i) {
-      w[i] = bd1.s[i] - bd2.s[i];
-    }
 
-    /* Test first exit condition (can't move further) */
+    /* Support function */
-    exeedtol_rel = (norm2(v) - dotprod (v,w) ) <= eps_rel * norm2(v);
+    support( &bd1 , vminus );
+    support( &bd2 , v );
+    w[0] = bd1.s[0] - bd2.s[0];
+    w[1] = bd1.s[1] - bd2.s[1];
+    w[2] = bd1.s[2] - bd2.s[2];
+
+    /* 1st exit condition */
+    exeedtol_rel = (norm2(v) - dotProduct (v,w) ) <= eps_rel2 * norm2(v);
     if ( exeedtol_rel ){
       break;
     }
-    nullV = norm2(v) < eps_rel;
+
-    if (nullV) {
+    /* 2nd exit condition */
+    if (norm2(v) < eps_rel2)
       break;
-    }
     
-    /* Add support vertex to simplex at the position nvrtx+1 */
+    /* Add new vertex to simplex */
+    i = s->nvrtx;
+    s->vrtx[i][0] = w[0];
+    s->vrtx[i][1] = w[1];
+    s->vrtx[i][2] = w[2];
     s->nvrtx++;
-    for ( i = 0; i < 3; ++i) {
+
-      s->vrtx[  s->nvrtx - 1  ][i] = w[i];
+    /* Invoke distance sub-algorithm */
+    subalgorithm ( s, v );
+
+    /* 3rd exit condition */
+    for (i = 0; i < s->nvrtx; i++)
+    {
+      tesnorm = norm2(s->vrtx[i]);
+      if (tesnorm > norm2Wmax) {
+        norm2Wmax = tesnorm;
       }
-    /* Invoke sub-distance algorithm */
-    subalgorithm ( s );
-
-    /* Termination tests */
-    maxitreached = k == mk;
-    origininsimplex = s->nvrtx == 4;
-    exeedtol_tot = norm2 (v) <= eps_tot ;
-
-    /* Update search direction */
-    for ( i = 0; i < 3; ++i) {
-      v[i] = 0;
     }
+    if (norm2(v) <= (eps_tot * eps_tot * norm2Wmax))
+      break;
 
-    for ( i = 0; i < s->nvrtx; ++i) {
+    /* 4th and 5th exit conditions */
-      v[0] += (s->lambdas[i] * s->vrtx[i][0]);
+  } while((s->nvrtx != 4) && (k != mk));
-      v[1] += (s->lambdas[i] * s->vrtx[i][1]);
-      v[2] += (s->lambdas[i] * s->vrtx[i][2]);
-    }
-    for ( i = 0; i < 3; ++i) {
-      vminus[i] = -v[i];
-    }
   
-  }while( !maxitreached && !origininsimplex && !exeedtol_tot && !exeedtol_rel );
+  /* Compute and return distance */ 
-
+  return sqrt(norm2(v));
-  /* Compute distance */
-  dd = sqrt ( norm2 (v));
-
-  return dd;
 }
 
 #ifdef MATLABDOESMEXSTUFF