Format all c and h files
Mattia Montanari
parent
d9a9bf2a4b
commit
66002145bd
|
|
@ -31,7 +31,8 @@
|
|||
#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;
|
||||
|
|
@ -50,20 +51,22 @@ 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++)
|
||||
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.
|
||||
|
|
@ -46,18 +44,18 @@ extern "C"
|
|||
/*! @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_
|
||||
{
|
||||
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. */
|
||||
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.
|
||||
*
|
||||
* 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. */
|
||||
gkFloat vrtx[4][3]; /*!< Coordinates of the points of the simplex. */
|
||||
} gkSimplex;
|
||||
|
|
|
|||
258
openGJK.c
258
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,7 +36,7 @@
|
|||
|
||||
#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
|
||||
|
|
@ -48,36 +57,51 @@
|
|||
|
||||
#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];
|
||||
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];
|
||||
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];
|
||||
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];
|
||||
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];
|
||||
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];
|
||||
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]; \
|
||||
|
|
@ -128,18 +152,21 @@
|
|||
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,66 +175,99 @@ 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) {
|
||||
inline static void
|
||||
S1D(gkSimplex* s, gkFloat* v) {
|
||||
gkFloat* s1p = s->vrtx[1];
|
||||
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* s2p = s->vrtx[1];
|
||||
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 si[3], sj[3], sk[3];
|
||||
int testLineThree, testLineFour, testPlaneTwo, testPlaneThree, testPlaneFour, dotTotal;
|
||||
|
|
@ -318,14 +380,26 @@ inline static void S3D(gkSimplex *s, gkFloat *v) {
|
|||
// simplex
|
||||
s->nvrtx = 3;
|
||||
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
|
||||
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[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];
|
||||
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);
|
||||
|
|
@ -392,7 +466,7 @@ 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
|
||||
projectOnPlane(s1, si, sk, v);
|
||||
|
|
@ -400,7 +474,7 @@ inline static void S3D(gkSimplex *s, gkFloat *v) {
|
|||
select_1k(); // select region 1k
|
||||
projectOnLine(s1, sk, v);
|
||||
}
|
||||
else {
|
||||
} else {
|
||||
if (!hff2(s1, sk, sj)) {
|
||||
select_1jk(); // select region 1jk
|
||||
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
|
||||
if (!hff2(s1, sk, sj))
|
||||
if (!hff2(s1, sk, sj)) {
|
||||
if (!hff2(s1, sj, sk)) {
|
||||
select_1jk(); // select region 1jk
|
||||
projectOnPlane(s1, sj, sk, v);
|
||||
|
|
@ -418,7 +492,7 @@ inline static void S3D(gkSimplex *s, gkFloat *v) {
|
|||
select_1j(); // select region 1j
|
||||
projectOnLine(s1, sj, v);
|
||||
}
|
||||
else {
|
||||
} else {
|
||||
if (!hff2(s1, sk, si)) {
|
||||
select_1ik(); // select region 1ik
|
||||
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* vrt;
|
||||
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) {
|
||||
case 4:
|
||||
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 i; /**< General purpose counter */
|
||||
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 */
|
||||
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,8 +753,7 @@ 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!!! "
|
||||
mexPrintf("\n * * * * * * * * * * * * MAXIMUM ITERATION NUMBER REACHED!!! "
|
||||
" * * * * * * * * * * * * * * \n");
|
||||
}
|
||||
|
||||
|
|
@ -676,7 +764,8 @@ 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[]) {
|
||||
void
|
||||
mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
|
||||
gkFloat* inCoordsA;
|
||||
gkFloat* inCoordsB;
|
||||
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);
|
||||
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;
|
||||
|
||||
|
|
@ -789,16 +883,26 @@ gkFloat csFunction(int nCoordsA, gkFloat *inCoordsA, int nCoordsB, gkFloat *inCo
|
|||
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));
|
||||
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));
|
||||
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