From 66002145bd04c9610beee0a6b4f609f495760819 Mon Sep 17 00:00:00 2001 From: Mattia Montanari Date: Mon, 13 Feb 2023 14:35:05 +0100 Subject: [PATCH] Format all c and h files --- examples/c/main.c | 34 ++- include/openGJK/openGJK.h | 36 ++- openGJK.c | 498 +++++++++++++++++++++++--------------- 3 files changed, 341 insertions(+), 227 deletions(-) diff --git a/examples/c/main.c b/examples/c/main.c index 33e2b7a..aea3c2a 100644 --- a/examples/c/main.c +++ b/examples/c/main.c @@ -31,10 +31,11 @@ #define fscanf_s fscanf /// @brief Function for reading input file with body's coordinates. -int readinput(const char *inputfile, double ***pts, int *out) { +int +readinput(const char* inputfile, double*** pts, int* out) { int npoints = 0; int idx = 0; - FILE *fp; + FILE* fp; /* Open file. */ #ifdef WIN32 @@ -50,19 +51,21 @@ int readinput(const char *inputfile, double ***pts, int *out) { } /* Read number of input vertices. */ - if (fscanf_s(fp, "%d", &npoints) != 1) + if (fscanf_s(fp, "%d", &npoints) != 1) { return 1; + } /* Allocate memory. */ - double **arr = (double **)malloc(npoints * sizeof(double *)); - for (int i = 0; i < npoints; i++) - arr[i] = (double *)malloc(3 * sizeof(double)); + double** arr = (double**)malloc(npoints * sizeof(double*)); + for (int i = 0; i < npoints; i++) { + arr[i] = (double*)malloc(3 * sizeof(double)); + } /* Read and store vertices' coordinates. */ for (idx = 0; idx < npoints; idx++) { - if (fscanf_s(fp, "%lf %lf %lf\n", &arr[idx][0], &arr[idx][1], &arr[idx][2]) != - 3) + if (fscanf_s(fp, "%lf %lf %lf\n", &arr[idx][0], &arr[idx][1], &arr[idx][2]) != 3) { return 1; + } } fclose(fp); @@ -77,7 +80,8 @@ int readinput(const char *inputfile, double ***pts, int *out) { * @brief Main program of example1_c (described in Section 3.1 of the paper). * */ -int main() { +int +main() { /* Squared distance computed by openGJK. */ double dd; /* Structure of simplex used by openGJK. */ @@ -96,14 +100,16 @@ int main() { * two bodies that will be passed to the GJK procedure. */ /* Import coordinates of object 1. */ - if (readinput(inputfileA, &vrtx1, &nvrtx1)) + if (readinput(inputfileA, &vrtx1, &nvrtx1)) { return (1); + } bd1.coord = vrtx1; bd1.numpoints = nvrtx1; /* Import coordinates of object 2. */ - if (readinput(inputfileB, &vrtx2, &nvrtx2)) + if (readinput(inputfileB, &vrtx2, &nvrtx2)) { return (1); + } bd2.coord = vrtx2; bd2.numpoints = nvrtx2; @@ -118,11 +124,13 @@ int main() { printf("Distance between bodies %f\n", dd); /* Free memory */ - for (int i = 0; i < bd1.numpoints; i++) + for (int i = 0; i < bd1.numpoints; i++) { free(bd1.coord[i]); + } free(bd1.coord); - for (int i = 0; i < bd2.numpoints; i++) + for (int i = 0; i < bd2.numpoints; i++) { free(bd2.coord[i]); + } free(bd2.coord); return (0); diff --git a/include/openGJK/openGJK.h b/include/openGJK/openGJK.h index 7529733..8bc4757 100644 --- a/include/openGJK/openGJK.h +++ b/include/openGJK/openGJK.h @@ -26,16 +26,14 @@ * @date 1 Jan 2022 * @brief Main interface of OpenGJK containing quick reference and API documentation. * - * More extensive explanation of what the header - * @see http://google.com + * @see https://www.mattiamontanari.com/opengjk/ */ #ifndef OPENGJK_H__ #define OPENGJK_H__ #ifdef __cplusplus -extern "C" -{ +extern "C" { #endif /*! @brief Precision of floating-point numbers. @@ -43,29 +41,29 @@ extern "C" * Default is set to 64-bit (Double). Change this to quickly play around with 16- and 32-bit. */ #define gkFloat double - /*! @brief Data structure for convex polytopes. +/*! @brief Data structure for convex polytopes. * * Polytopes are three-dimensional shapes and the GJK algorithm works directly on their convex-hull. However the convex-hull is never computed explicity, instead each GJK-iteraion employs a support function that has a cost linearly dependen on the number of points defining the polytope. */ - typedef struct gkPolytope_ - { - int numpoints; /*!< Number of points defining the polytope. */ - gkFloat s[3]; /*!< Furthest point retunred by the support function and updated at each GJK-iteration. For the first itearion this value is a guess - and this guess not irrelevant. */ - gkFloat **coord; /*!< Coordinates of the points of the polytope. This is owned by user who manages and garbage-collects the memory for these coordinates. */ - } gkPolytope; +typedef struct gkPolytope_ { + int numpoints; /*!< Number of points defining the polytope. */ + gkFloat s + [3]; /*!< Furthest point retunred by the support function and updated at each GJK-iteration. For the first itearion this value is a guess - and this guess not irrelevant. */ + gkFloat** + coord; /*!< Coordinates of the points of the polytope. This is owned by user who manages and garbage-collects the memory for these coordinates. */ +} gkPolytope; - /*! @brief Data structure for simplex. +/*! @brief Data structure for simplex. * * The simplex is updated at each GJK-iteration. For the first itearion this value is a guess - and this guess not irrelevant. */ - typedef struct gkSimplex_ - { - int nvrtx; /*!< Number of points defining the simplex. */ - gkFloat vrtx[4][3]; /*!< Coordinates of the points of the simplex. */ - } gkSimplex; +typedef struct gkSimplex_ { + int nvrtx; /*!< Number of points defining the simplex. */ + gkFloat vrtx[4][3]; /*!< Coordinates of the points of the simplex. */ +} gkSimplex; - /*! @brief Invoke the GJK algorithm to compute the minimum distance between two polytopes. +/*! @brief Invoke the GJK algorithm to compute the minimum distance between two polytopes. * * The simplex has to be initialised prior the call to this function. */ - gkFloat compute_minimum_distance(const gkPolytope p_, const gkPolytope q_, gkSimplex *s_); +gkFloat compute_minimum_distance(const gkPolytope p_, const gkPolytope q_, gkSimplex* s_); #ifdef __cplusplus } diff --git a/openGJK.c b/openGJK.c index d8777ec..c7bc73e 100644 --- a/openGJK.c +++ b/openGJK.c @@ -20,6 +20,15 @@ // ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // // FOR A PARTICULAR PURPOSE. See GNU General Public License for details. // +/** + * @file openGJK.c + * @author Mattia Montanari + * @date 1 Jan 2022 + * @brief Source of OpenGJK and its fast sub-algorithm. + * + * @see https://www.mattiamontanari.com/opengjk/ + */ + #include "openGJK/openGJK.h" #include @@ -27,119 +36,137 @@ #include "math.h" -/* If instricuted, compile a mex function for Matlab. */ +/** If instricuted, compile a mex function for Matlab. */ #ifdef MATLAB_MEX_BUILD #include "mex.h" #else #define mexPrintf printf #endif -#define eps_rel22 1e-10 -#define eps_tot22 1e-12 +#define eps_rel22 1e-10 +#define eps_tot22 1e-12 -#define norm2(a) (a[0] * a[0] + a[1] * a[1] + a[2] * a[2]) +#define norm2(a) (a[0] * a[0] + a[1] * a[1] + a[2] * a[2]) #define dotProduct(a, b) (a[0] * b[0] + a[1] * b[1] + a[2] * b[2]) -#define S3Dregion1234() \ - v[0] = 0; \ - v[1] = 0; \ - v[2] = 0; \ +#define S3Dregion1234() \ + v[0] = 0; \ + v[1] = 0; \ + v[2] = 0; \ s->nvrtx = 4; -#define select_1ik() \ - s->nvrtx = 3; \ - for (t = 0; t < 3; t++) s->vrtx[2][t] = s->vrtx[3][t]; \ - for (t = 0; t < 3; t++) s->vrtx[1][t] = si[t]; \ - for (t = 0; t < 3; t++) s->vrtx[0][t] = sk[t]; +#define select_1ik() \ + s->nvrtx = 3; \ + for (t = 0; t < 3; t++) \ + s->vrtx[2][t] = s->vrtx[3][t]; \ + for (t = 0; t < 3; t++) \ + s->vrtx[1][t] = si[t]; \ + for (t = 0; t < 3; t++) \ + s->vrtx[0][t] = sk[t]; -#define select_1ij() \ - s->nvrtx = 3; \ - for (t = 0; t < 3; t++) s->vrtx[2][t] = s->vrtx[3][t]; \ - for (t = 0; t < 3; t++) s->vrtx[1][t] = si[t]; \ - for (t = 0; t < 3; t++) s->vrtx[0][t] = sj[t]; +#define select_1ij() \ + s->nvrtx = 3; \ + for (t = 0; t < 3; t++) \ + s->vrtx[2][t] = s->vrtx[3][t]; \ + for (t = 0; t < 3; t++) \ + s->vrtx[1][t] = si[t]; \ + for (t = 0; t < 3; t++) \ + s->vrtx[0][t] = sj[t]; -#define select_1jk() \ - s->nvrtx = 3; \ - for (t = 0; t < 3; t++) s->vrtx[2][t] = s->vrtx[3][t]; \ - for (t = 0; t < 3; t++) s->vrtx[1][t] = sj[t]; \ - for (t = 0; t < 3; t++) s->vrtx[0][t] = sk[t]; +#define select_1jk() \ + s->nvrtx = 3; \ + for (t = 0; t < 3; t++) \ + s->vrtx[2][t] = s->vrtx[3][t]; \ + for (t = 0; t < 3; t++) \ + s->vrtx[1][t] = sj[t]; \ + for (t = 0; t < 3; t++) \ + s->vrtx[0][t] = sk[t]; -#define select_1i() \ - s->nvrtx = 2; \ - for (t = 0; t < 3; t++) s->vrtx[1][t] = s->vrtx[3][t]; \ - for (t = 0; t < 3; t++) s->vrtx[0][t] = si[t]; +#define select_1i() \ + s->nvrtx = 2; \ + for (t = 0; t < 3; t++) \ + s->vrtx[1][t] = s->vrtx[3][t]; \ + for (t = 0; t < 3; t++) \ + s->vrtx[0][t] = si[t]; -#define select_1j() \ - s->nvrtx = 2; \ - for (t = 0; t < 3; t++) s->vrtx[1][t] = s->vrtx[3][t]; \ - for (t = 0; t < 3; t++) s->vrtx[0][t] = sj[t]; +#define select_1j() \ + s->nvrtx = 2; \ + for (t = 0; t < 3; t++) \ + s->vrtx[1][t] = s->vrtx[3][t]; \ + for (t = 0; t < 3; t++) \ + s->vrtx[0][t] = sj[t]; -#define select_1k() \ - s->nvrtx = 2; \ - for (t = 0; t < 3; t++) s->vrtx[1][t] = s->vrtx[3][t]; \ - for (t = 0; t < 3; t++) s->vrtx[0][t] = sk[t]; +#define select_1k() \ + s->nvrtx = 2; \ + for (t = 0; t < 3; t++) \ + s->vrtx[1][t] = s->vrtx[3][t]; \ + for (t = 0; t < 3; t++) \ + s->vrtx[0][t] = sk[t]; -#define getvrtx(point, location) \ - point[0] = s->vrtx[location][0]; \ - point[1] = s->vrtx[location][1]; \ +#define getvrtx(point, location) \ + point[0] = s->vrtx[location][0]; \ + point[1] = s->vrtx[location][1]; \ point[2] = s->vrtx[location][2]; -#define calculateEdgeVector(p1p2, p2) \ - p1p2[0] = p2[0] - s->vrtx[3][0]; \ - p1p2[1] = p2[1] - s->vrtx[3][1]; \ +#define calculateEdgeVector(p1p2, p2) \ + p1p2[0] = p2[0] - s->vrtx[3][0]; \ + p1p2[1] = p2[1] - s->vrtx[3][1]; \ p1p2[2] = p2[2] - s->vrtx[3][2]; -#define S1Dregion1() \ - v[0] = s->vrtx[1][0]; \ - v[1] = s->vrtx[1][1]; \ - v[2] = s->vrtx[1][2]; \ - s->nvrtx = 1; \ - s->vrtx[0][0] = s->vrtx[1][0]; \ - s->vrtx[0][1] = s->vrtx[1][1]; \ +#define S1Dregion1() \ + v[0] = s->vrtx[1][0]; \ + v[1] = s->vrtx[1][1]; \ + v[2] = s->vrtx[1][2]; \ + s->nvrtx = 1; \ + s->vrtx[0][0] = s->vrtx[1][0]; \ + s->vrtx[0][1] = s->vrtx[1][1]; \ s->vrtx[0][2] = s->vrtx[1][2]; -#define S2Dregion1() \ - v[0] = s->vrtx[2][0]; \ - v[1] = s->vrtx[2][1]; \ - v[2] = s->vrtx[2][2]; \ - s->nvrtx = 1; \ - s->vrtx[0][0] = s->vrtx[2][0]; \ - s->vrtx[0][1] = s->vrtx[2][1]; \ +#define S2Dregion1() \ + v[0] = s->vrtx[2][0]; \ + v[1] = s->vrtx[2][1]; \ + v[2] = s->vrtx[2][2]; \ + s->nvrtx = 1; \ + s->vrtx[0][0] = s->vrtx[2][0]; \ + s->vrtx[0][1] = s->vrtx[2][1]; \ s->vrtx[0][2] = s->vrtx[2][2]; -#define S2Dregion12() \ - s->nvrtx = 2; \ - s->vrtx[0][0] = s->vrtx[2][0]; \ - s->vrtx[0][1] = s->vrtx[2][1]; \ +#define S2Dregion12() \ + s->nvrtx = 2; \ + s->vrtx[0][0] = s->vrtx[2][0]; \ + s->vrtx[0][1] = s->vrtx[2][1]; \ s->vrtx[0][2] = s->vrtx[2][2]; -#define S2Dregion13() \ - s->nvrtx = 2; \ - s->vrtx[1][0] = s->vrtx[2][0]; \ - s->vrtx[1][1] = s->vrtx[2][1]; \ +#define S2Dregion13() \ + s->nvrtx = 2; \ + s->vrtx[1][0] = s->vrtx[2][0]; \ + s->vrtx[1][1] = s->vrtx[2][1]; \ s->vrtx[1][2] = s->vrtx[2][2]; -#define S3Dregion1() \ - v[0] = s1[0]; \ - v[1] = s1[1]; \ - v[2] = s1[2]; \ - s->nvrtx = 1; \ - s->vrtx[0][0] = s1[0]; \ - s->vrtx[0][1] = s1[1]; \ +#define S3Dregion1() \ + v[0] = s1[0]; \ + v[1] = s1[1]; \ + v[2] = s1[2]; \ + s->nvrtx = 1; \ + s->vrtx[0][0] = s1[0]; \ + s->vrtx[0][1] = s1[1]; \ s->vrtx[0][2] = s1[2]; -inline static gkFloat determinant(const gkFloat *p, const gkFloat *q, const gkFloat *r) { - return p[0] * ((q[1] * r[2]) - (r[1] * q[2])) - p[1] * (q[0] * r[2] - r[0] * q[2]) + - p[2] * (q[0] * r[1] - r[0] * q[1]); +inline static gkFloat +determinant(const gkFloat* p, const gkFloat* q, const gkFloat* r) { + return p[0] * ((q[1] * r[2]) - (r[1] * q[2])) - p[1] * (q[0] * r[2] - r[0] * q[2]) + + p[2] * (q[0] * r[1] - r[0] * q[1]); } -inline static void crossProduct(const gkFloat *a, const gkFloat *b, gkFloat *c) { +inline static void +crossProduct(const gkFloat* a, const gkFloat* b, gkFloat* c) { c[0] = a[1] * b[2] - a[2] * b[1]; c[1] = a[2] * b[0] - a[0] * b[2]; c[2] = a[0] * b[1] - a[1] * b[0]; } -inline static void projectOnLine(const gkFloat *p, const gkFloat *q, gkFloat *v) { +inline static void +projectOnLine(const gkFloat* p, const gkFloat* q, gkFloat* v) { gkFloat pq[3]; gkFloat tmp; pq[0] = p[0] - q[0]; @@ -148,82 +175,116 @@ inline static void projectOnLine(const gkFloat *p, const gkFloat *q, gkFloat *v) tmp = dotProduct(p, pq) / dotProduct(pq, pq); - for (int i = 0; i < 3; i++) v[i] = p[i] - pq[i] * tmp; + for (int i = 0; i < 3; i++) { + v[i] = p[i] - pq[i] * tmp; + } } -inline static void projectOnPlane(const gkFloat *p, const gkFloat *q, const gkFloat *r, gkFloat *v) { +inline static void +projectOnPlane(const gkFloat* p, const gkFloat* q, const gkFloat* r, gkFloat* v) { gkFloat n[3], pq[3], pr[3]; gkFloat tmp; - for (int i = 0; i < 3; i++) pq[i] = p[i] - q[i]; - for (int i = 0; i < 3; i++) pr[i] = p[i] - r[i]; + for (int i = 0; i < 3; i++) { + pq[i] = p[i] - q[i]; + } + for (int i = 0; i < 3; i++) { + pr[i] = p[i] - r[i]; + } crossProduct(pq, pr, n); tmp = dotProduct(n, p) / dotProduct(n, n); - for (int i = 0; i < 3; i++) v[i] = n[i] * tmp; + for (int i = 0; i < 3; i++) { + v[i] = n[i] * tmp; + } } -inline static int hff1(const gkFloat *p, const gkFloat *q) { +inline static int +hff1(const gkFloat* p, const gkFloat* q) { gkFloat tmp = 0; - for (int i = 0; i < 3; i++) tmp += (p[i] * p[i] - p[i] * q[i]); + for (int i = 0; i < 3; i++) { + tmp += (p[i] * p[i] - p[i] * q[i]); + } - if (tmp > 0) return 1; // keep q + if (tmp > 0) { + return 1; // keep q + } return 0; } -inline static int hff2(const gkFloat *p, const gkFloat *q, const gkFloat *r) { +inline static int +hff2(const gkFloat* p, const gkFloat* q, const gkFloat* r) { gkFloat ntmp[3]; gkFloat n[3], pq[3], pr[3]; gkFloat tmp = 0; - for (int i = 0; i < 3; i++) pq[i] = q[i] - p[i]; - for (int i = 0; i < 3; i++) pr[i] = r[i] - p[i]; + for (int i = 0; i < 3; i++) { + pq[i] = q[i] - p[i]; + } + for (int i = 0; i < 3; i++) { + pr[i] = r[i] - p[i]; + } crossProduct(pq, pr, ntmp); crossProduct(pq, ntmp, n); - for (int i = 0; i < 3; i++) tmp = tmp + (p[i] * n[i]); + for (int i = 0; i < 3; i++) { + tmp = tmp + (p[i] * n[i]); + } - if (tmp < 0) return 1; // Discard r + if (tmp < 0) { + return 1; // Discard r + } return 0; } -inline static int hff3(const gkFloat *p, const gkFloat *q, const gkFloat *r) { +inline static int +hff3(const gkFloat* p, const gkFloat* q, const gkFloat* r) { gkFloat n[3], pq[3], pr[3]; gkFloat tmp = 0; - for (int i = 0; i < 3; i++) pq[i] = q[i] - p[i]; - for (int i = 0; i < 3; i++) pr[i] = r[i] - p[i]; + for (int i = 0; i < 3; i++) { + pq[i] = q[i] - p[i]; + } + for (int i = 0; i < 3; i++) { + pr[i] = r[i] - p[i]; + } crossProduct(pq, pr, n); - for (int i = 0; i < 3; i++) tmp = tmp + (p[i] * n[i]); + for (int i = 0; i < 3; i++) { + tmp = tmp + (p[i] * n[i]); + } - if (tmp > 0) return 0; // discard s + if (tmp > 0) { + return 0; // discard s + } return 1; } -inline static void S1D(gkSimplex *s, gkFloat *v) { - gkFloat *s1p = s->vrtx[1]; - gkFloat *s2p = s->vrtx[0]; +inline static void +S1D(gkSimplex* s, gkFloat* v) { + gkFloat* s1p = s->vrtx[1]; + gkFloat* s2p = s->vrtx[0]; if (hff1(s1p, s2p)) { - projectOnLine(s1p, s2p, v); // Update v, no need to update s - return; // Return V{1,2} + projectOnLine(s1p, s2p, v); // Update v, no need to update s + return; // Return V{1,2} } else { - S1Dregion1(); // Update v and s - return; // Return V{1} + S1Dregion1(); // Update v and s + return; // Return V{1} } } -inline static void S2D(gkSimplex *s, gkFloat *v) { - gkFloat *s1p = s->vrtx[2]; - gkFloat *s2p = s->vrtx[1]; - gkFloat *s3p = s->vrtx[0]; +inline static void +S2D(gkSimplex* s, gkFloat* v) { + gkFloat* s1p = s->vrtx[2]; + gkFloat* s2p = s->vrtx[1]; + gkFloat* s3p = s->vrtx[0]; int hff1f_s12 = hff1(s1p, s2p); int hff1f_s13 = hff1(s1p, s3p); @@ -233,39 +294,40 @@ inline static void S2D(gkSimplex *s, gkFloat *v) { if (hff1f_s13) { int hff2f_32 = !hff2(s1p, s3p, s2p); if (hff2f_32) { - projectOnPlane(s1p, s2p, s3p, v); // Update s, no need to update c - return; // Return V{1,2,3} + projectOnPlane(s1p, s2p, s3p, v); // Update s, no need to update c + return; // Return V{1,2,3} } else { - projectOnLine(s1p, s3p, v); // Update v - S2Dregion13(); // Update s - return; // Return V{1,3} + projectOnLine(s1p, s3p, v); // Update v + S2Dregion13(); // Update s + return; // Return V{1,3} } } else { - projectOnPlane(s1p, s2p, s3p, v); // Update s, no need to update c - return; // Return V{1,2,3} + projectOnPlane(s1p, s2p, s3p, v); // Update s, no need to update c + return; // Return V{1,2,3} } } else { - projectOnLine(s1p, s2p, v); // Update v - S2Dregion12(); // Update s - return; // Return V{1,2} + projectOnLine(s1p, s2p, v); // Update v + S2Dregion12(); // Update s + return; // Return V{1,2} } } else if (hff1f_s13) { int hff2f_32 = !hff2(s1p, s3p, s2p); if (hff2f_32) { - projectOnPlane(s1p, s2p, s3p, v); // Update s, no need to update v - return; // Return V{1,2,3} + projectOnPlane(s1p, s2p, s3p, v); // Update s, no need to update v + return; // Return V{1,2,3} } else { - projectOnLine(s1p, s3p, v); // Update v - S2Dregion13(); // Update s - return; // Return V{1,3} + projectOnLine(s1p, s3p, v); // Update v + S2Dregion13(); // Update s + return; // Return V{1,3} } } else { - S2Dregion1(); // Update s and v - return; // Return V{1} + S2Dregion1(); // Update s and v + return; // Return V{1} } } -inline static void S3D(gkSimplex *s, gkFloat *v) { +inline static void +S3D(gkSimplex* s, gkFloat* v) { gkFloat s1[3], s2[3], s3[3], s4[3], s1s2[3], s1s3[3], s1s4[3]; gkFloat si[3], sj[3], sk[3]; int testLineThree, testLineFour, testPlaneTwo, testPlaneThree, testPlaneFour, dotTotal; @@ -317,15 +379,27 @@ inline static void S3D(gkSimplex *s, gkFloat *v) { // 1,i,j, are the indices of the points on the triangle and remove k from // simplex s->nvrtx = 3; - if (!testPlaneTwo) { // k = 2; removes s2 - for (i = 0; i < 3; i++) s->vrtx[2][i] = s->vrtx[3][i]; - } else if (!testPlaneThree) { // k = 1; // removes s3 - for (i = 0; i < 3; i++) s->vrtx[1][i] = s2[i]; - for (i = 0; i < 3; i++) s->vrtx[2][i] = s->vrtx[3][i]; - } else if (!testPlaneFour) { // k = 0; // removes s4 and no need to reorder - for (i = 0; i < 3; i++) s->vrtx[0][i] = s3[i]; - for (i = 0; i < 3; i++) s->vrtx[1][i] = s2[i]; - for (i = 0; i < 3; i++) s->vrtx[2][i] = s->vrtx[3][i]; + if (!testPlaneTwo) { // k = 2; removes s2 + for (i = 0; i < 3; i++) { + s->vrtx[2][i] = s->vrtx[3][i]; + } + } else if (!testPlaneThree) { // k = 1; // removes s3 + for (i = 0; i < 3; i++) { + s->vrtx[1][i] = s2[i]; + } + for (i = 0; i < 3; i++) { + s->vrtx[2][i] = s->vrtx[3][i]; + } + } else if (!testPlaneFour) { // k = 0; // removes s4 and no need to reorder + for (i = 0; i < 3; i++) { + s->vrtx[0][i] = s3[i]; + } + for (i = 0; i < 3; i++) { + s->vrtx[1][i] = s2[i]; + } + for (i = 0; i < 3; i++) { + s->vrtx[2][i] = s->vrtx[3][i]; + } } // Call S2D S2D(s, v); @@ -339,15 +413,15 @@ inline static void S3D(gkSimplex *s, gkFloat *v) { // simplex s->nvrtx = 3; if (testPlaneTwo) { - k = 2; // s2 + k = 2; // s2 i = 1; j = 0; } else if (testPlaneThree) { - k = 1; // s3 + k = 1; // s3 i = 0; j = 2; } else { - k = 0; // s4 + k = 0; // s4 i = 2; j = 1; } @@ -365,7 +439,7 @@ inline static void S3D(gkSimplex *s, gkFloat *v) { select_1jk(); projectOnPlane(s1, sj, sk, v); } else { - select_1k(); // select region 1i + select_1k(); // select region 1i projectOnLine(s1, sk, v); } } else if (hff1_tests[i]) { @@ -373,7 +447,7 @@ inline static void S3D(gkSimplex *s, gkFloat *v) { select_1ik(); projectOnPlane(s1, si, sk, v); } else { - select_1i(); // select region 1i + select_1i(); // select region 1i projectOnLine(s1, si, v); } } else { @@ -381,7 +455,7 @@ inline static void S3D(gkSimplex *s, gkFloat *v) { select_1jk(); projectOnPlane(s1, sj, sk, v); } else { - select_1j(); // select region 1i + select_1j(); // select region 1i projectOnLine(s1, sj, v); } } @@ -392,38 +466,38 @@ inline static void S3D(gkSimplex *s, gkFloat *v) { // hff1_1k is positive if (hff1_tests[i]) { - if (!hff2(s1, sk, si)) + if (!hff2(s1, sk, si)) { if (!hff2(s1, si, sk)) { - select_1ik(); // select region 1ik + select_1ik(); // select region 1ik projectOnPlane(s1, si, sk, v); } else { - select_1k(); // select region 1k + select_1k(); // select region 1k projectOnLine(s1, sk, v); } - else { + } else { if (!hff2(s1, sk, sj)) { - select_1jk(); // select region 1jk + select_1jk(); // select region 1jk projectOnPlane(s1, sj, sk, v); } else { - select_1k(); // select region 1k + select_1k(); // select region 1k projectOnLine(s1, sk, v); } } - } else if (hff1_tests[j]) { // there is no other choice - if (!hff2(s1, sk, sj)) + } else if (hff1_tests[j]) { // there is no other choice + if (!hff2(s1, sk, sj)) { if (!hff2(s1, sj, sk)) { - select_1jk(); // select region 1jk + select_1jk(); // select region 1jk projectOnPlane(s1, sj, sk, v); } else { - select_1j(); // select region 1j + select_1j(); // select region 1j projectOnLine(s1, sj, v); } - else { + } else { if (!hff2(s1, sk, si)) { - select_1ik(); // select region 1ik + select_1ik(); // select region 1ik projectOnPlane(s1, si, sk, v); } else { - select_1k(); // select region 1k + select_1k(); // select region 1k projectOnLine(s1, sk, v); } } @@ -477,15 +551,15 @@ inline static void S3D(gkSimplex *s, gkFloat *v) { // Here si is set such that hff(s1,si) > 0 if (testLineThree) { k = 2; - i = 1; // s3 + i = 1; // s3 j = 0; } else if (testLineFour) { - k = 1; // s3 + k = 1; // s3 i = 0; j = 2; } else { k = 0; - i = 2; // s2 + i = 2; // s2 j = 1; } getvrtx(si, i); @@ -507,15 +581,15 @@ inline static void S3D(gkSimplex *s, gkFloat *v) { s->nvrtx = 3; if (!testLineThree) { k = 2; - i = 1; // s3 + i = 1; // s3 j = 0; } else if (!testLineFour) { k = 1; - i = 0; // s4 + i = 0; // s4 j = 2; } else { k = 0; - i = 2; // s2 + i = 2; // s2 j = 1; } getvrtx(si, i); @@ -524,7 +598,7 @@ inline static void S3D(gkSimplex *s, gkFloat *v) { if (!hff2(s1, sj, sk)) { if (!hff2(s1, sk, sj)) { - select_1jk(); // select region 1jk + select_1jk(); // select region 1jk projectOnPlane(s1, sj, sk, v); } else if (!hff2(s1, sk, si)) { select_1ik(); @@ -547,9 +621,10 @@ inline static void S3D(gkSimplex *s, gkFloat *v) { } } -inline static void support(gkPolytope *body, const gkFloat *v) { +inline static void +support(gkPolytope* body, const gkFloat* v) { gkFloat s, maxs; - gkFloat *vrt; + gkFloat* vrt; int better = -1; maxs = dotProduct(body->s, v); @@ -570,7 +645,8 @@ inline static void support(gkPolytope *body, const gkFloat *v) { } } -inline static void subalgorithm(gkSimplex *s, gkFloat *v) { +inline static void +subalgorithm(gkSimplex* s, gkFloat* v) { switch (s->nvrtx) { case 4: S3D(s, v); @@ -586,10 +662,11 @@ inline static void subalgorithm(gkSimplex *s, gkFloat *v) { } } -gkFloat compute_minimum_distance(gkPolytope bd1, gkPolytope bd2, gkSimplex *s) { - int k = 0; /**< Iteration counter */ - int i; /**< General purpose counter */ - int mk = 25; /**< Maximum number of iterations of the GJK algorithm */ +gkFloat +compute_minimum_distance(gkPolytope bd1, gkPolytope bd2, gkSimplex* s) { + int k = 0; /**< Iteration counter */ + int i; /**< General purpose counter */ + int mk = 25; /**< Maximum number of iterations of the GJK algorithm */ int absTestin; gkFloat norm2Wmax = 0; gkFloat tesnorm; @@ -600,7 +677,7 @@ gkFloat compute_minimum_distance(gkPolytope bd1, gkPolytope bd2, gkSimplex *s) { gkFloat eps_rel = eps_rel22; /**< Tolerance on relative */ gkFloat eps_rel2 = eps_rel * eps_rel; gkFloat eps_tot = eps_tot22; - gkFloat exeedtol_rel; /**< Test for 1st exit condition */ + gkFloat exeedtol_rel; /**< Test for 1st exit condition */ int nullV = 0; /* Initialise search direction */ @@ -610,23 +687,33 @@ gkFloat compute_minimum_distance(gkPolytope bd1, gkPolytope bd2, gkSimplex *s) { /* Inialise simplex */ s->nvrtx = 1; - for (int t = 0; t < 3; ++t) s->vrtx[0][t] = v[t]; + for (int t = 0; t < 3; ++t) { + s->vrtx[0][t] = v[t]; + } - for (int t = 0; t < 3; ++t) bd1.s[t] = bd1.coord[0][t]; + for (int t = 0; t < 3; ++t) { + bd1.s[t] = bd1.coord[0][t]; + } - for (int t = 0; t < 3; ++t) bd2.s[t] = bd2.coord[0][t]; + for (int t = 0; t < 3; ++t) { + bd2.s[t] = bd2.coord[0][t]; + } /* Begin GJK iteration */ do { k++; /* Update negative search direction */ - for (int t = 0; t < 3; ++t) vminus[t] = -v[t]; + for (int t = 0; t < 3; ++t) { + vminus[t] = -v[t]; + } /* Support function */ support(&bd1, vminus); support(&bd2, v); - for (int t = 0; t < 3; ++t) w[t] = bd1.s[t] - bd2.s[t]; + for (int t = 0; t < 3; ++t) { + w[t] = bd1.s[t] - bd2.s[t]; + } /* Test first exit condition (new point already in simplex/can't move * further) */ @@ -642,7 +729,9 @@ gkFloat compute_minimum_distance(gkPolytope bd1, gkPolytope bd2, gkSimplex *s) { /* Add new vertex to simplex */ i = s->nvrtx; - for (int t = 0; t < 3; ++t) s->vrtx[i][t] = w[t]; + for (int t = 0; t < 3; ++t) { + s->vrtx[i][t] = w[t]; + } s->nvrtx++; /* Invoke distance sub-algorithm */ @@ -664,9 +753,8 @@ gkFloat compute_minimum_distance(gkPolytope bd1, gkPolytope bd2, gkSimplex *s) { } while ((s->nvrtx != 4) && (k != mk)); if (k == mk) { - mexPrintf( - "\n * * * * * * * * * * * * MAXIMUM ITERATION NUMBER REACHED!!! " - " * * * * * * * * * * * * * * \n"); + mexPrintf("\n * * * * * * * * * * * * MAXIMUM ITERATION NUMBER REACHED!!! " + " * * * * * * * * * * * * * * \n"); } return sqrt(norm2(v)); @@ -676,17 +764,18 @@ gkFloat compute_minimum_distance(gkPolytope bd1, gkPolytope bd2, gkSimplex *s) { /** * @brief Mex function for Matlab. */ -void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { - gkFloat *inCoordsA; - gkFloat *inCoordsB; +void +mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { + gkFloat* inCoordsA; + gkFloat* inCoordsB; size_t nCoordsA; size_t nCoordsB; int i; - gkFloat *distance; + gkFloat* distance; int c = 3; int count = 0; - gkFloat **arr1; - gkFloat **arr2; + gkFloat** arr1; + gkFloat** arr2; /**************** PARSE INPUTS AND OUTPUTS **********************/ /*----------------------------------------------------------------*/ @@ -737,12 +826,16 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { distance = mxGetPr(plhs[0]); /* Copy data from Matlab's vectors into two new arrays */ - arr1 = (gkFloat **)mxMalloc(sizeof(gkFloat *) * (int)nCoordsA); - arr2 = (gkFloat **)mxMalloc(sizeof(gkFloat *) * (int)nCoordsB); + arr1 = (gkFloat**)mxMalloc(sizeof(gkFloat*) * (int)nCoordsA); + arr2 = (gkFloat**)mxMalloc(sizeof(gkFloat*) * (int)nCoordsB); - for (i = 0; i < nCoordsA; i++) arr1[i] = &inCoordsA[i * 3]; + for (i = 0; i < nCoordsA; i++) { + arr1[i] = &inCoordsA[i * 3]; + } - for (i = 0; i < nCoordsB; i++) arr2[i] = &inCoordsB[i * 3]; + for (i = 0; i < nCoordsB; i++) { + arr2[i] = &inCoordsB[i * 3]; + } /*----------------------------------------------------------------*/ /* POPULATE BODIES' STRUCTURES */ @@ -774,7 +867,8 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { /** * @brief Invoke this function from C# applications */ -gkFloat csFunction(int nCoordsA, gkFloat *inCoordsA, int nCoordsB, gkFloat *inCoordsB) { +gkFloat +csFunction(int nCoordsA, gkFloat* inCoordsA, int nCoordsB, gkFloat* inCoordsB) { gkFloat distance = 0; int i, j; @@ -788,17 +882,27 @@ gkFloat csFunction(int nCoordsA, gkFloat *inCoordsA, int nCoordsB, gkFloat *inCo bd1.numpoints = (int)nCoordsA; bd2.numpoints = (int)nCoordsB; - gkFloat **pinCoordsA = (gkFloat **)malloc(bd1.numpoints * sizeof(gkFloat *)); - for (i = 0; i < bd1.numpoints; i++) pinCoordsA[i] = (gkFloat *)malloc(3 * sizeof(gkFloat)); + gkFloat** pinCoordsA = (gkFloat**)malloc(bd1.numpoints * sizeof(gkFloat*)); + for (i = 0; i < bd1.numpoints; i++) { + pinCoordsA[i] = (gkFloat*)malloc(3 * sizeof(gkFloat)); + } - for (i = 0; i < 3; i++) - for (j = 0; j < bd1.numpoints; j++) pinCoordsA[j][i] = inCoordsA[i * bd1.numpoints + j]; + for (i = 0; i < 3; i++) { + for (j = 0; j < bd1.numpoints; j++) { + pinCoordsA[j][i] = inCoordsA[i * bd1.numpoints + j]; + } + } - gkFloat **pinCoordsB = (gkFloat **)malloc(bd2.numpoints * sizeof(gkFloat *)); - for (i = 0; i < bd2.numpoints; i++) pinCoordsB[i] = (gkFloat *)malloc(3 * sizeof(gkFloat)); + gkFloat** pinCoordsB = (gkFloat**)malloc(bd2.numpoints * sizeof(gkFloat*)); + for (i = 0; i < bd2.numpoints; i++) { + pinCoordsB[i] = (gkFloat*)malloc(3 * sizeof(gkFloat)); + } - for (i = 0; i < 3; i++) - for (j = 0; j < bd2.numpoints; j++) pinCoordsB[j][i] = inCoordsB[i * bd2.numpoints + j]; + for (i = 0; i < 3; i++) { + for (j = 0; j < bd2.numpoints; j++) { + pinCoordsB[j][i] = inCoordsB[i * bd2.numpoints + j]; + } + } bd1.coord = pinCoordsA; bd2.coord = pinCoordsB; @@ -813,10 +917,14 @@ gkFloat csFunction(int nCoordsA, gkFloat *inCoordsA, int nCoordsB, gkFloat *inCo /* Compute squared distance using GJK algorithm */ distance = compute_minimum_distance(bd1, bd2, &s); - for (i = 0; i < bd1.numpoints; i++) free(pinCoordsA[i]); + for (i = 0; i < bd1.numpoints; i++) { + free(pinCoordsA[i]); + } free(pinCoordsA); - for (i = 0; i < bd2.numpoints; i++) free(pinCoordsB[i]); + for (i = 0; i < bd2.numpoints; i++) { + free(pinCoordsB[i]); + } free(pinCoordsB); return distance;