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

28
examples/c/main.c
View File

@ -31,7 +31,8 @@
#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;
@ -50,20 +51,22 @@ 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);

18
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.
@ -46,18 +44,18 @@ extern "C"
/*! @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[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 s
gkFloat **coord; /*!< Coordinates of the points of the polytope. This is owned by user who manages and garbage-collects the memory for these coordinates. */ [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; } 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;

258
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,7 +36,7 @@
#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
@ -48,36 +57,51 @@
#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]; \
@ -128,18 +152,21 @@
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,66 +175,99 @@ 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
S1D(gkSimplex* s, gkFloat* v) {
gkFloat* s1p = s->vrtx[1]; gkFloat* s1p = s->vrtx[1];
gkFloat* s2p = s->vrtx[0]; gkFloat* s2p = s->vrtx[0];
@ -220,7 +280,8 @@ inline static void S1D(gkSimplex *s, gkFloat *v) {
} }
} }
inline static void S2D(gkSimplex *s, gkFloat *v) { inline static void
S2D(gkSimplex* s, gkFloat* v) {
gkFloat* s1p = s->vrtx[2]; gkFloat* s1p = s->vrtx[2];
gkFloat* s2p = s->vrtx[1]; gkFloat* s2p = s->vrtx[1];
gkFloat* s3p = s->vrtx[0]; gkFloat* s3p = s->vrtx[0];
@ -265,7 +326,8 @@ inline static void S2D(gkSimplex *s, gkFloat *v) {
} }
} }
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;
@ -318,14 +380,26 @@ inline static void S3D(gkSimplex *s, gkFloat *v) {
// 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++) {
s->vrtx[2][i] = s->vrtx[3][i];
}
} else if (!testPlaneThree) { // k = 1; // removes s3 } 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++) {
for (i = 0; i < 3; i++) s->vrtx[2][i] = s->vrtx[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 } 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++) {
for (i = 0; i < 3; i++) s->vrtx[1][i] = s2[i]; s->vrtx[0][i] = s3[i];
for (i = 0; i < 3; i++) 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];
}
} }
// Call S2D // Call S2D
S2D(s, v); S2D(s, v);
@ -392,7 +466,7 @@ 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);
@ -400,7 +474,7 @@ inline static void S3D(gkSimplex *s, gkFloat *v) {
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);
@ -410,7 +484,7 @@ inline static void S3D(gkSimplex *s, gkFloat *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);
@ -418,7 +492,7 @@ inline static void S3D(gkSimplex *s, gkFloat *v) {
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);
@ -547,7 +621,8 @@ 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;
@ -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,7 +662,8 @@ inline static void subalgorithm(gkSimplex *s, gkFloat *v) {
} }
} }
gkFloat compute_minimum_distance(gkPolytope bd1, gkPolytope bd2, gkSimplex *s) { gkFloat
compute_minimum_distance(gkPolytope bd1, gkPolytope bd2, gkSimplex* s) {
int k = 0; /**< Iteration counter */ int k = 0; /**< Iteration counter */
int i; /**< General purpose counter */ int i; /**< General purpose counter */
int mk = 25; /**< Maximum number of iterations of the GJK algorithm */ int mk = 25; /**< Maximum number of iterations of the GJK algorithm */
@ -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,8 +753,7 @@ 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");
} }
@ -676,7 +764,8 @@ 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
mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
gkFloat* inCoordsA; gkFloat* inCoordsA;
gkFloat* inCoordsB; gkFloat* inCoordsB;
size_t nCoordsA; size_t nCoordsA;
@ -740,9 +829,13 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
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;
@ -789,16 +883,26 @@ gkFloat csFunction(int nCoordsA, gkFloat *inCoordsA, int nCoordsB, gkFloat *inCo
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;