Format all c and h files
Mattia Montanari
parent
d9a9bf2a4b
commit
66002145bd
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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
498
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 <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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue