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

Format all c and h files

fixes-turtlebasket
Mattia Montanari 2023-02-13 14:35:05 +01:00
parent d9a9bf2a4b
commit 66002145bd
3 changed files with 341 additions and 227 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
examples/c/main.c
View File

@ -31,10 +31,11 @@
#define fscanf_s fscanf #define fscanf_s fscanf
/// @brief Function for reading input file with body's coordinates. /// @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 npoints = 0;
int idx = 0; int idx = 0;
FILE *fp; FILE* fp;
/* Open file. */ /* Open file. */
#ifdef WIN32 #ifdef WIN32
@ -50,19 +51,21 @@ int readinput(const char *inputfile, double ***pts, int *out) {
} }
/* Read number of input vertices. */ /* Read number of input vertices. */
if (fscanf_s(fp, "%d", &npoints) != 1) if (fscanf_s(fp, "%d", &npoints) != 1) {
return 1; return 1;
}
/* Allocate memory. */ /* Allocate memory. */
double **arr = (double **)malloc(npoints * sizeof(double *)); double** arr = (double**)malloc(npoints * sizeof(double*));
for (int i = 0; i < npoints; i++) for (int i = 0; i < npoints; i++) {
arr[i] = (double *)malloc(3 * sizeof(double)); arr[i] = (double*)malloc(3 * sizeof(double));
}
/* Read and store vertices' coordinates. */ /* Read and store vertices' coordinates. */
for (idx = 0; idx < npoints; idx++) { for (idx = 0; idx < npoints; idx++) {
if (fscanf_s(fp, "%lf %lf %lf\n", &arr[idx][0], &arr[idx][1], &arr[idx][2]) != if (fscanf_s(fp, "%lf %lf %lf\n", &arr[idx][0], &arr[idx][1], &arr[idx][2]) != 3) {
3)
return 1; return 1;
}
} }
fclose(fp); 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). * @brief Main program of example1_c (described in Section 3.1 of the paper).
* *
*/ */
int main() { int
main() {
/* Squared distance computed by openGJK. */ /* Squared distance computed by openGJK. */
double dd; double dd;
/* Structure of simplex used by openGJK. */ /* Structure of simplex used by openGJK. */
@ -96,14 +100,16 @@ int main() {
* two bodies that will be passed to the GJK procedure. */ * two bodies that will be passed to the GJK procedure. */
/* Import coordinates of object 1. */ /* Import coordinates of object 1. */
if (readinput(inputfileA, &vrtx1, &nvrtx1)) if (readinput(inputfileA, &vrtx1, &nvrtx1)) {
return (1); return (1);
}
bd1.coord = vrtx1; bd1.coord = vrtx1;
bd1.numpoints = nvrtx1; bd1.numpoints = nvrtx1;
/* Import coordinates of object 2. */ /* Import coordinates of object 2. */
if (readinput(inputfileB, &vrtx2, &nvrtx2)) if (readinput(inputfileB, &vrtx2, &nvrtx2)) {
return (1); return (1);
}
bd2.coord = vrtx2; bd2.coord = vrtx2;
bd2.numpoints = nvrtx2; bd2.numpoints = nvrtx2;
@ -118,11 +124,13 @@ int main() {
printf("Distance between bodies %f\n", dd); printf("Distance between bodies %f\n", dd);
/* Free memory */ /* 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[i]);
}
free(bd1.coord); 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[i]);
}
free(bd2.coord); free(bd2.coord);
return (0); return (0);

36
include/openGJK/openGJK.h
View File

@ -26,16 +26,14 @@
* @date 1 Jan 2022 * @date 1 Jan 2022
* @brief Main interface of OpenGJK containing quick reference and API documentation. * @brief Main interface of OpenGJK containing quick reference and API documentation.
* *
* More extensive explanation of what the header * @see https://www.mattiamontanari.com/opengjk/
* @see http://google.com
*/ */
#ifndef OPENGJK_H__ #ifndef OPENGJK_H__
#define OPENGJK_H__ #define OPENGJK_H__
#ifdef __cplusplus #ifdef __cplusplus
extern "C" extern "C" {
{
#endif #endif
/*! @brief Precision of floating-point numbers. /*! @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. */ * Default is set to 64-bit (Double). Change this to quickly play around with 16- and 32-bit. */
#define gkFloat double #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. */ * 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_ typedef struct gkPolytope_ {
{ int numpoints; /*!< Number of points defining the polytope. */
int numpoints; /*!< Number of points defining the polytope. */ gkFloat s
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. */ [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. */ gkFloat**
} gkPolytope; 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. */ * 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_ typedef struct gkSimplex_ {
{ int nvrtx; /*!< Number of points defining the simplex. */
int nvrtx; /*!< Number of points defining the simplex. */ gkFloat vrtx[4][3]; /*!< Coordinates of the points of the simplex. */
gkFloat vrtx[4][3]; /*!< Coordinates of the points of the simplex. */ } gkSimplex;
} 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. */ * 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 #ifdef __cplusplus
} }

498
openGJK.c
View File

@ -20,6 +20,15 @@
// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS // // ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS //
// FOR A PARTICULAR PURPOSE. See GNU General Public License for details. // // 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 "openGJK/openGJK.h"
#include <stdio.h> #include <stdio.h>
@ -27,119 +36,137 @@
#include "math.h" #include "math.h"
/* If instricuted, compile a mex function for Matlab. */ /** If instricuted, compile a mex function for Matlab. */
#ifdef MATLAB_MEX_BUILD #ifdef MATLAB_MEX_BUILD
#include "mex.h" #include "mex.h"
#else #else
#define mexPrintf printf #define mexPrintf printf
#endif #endif
#define eps_rel22 1e-10 #define eps_rel22 1e-10
#define eps_tot22 1e-12 #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 dotProduct(a, b) (a[0] * b[0] + a[1] * b[1] + a[2] * b[2])
#define S3Dregion1234() \ #define S3Dregion1234() \
v[0] = 0; \ v[0] = 0; \
v[1] = 0; \ v[1] = 0; \
v[2] = 0; \ v[2] = 0; \
s->nvrtx = 4; s->nvrtx = 4;
#define select_1ik() \ #define select_1ik() \
s->nvrtx = 3; \ s->nvrtx = 3; \
for (t = 0; t < 3; t++) s->vrtx[2][t] = s->vrtx[3][t]; \ for (t = 0; t < 3; t++) \
for (t = 0; t < 3; t++) s->vrtx[1][t] = si[t]; \ s->vrtx[2][t] = s->vrtx[3][t]; \
for (t = 0; t < 3; t++) s->vrtx[0][t] = sk[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() \ #define select_1ij() \
s->nvrtx = 3; \ s->nvrtx = 3; \
for (t = 0; t < 3; t++) s->vrtx[2][t] = s->vrtx[3][t]; \ for (t = 0; t < 3; t++) \
for (t = 0; t < 3; t++) s->vrtx[1][t] = si[t]; \ s->vrtx[2][t] = s->vrtx[3][t]; \
for (t = 0; t < 3; t++) s->vrtx[0][t] = sj[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() \ #define select_1jk() \
s->nvrtx = 3; \ s->nvrtx = 3; \
for (t = 0; t < 3; t++) s->vrtx[2][t] = s->vrtx[3][t]; \ for (t = 0; t < 3; t++) \
for (t = 0; t < 3; t++) s->vrtx[1][t] = sj[t]; \ s->vrtx[2][t] = s->vrtx[3][t]; \
for (t = 0; t < 3; t++) s->vrtx[0][t] = sk[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() \ #define select_1i() \
s->nvrtx = 2; \ s->nvrtx = 2; \
for (t = 0; t < 3; t++) s->vrtx[1][t] = s->vrtx[3][t]; \ for (t = 0; t < 3; t++) \
for (t = 0; t < 3; t++) s->vrtx[0][t] = si[t]; s->vrtx[1][t] = s->vrtx[3][t]; \
for (t = 0; t < 3; t++) \
s->vrtx[0][t] = si[t];
#define select_1j() \ #define select_1j() \
s->nvrtx = 2; \ s->nvrtx = 2; \
for (t = 0; t < 3; t++) s->vrtx[1][t] = s->vrtx[3][t]; \ for (t = 0; t < 3; t++) \
for (t = 0; t < 3; t++) s->vrtx[0][t] = sj[t]; s->vrtx[1][t] = s->vrtx[3][t]; \
for (t = 0; t < 3; t++) \
s->vrtx[0][t] = sj[t];
#define select_1k() \ #define select_1k() \
s->nvrtx = 2; \ s->nvrtx = 2; \
for (t = 0; t < 3; t++) s->vrtx[1][t] = s->vrtx[3][t]; \ for (t = 0; t < 3; t++) \
for (t = 0; t < 3; t++) s->vrtx[0][t] = sk[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) \ #define getvrtx(point, location) \
point[0] = s->vrtx[location][0]; \ point[0] = s->vrtx[location][0]; \
point[1] = s->vrtx[location][1]; \ point[1] = s->vrtx[location][1]; \
point[2] = s->vrtx[location][2]; point[2] = s->vrtx[location][2];
#define calculateEdgeVector(p1p2, p2) \ #define calculateEdgeVector(p1p2, p2) \
p1p2[0] = p2[0] - s->vrtx[3][0]; \ p1p2[0] = p2[0] - s->vrtx[3][0]; \
p1p2[1] = p2[1] - s->vrtx[3][1]; \ p1p2[1] = p2[1] - s->vrtx[3][1]; \
p1p2[2] = p2[2] - s->vrtx[3][2]; p1p2[2] = p2[2] - s->vrtx[3][2];
#define S1Dregion1() \ #define S1Dregion1() \
v[0] = s->vrtx[1][0]; \ v[0] = s->vrtx[1][0]; \
v[1] = s->vrtx[1][1]; \ v[1] = s->vrtx[1][1]; \
v[2] = s->vrtx[1][2]; \ v[2] = s->vrtx[1][2]; \
s->nvrtx = 1; \ s->nvrtx = 1; \
s->vrtx[0][0] = s->vrtx[1][0]; \ s->vrtx[0][0] = s->vrtx[1][0]; \
s->vrtx[0][1] = s->vrtx[1][1]; \ s->vrtx[0][1] = s->vrtx[1][1]; \
s->vrtx[0][2] = s->vrtx[1][2]; s->vrtx[0][2] = s->vrtx[1][2];
#define S2Dregion1() \ #define S2Dregion1() \
v[0] = s->vrtx[2][0]; \ v[0] = s->vrtx[2][0]; \
v[1] = s->vrtx[2][1]; \ v[1] = s->vrtx[2][1]; \
v[2] = s->vrtx[2][2]; \ v[2] = s->vrtx[2][2]; \
s->nvrtx = 1; \ s->nvrtx = 1; \
s->vrtx[0][0] = s->vrtx[2][0]; \ s->vrtx[0][0] = s->vrtx[2][0]; \
s->vrtx[0][1] = s->vrtx[2][1]; \ s->vrtx[0][1] = s->vrtx[2][1]; \
s->vrtx[0][2] = s->vrtx[2][2]; s->vrtx[0][2] = s->vrtx[2][2];
#define S2Dregion12() \ #define S2Dregion12() \
s->nvrtx = 2; \ s->nvrtx = 2; \
s->vrtx[0][0] = s->vrtx[2][0]; \ s->vrtx[0][0] = s->vrtx[2][0]; \
s->vrtx[0][1] = s->vrtx[2][1]; \ s->vrtx[0][1] = s->vrtx[2][1]; \
s->vrtx[0][2] = s->vrtx[2][2]; s->vrtx[0][2] = s->vrtx[2][2];
#define S2Dregion13() \ #define S2Dregion13() \
s->nvrtx = 2; \ s->nvrtx = 2; \
s->vrtx[1][0] = s->vrtx[2][0]; \ s->vrtx[1][0] = s->vrtx[2][0]; \
s->vrtx[1][1] = s->vrtx[2][1]; \ s->vrtx[1][1] = s->vrtx[2][1]; \
s->vrtx[1][2] = s->vrtx[2][2]; s->vrtx[1][2] = s->vrtx[2][2];
#define S3Dregion1() \ #define S3Dregion1() \
v[0] = s1[0]; \ v[0] = s1[0]; \
v[1] = s1[1]; \ v[1] = s1[1]; \
v[2] = s1[2]; \ v[2] = s1[2]; \
s->nvrtx = 1; \ s->nvrtx = 1; \
s->vrtx[0][0] = s1[0]; \ s->vrtx[0][0] = s1[0]; \
s->vrtx[0][1] = s1[1]; \ s->vrtx[0][1] = s1[1]; \
s->vrtx[0][2] = s1[2]; s->vrtx[0][2] = s1[2];
inline static gkFloat determinant(const gkFloat *p, const gkFloat *q, const gkFloat *r) { inline static gkFloat
return p[0] * ((q[1] * r[2]) - (r[1] * q[2])) - p[1] * (q[0] * r[2] - r[0] * q[2]) + determinant(const gkFloat* p, const gkFloat* q, const gkFloat* r) {
p[2] * (q[0] * r[1] - r[0] * q[1]); 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[0] = a[1] * b[2] - a[2] * b[1];
c[1] = a[2] * b[0] - a[0] * b[2]; c[1] = a[2] * b[0] - a[0] * b[2];
c[2] = a[0] * b[1] - a[1] * b[0]; 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 pq[3];
gkFloat tmp; gkFloat tmp;
pq[0] = p[0] - q[0]; 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); 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 n[3], pq[3], pr[3];
gkFloat tmp; gkFloat tmp;
for (int i = 0; i < 3; i++) pq[i] = p[i] - q[i]; for (int i = 0; i < 3; i++) {
for (int i = 0; i < 3; i++) pr[i] = p[i] - r[i]; pq[i] = p[i] - q[i];
}
for (int i = 0; i < 3; i++) {
pr[i] = p[i] - r[i];
}
crossProduct(pq, pr, n); crossProduct(pq, pr, n);
tmp = dotProduct(n, p) / dotProduct(n, 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; 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; 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 ntmp[3];
gkFloat n[3], pq[3], pr[3]; gkFloat n[3], pq[3], pr[3];
gkFloat tmp = 0; gkFloat tmp = 0;
for (int i = 0; i < 3; i++) pq[i] = q[i] - p[i]; for (int i = 0; i < 3; i++) {
for (int i = 0; i < 3; i++) pr[i] = r[i] - p[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, pr, ntmp);
crossProduct(pq, ntmp, n); 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; 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 n[3], pq[3], pr[3];
gkFloat tmp = 0; gkFloat tmp = 0;
for (int i = 0; i < 3; i++) pq[i] = q[i] - p[i]; for (int i = 0; i < 3; i++) {
for (int i = 0; i < 3; i++) pr[i] = r[i] - p[i]; pq[i] = q[i] - p[i];
}
for (int i = 0; i < 3; i++) {
pr[i] = r[i] - p[i];
}
crossProduct(pq, pr, n); 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; return 1;
} }
inline static void S1D(gkSimplex *s, gkFloat *v) { inline static void
gkFloat *s1p = s->vrtx[1]; S1D(gkSimplex* s, gkFloat* v) {
gkFloat *s2p = s->vrtx[0]; gkFloat* s1p = s->vrtx[1];
gkFloat* s2p = s->vrtx[0];
if (hff1(s1p, s2p)) { if (hff1(s1p, s2p)) {
projectOnLine(s1p, s2p, v); // Update v, no need to update s projectOnLine(s1p, s2p, v); // Update v, no need to update s
return; // Return V{1,2} return; // Return V{1,2}
} else { } else {
S1Dregion1(); // Update v and s S1Dregion1(); // Update v and s
return; // Return V{1} return; // Return V{1}
} }
} }
inline static void S2D(gkSimplex *s, gkFloat *v) { inline static void
gkFloat *s1p = s->vrtx[2]; S2D(gkSimplex* s, gkFloat* v) {
gkFloat *s2p = s->vrtx[1]; gkFloat* s1p = s->vrtx[2];
gkFloat *s3p = s->vrtx[0]; gkFloat* s2p = s->vrtx[1];
gkFloat* s3p = s->vrtx[0];
int hff1f_s12 = hff1(s1p, s2p); int hff1f_s12 = hff1(s1p, s2p);
int hff1f_s13 = hff1(s1p, s3p); int hff1f_s13 = hff1(s1p, s3p);
@ -233,39 +294,40 @@ inline static void S2D(gkSimplex *s, gkFloat *v) {
if (hff1f_s13) { if (hff1f_s13) {
int hff2f_32 = !hff2(s1p, s3p, s2p); int hff2f_32 = !hff2(s1p, s3p, s2p);
if (hff2f_32) { if (hff2f_32) {
projectOnPlane(s1p, s2p, s3p, v); // Update s, no need to update c projectOnPlane(s1p, s2p, s3p, v); // Update s, no need to update c
return; // Return V{1,2,3} return; // Return V{1,2,3}
} else { } else {
projectOnLine(s1p, s3p, v); // Update v projectOnLine(s1p, s3p, v); // Update v
S2Dregion13(); // Update s S2Dregion13(); // Update s
return; // Return V{1,3} return; // Return V{1,3}
} }
} else { } else {
projectOnPlane(s1p, s2p, s3p, v); // Update s, no need to update c projectOnPlane(s1p, s2p, s3p, v); // Update s, no need to update c
return; // Return V{1,2,3} return; // Return V{1,2,3}
} }
} else { } else {
projectOnLine(s1p, s2p, v); // Update v projectOnLine(s1p, s2p, v); // Update v
S2Dregion12(); // Update s S2Dregion12(); // Update s
return; // Return V{1,2} return; // Return V{1,2}
} }
} else if (hff1f_s13) { } else if (hff1f_s13) {
int hff2f_32 = !hff2(s1p, s3p, s2p); int hff2f_32 = !hff2(s1p, s3p, s2p);
if (hff2f_32) { if (hff2f_32) {
projectOnPlane(s1p, s2p, s3p, v); // Update s, no need to update v projectOnPlane(s1p, s2p, s3p, v); // Update s, no need to update v
return; // Return V{1,2,3} return; // Return V{1,2,3}
} else { } else {
projectOnLine(s1p, s3p, v); // Update v projectOnLine(s1p, s3p, v); // Update v
S2Dregion13(); // Update s S2Dregion13(); // Update s
return; // Return V{1,3} return; // Return V{1,3}
} }
} else { } else {
S2Dregion1(); // Update s and v S2Dregion1(); // Update s and v
return; // Return V{1} 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 s1[3], s2[3], s3[3], s4[3], s1s2[3], s1s3[3], s1s4[3];
gkFloat si[3], sj[3], sk[3]; gkFloat si[3], sj[3], sk[3];
int testLineThree, testLineFour, testPlaneTwo, testPlaneThree, testPlaneFour, dotTotal; 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 // 1,i,j, are the indices of the points on the triangle and remove k from
// simplex // simplex
s->nvrtx = 3; s->nvrtx = 3;
if (!testPlaneTwo) { // k = 2; removes s2 if (!testPlaneTwo) { // k = 2; removes s2
for (i = 0; i < 3; i++) s->vrtx[2][i] = s->vrtx[3][i]; for (i = 0; i < 3; i++) {
} else if (!testPlaneThree) { // k = 1; // removes s3 s->vrtx[2][i] = s->vrtx[3][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]; } else if (!testPlaneThree) { // k = 1; // removes s3
} else if (!testPlaneFour) { // k = 0; // removes s4 and no need to reorder for (i = 0; i < 3; i++) {
for (i = 0; i < 3; i++) s->vrtx[0][i] = s3[i]; s->vrtx[1][i] = s2[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]; 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 // Call S2D
S2D(s, v); S2D(s, v);
@ -339,15 +413,15 @@ inline static void S3D(gkSimplex *s, gkFloat *v) {
// simplex // simplex
s->nvrtx = 3; s->nvrtx = 3;
if (testPlaneTwo) { if (testPlaneTwo) {
k = 2; // s2 k = 2; // s2
i = 1; i = 1;
j = 0; j = 0;
} else if (testPlaneThree) { } else if (testPlaneThree) {
k = 1; // s3 k = 1; // s3
i = 0; i = 0;
j = 2; j = 2;
} else { } else {
k = 0; // s4 k = 0; // s4
i = 2; i = 2;
j = 1; j = 1;
} }
@ -365,7 +439,7 @@ inline static void S3D(gkSimplex *s, gkFloat *v) {
select_1jk(); select_1jk();
projectOnPlane(s1, sj, sk, v); projectOnPlane(s1, sj, sk, v);
} else { } else {
select_1k(); // select region 1i select_1k(); // select region 1i
projectOnLine(s1, sk, v); projectOnLine(s1, sk, v);
} }
} else if (hff1_tests[i]) { } else if (hff1_tests[i]) {
@ -373,7 +447,7 @@ inline static void S3D(gkSimplex *s, gkFloat *v) {
select_1ik(); select_1ik();
projectOnPlane(s1, si, sk, v); projectOnPlane(s1, si, sk, v);
} else { } else {
select_1i(); // select region 1i select_1i(); // select region 1i
projectOnLine(s1, si, v); projectOnLine(s1, si, v);
} }
} else { } else {
@ -381,7 +455,7 @@ inline static void S3D(gkSimplex *s, gkFloat *v) {
select_1jk(); select_1jk();
projectOnPlane(s1, sj, sk, v); projectOnPlane(s1, sj, sk, v);
} else { } else {
select_1j(); // select region 1i select_1j(); // select region 1i
projectOnLine(s1, sj, v); projectOnLine(s1, sj, v);
} }
} }
@ -392,38 +466,38 @@ inline static void S3D(gkSimplex *s, gkFloat *v) {
// hff1_1k is positive // hff1_1k is positive
if (hff1_tests[i]) { if (hff1_tests[i]) {
if (!hff2(s1, sk, si)) if (!hff2(s1, sk, si)) {
if (!hff2(s1, si, sk)) { if (!hff2(s1, si, sk)) {
select_1ik(); // select region 1ik select_1ik(); // select region 1ik
projectOnPlane(s1, si, sk, v); projectOnPlane(s1, si, sk, v);
} else { } else {
select_1k(); // select region 1k select_1k(); // select region 1k
projectOnLine(s1, sk, v); projectOnLine(s1, sk, v);
} }
else { } else {
if (!hff2(s1, sk, sj)) { if (!hff2(s1, sk, sj)) {
select_1jk(); // select region 1jk select_1jk(); // select region 1jk
projectOnPlane(s1, sj, sk, v); projectOnPlane(s1, sj, sk, v);
} else { } else {
select_1k(); // select region 1k select_1k(); // select region 1k
projectOnLine(s1, sk, v); projectOnLine(s1, sk, v);
} }
} }
} else if (hff1_tests[j]) { // there is no other choice } else if (hff1_tests[j]) { // there is no other choice
if (!hff2(s1, sk, sj)) if (!hff2(s1, sk, sj)) {
if (!hff2(s1, sj, sk)) { if (!hff2(s1, sj, sk)) {
select_1jk(); // select region 1jk select_1jk(); // select region 1jk
projectOnPlane(s1, sj, sk, v); projectOnPlane(s1, sj, sk, v);
} else { } else {
select_1j(); // select region 1j select_1j(); // select region 1j
projectOnLine(s1, sj, v); projectOnLine(s1, sj, v);
} }
else { } else {
if (!hff2(s1, sk, si)) { if (!hff2(s1, sk, si)) {
select_1ik(); // select region 1ik select_1ik(); // select region 1ik
projectOnPlane(s1, si, sk, v); projectOnPlane(s1, si, sk, v);
} else { } else {
select_1k(); // select region 1k select_1k(); // select region 1k
projectOnLine(s1, sk, v); 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 // Here si is set such that hff(s1,si) > 0
if (testLineThree) { if (testLineThree) {
k = 2; k = 2;
i = 1; // s3 i = 1; // s3
j = 0; j = 0;
} else if (testLineFour) { } else if (testLineFour) {
k = 1; // s3 k = 1; // s3
i = 0; i = 0;
j = 2; j = 2;
} else { } else {
k = 0; k = 0;
i = 2; // s2 i = 2; // s2
j = 1; j = 1;
} }
getvrtx(si, i); getvrtx(si, i);
@ -507,15 +581,15 @@ inline static void S3D(gkSimplex *s, gkFloat *v) {
s->nvrtx = 3; s->nvrtx = 3;
if (!testLineThree) { if (!testLineThree) {
k = 2; k = 2;
i = 1; // s3 i = 1; // s3
j = 0; j = 0;
} else if (!testLineFour) { } else if (!testLineFour) {
k = 1; k = 1;
i = 0; // s4 i = 0; // s4
j = 2; j = 2;
} else { } else {
k = 0; k = 0;
i = 2; // s2 i = 2; // s2
j = 1; j = 1;
} }
getvrtx(si, i); getvrtx(si, i);
@ -524,7 +598,7 @@ inline static void S3D(gkSimplex *s, gkFloat *v) {
if (!hff2(s1, sj, sk)) { if (!hff2(s1, sj, sk)) {
if (!hff2(s1, sk, sj)) { if (!hff2(s1, sk, sj)) {
select_1jk(); // select region 1jk select_1jk(); // select region 1jk
projectOnPlane(s1, sj, sk, v); projectOnPlane(s1, sj, sk, v);
} else if (!hff2(s1, sk, si)) { } else if (!hff2(s1, sk, si)) {
select_1ik(); 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 s, maxs;
gkFloat *vrt; gkFloat* vrt;
int better = -1; int better = -1;
maxs = dotProduct(body->s, v); 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) { switch (s->nvrtx) {
case 4: case 4:
S3D(s, v); 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) { gkFloat
int k = 0; /**< Iteration counter */ compute_minimum_distance(gkPolytope bd1, gkPolytope bd2, gkSimplex* s) {
int i; /**< General purpose counter */ int k = 0; /**< Iteration counter */
int mk = 25; /**< Maximum number of iterations of the GJK algorithm */ int i; /**< General purpose counter */
int mk = 25; /**< Maximum number of iterations of the GJK algorithm */
int absTestin; int absTestin;
gkFloat norm2Wmax = 0; gkFloat norm2Wmax = 0;
gkFloat tesnorm; 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_rel = eps_rel22; /**< Tolerance on relative */
gkFloat eps_rel2 = eps_rel * eps_rel; gkFloat eps_rel2 = eps_rel * eps_rel;
gkFloat eps_tot = eps_tot22; gkFloat eps_tot = eps_tot22;
gkFloat exeedtol_rel; /**< Test for 1st exit condition */ gkFloat exeedtol_rel; /**< Test for 1st exit condition */
int nullV = 0; int nullV = 0;
/* Initialise search direction */ /* Initialise search direction */
@ -610,23 +687,33 @@ gkFloat compute_minimum_distance(gkPolytope bd1, gkPolytope bd2, gkSimplex *s) {
/* Inialise simplex */ /* Inialise simplex */
s->nvrtx = 1; 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 */ /* Begin GJK iteration */
do { do {
k++; k++;
/* Update negative search direction */ /* 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 function */
support(&bd1, vminus); support(&bd1, vminus);
support(&bd2, v); 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 /* Test first exit condition (new point already in simplex/can't move
* further) */ * further) */
@ -642,7 +729,9 @@ gkFloat compute_minimum_distance(gkPolytope bd1, gkPolytope bd2, gkSimplex *s) {
/* Add new vertex to simplex */ /* Add new vertex to simplex */
i = s->nvrtx; 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++; s->nvrtx++;
/* Invoke distance sub-algorithm */ /* Invoke distance sub-algorithm */
@ -664,9 +753,8 @@ gkFloat compute_minimum_distance(gkPolytope bd1, gkPolytope bd2, gkSimplex *s) {
} while ((s->nvrtx != 4) && (k != mk)); } while ((s->nvrtx != 4) && (k != mk));
if (k == mk) { if (k == mk) {
mexPrintf( mexPrintf("\n * * * * * * * * * * * * MAXIMUM ITERATION NUMBER REACHED!!! "
"\n * * * * * * * * * * * * MAXIMUM ITERATION NUMBER REACHED!!! " " * * * * * * * * * * * * * * \n");
" * * * * * * * * * * * * * * \n");
} }
return sqrt(norm2(v)); return sqrt(norm2(v));
@ -676,17 +764,18 @@ gkFloat compute_minimum_distance(gkPolytope bd1, gkPolytope bd2, gkSimplex *s) {
/** /**
* @brief Mex function for Matlab. * @brief Mex function for Matlab.
*/ */
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { void
gkFloat *inCoordsA; mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
gkFloat *inCoordsB; gkFloat* inCoordsA;
gkFloat* inCoordsB;
size_t nCoordsA; size_t nCoordsA;
size_t nCoordsB; size_t nCoordsB;
int i; int i;
gkFloat *distance; gkFloat* distance;
int c = 3; int c = 3;
int count = 0; int count = 0;
gkFloat **arr1; gkFloat** arr1;
gkFloat **arr2; gkFloat** arr2;
/**************** PARSE INPUTS AND OUTPUTS **********************/ /**************** PARSE INPUTS AND OUTPUTS **********************/
/*----------------------------------------------------------------*/ /*----------------------------------------------------------------*/
@ -737,12 +826,16 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
distance = mxGetPr(plhs[0]); distance = mxGetPr(plhs[0]);
/* Copy data from Matlab's vectors into two new arrays */ /* Copy data from Matlab's vectors into two new arrays */
arr1 = (gkFloat **)mxMalloc(sizeof(gkFloat *) * (int)nCoordsA); arr1 = (gkFloat**)mxMalloc(sizeof(gkFloat*) * (int)nCoordsA);
arr2 = (gkFloat **)mxMalloc(sizeof(gkFloat *) * (int)nCoordsB); 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 */ /* 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 * @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; gkFloat distance = 0;
int i, j; int i, j;
@ -788,17 +882,27 @@ gkFloat csFunction(int nCoordsA, gkFloat *inCoordsA, int nCoordsB, gkFloat *inCo
bd1.numpoints = (int)nCoordsA; bd1.numpoints = (int)nCoordsA;
bd2.numpoints = (int)nCoordsB; bd2.numpoints = (int)nCoordsB;
gkFloat **pinCoordsA = (gkFloat **)malloc(bd1.numpoints * 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 < bd1.numpoints; i++) {
pinCoordsA[i] = (gkFloat*)malloc(3 * sizeof(gkFloat));
}
for (i = 0; i < 3; i++) for (i = 0; i < 3; i++) {
for (j = 0; j < bd1.numpoints; j++) pinCoordsA[j][i] = inCoordsA[i * bd1.numpoints + j]; for (j = 0; j < bd1.numpoints; j++) {
pinCoordsA[j][i] = inCoordsA[i * bd1.numpoints + j];
}
}
gkFloat **pinCoordsB = (gkFloat **)malloc(bd2.numpoints * 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 < bd2.numpoints; i++) {
pinCoordsB[i] = (gkFloat*)malloc(3 * sizeof(gkFloat));
}
for (i = 0; i < 3; i++) for (i = 0; i < 3; i++) {
for (j = 0; j < bd2.numpoints; j++) pinCoordsB[j][i] = inCoordsB[i * bd2.numpoints + j]; for (j = 0; j < bd2.numpoints; j++) {
pinCoordsB[j][i] = inCoordsB[i * bd2.numpoints + j];
}
}
bd1.coord = pinCoordsA; bd1.coord = pinCoordsA;
bd2.coord = pinCoordsB; bd2.coord = pinCoordsB;
@ -813,10 +917,14 @@ gkFloat csFunction(int nCoordsA, gkFloat *inCoordsA, int nCoordsB, gkFloat *inCo
/* Compute squared distance using GJK algorithm */ /* Compute squared distance using GJK algorithm */
distance = compute_minimum_distance(bd1, bd2, &s); 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); free(pinCoordsA);
for (i = 0; i < bd2.numpoints; i++) free(pinCoordsB[i]); for (i = 0; i < bd2.numpoints; i++) {
free(pinCoordsB[i]);
}
free(pinCoordsB); free(pinCoordsB);
return distance; return distance;