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

View File

@ -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);

View File

@ -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
}

498
openGJK.c
View File

@ -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 <stdio.h>
@ -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;