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

947 lines
28 KiB
C
Raw Blame History

/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
* ##### # # # *
* #### ##### ###### # # # # # # # *
* # # # # # ## # # # # # *
* # # # # ##### # # # # #### # ### *
* # # ##### # # # # # # # # # # *
* # # # # # ## # # # # # # *
* #### # ###### # # ##### ##### # # *
* *
* This file is part of openGJK. *
* *
* openGJK is free software: you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation, either version 3 of the License, or *
* any later version. *
* *
* openGJK is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See The *
* GNU General Public License for more details. *
* *
* You should have received a copy of the GNU General Public License *
* along with Foobar. If not, see <https://www.gnu.org/licenses/>. *
* *
* openGJK: open-source Gilbert-Johnson-Keerthi algorithm *
* Copyright (C) Mattia Montanari 2018 - 2019 *
* http://iel.eng.ox.ac.uk/?page_id=504 *
* *
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
#include "openGJK.h"
/* If instricuted, compile a mex function for Matlab. */
#ifdef MATLABDOESMEXSTUFF
#include "mex.h"
#else
#define mexPrintf printf
#endif
#define eps_rel22 1e-5
#define eps_tot22 1e-14
/* Select distance sub-algorithm */
#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;\
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_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_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_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];\
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];\
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];\
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];\
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];\
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];\
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];\
s->vrtx[0][2] = s1[2];
inline static double determinant(const double *p, const double *q, const double *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 double *a, const double *b, double *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 double *p, const double *q, double *v)
{
double pq[3];
double tmp;
pq[0] = p[0] - q[0];
pq[1] = p[1] - q[1];
pq[2] = p[2] - q[2];
tmp = dotProduct(p, pq) / dotProduct(pq, pq);
for (int i = 0; i < 3; i++)
v[i] = p[i] - pq[i] * tmp;
}
inline static void projectOnPlane(const double *p, const double *q, const double *r, double *v)
{
double n[3], pq[3], pr[3];
double 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];
crossProduct(pq, pr, n);
tmp = dotProduct(n, p) / dotProduct(n, n);
for (int i = 0; i < 3; i++)
v[i] = n[i] * tmp;
}
inline static int hff1(const double *p, const double *q)
{
double tmp = 0;
#pragma omp simd reduction(+:tmp)
for (int i = 0; i < 3; i++)
tmp += (p[i] * p[i] - p[i] * q[i]);
if (tmp > 0)
return 1; // keep q
return 0;
}
inline static int hff2(const double *p, const double *q, const double *r)
{
double ntmp[3];
double n[3], pq[3], pr[3];
double 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];
crossProduct(pq, pr, ntmp);
crossProduct(pq, ntmp, n);
#pragma omp simd reduction(+:tmp)
for (int i = 0; i < 3; i++)
tmp = tmp + (p[i] * n[i]);
if (tmp < 0)
return 1; // Discard r
return 0;
}
inline static int hff3(const double *p, const double *q, const double *r)
{
double n[3], pq[3], pr[3];
double 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];
crossProduct(pq, pr, n);
#pragma omp simd reduction(+:tmp)
for (int i = 0; i < 3; i++)
tmp = tmp + (p[i] * n[i]);
if (tmp > 0)
return 0; // discard s
return 1;
}
inline static void S1D(struct simplex * s, double *v)
{
double *s1p = s->vrtx[1];
double *s2p = s->vrtx[0];
if (hff1(s1p, s2p)) {
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}
}
}
inline static void S2D(struct simplex * s, double *v)
{
double *s1p = s->vrtx[2];
double *s2p = s->vrtx[1];
double *s3p = s->vrtx[0];
int hff1f_s12 = hff1(s1p, s2p);
int hff1f_s13 = hff1(s1p, s3p);
int hff2f_23 = !hff2(s1p, s2p, s3p);
int hff2f_32 = !hff2(s1p, s3p, s2p);
if (hff1f_s12) {
if (hff2f_23) {
if (hff1f_s13) {
if (hff2f_32) {
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}
}
}
else
{
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}
}
}
else if (hff1f_s13) {
if (hff2f_32) {
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}
}
}
else {
S2Dregion1(); // Update s and v
return; // Return V{1}
}
}
inline static void S3D(struct simplex * s, double *v) {
double s1[3], s2[3], s3[3], s4[3], s1s2[3], s1s3[3], s1s4[3];
double si[3], sj[3], sk[3];
int testLineThree, testLineFour, testPlaneTwo, testPlaneThree, testPlaneFour, dotTotal;
int i, j, k, t;
getvrtx(s1, 3);
getvrtx(s2, 2);
getvrtx(s3, 1);
getvrtx(s4, 0);
calculateEdgeVector(s1s2, s2);
calculateEdgeVector(s1s3, s3);
calculateEdgeVector(s1s4, s4);
int hff1_tests[3];
hff1_tests[2] = hff1(s1, s2);
hff1_tests[1] = hff1(s1, s3);
hff1_tests[0] = hff1(s1, s4);
testLineThree = hff1(s1, s3);
testLineFour = hff1(s1, s4);
dotTotal = hff1(s1, s2) + testLineThree + testLineFour;
if (dotTotal == 0) { /* case 0.0 -------------------------------------- */
S3Dregion1();
return;
}
double det134 = determinant(s1s3, s1s4, s1s2);
int sss;
if (det134 > 0) {
sss = 0;
}
else {
sss = 1;
}
testPlaneTwo = hff3(s1, s3, s4) - sss;
testPlaneTwo = testPlaneTwo * testPlaneTwo;
testPlaneThree = hff3(s1, s4, s2) - sss;
testPlaneThree = testPlaneThree * testPlaneThree;
testPlaneFour = hff3(s1, s2, s3) - sss;
testPlaneFour = testPlaneFour * testPlaneFour;
switch (testPlaneTwo + testPlaneThree + testPlaneFour) {
case 3:
S3Dregion1234();
break;
case 2:
// Only one facing the oring
// 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];
}
// Call S2D
S2D(s, v);
break;
case 1:
// Two triangles face the origins:
// The only positive hff3 is for triangle 1,i,j, therefore k must be in the solution as it supports the the point of minimum norm.
// 1,i,j, are the indices of the points on the triangle and remove k from simplex
s->nvrtx = 3;
if (testPlaneTwo) {
k = 2; // s2
i = 1;
j = 0;
}
else if (testPlaneThree) {
k = 1; // s3
i = 0;
j = 2;
}
else {
k = 0; // s4
i = 2;
j = 1;
}
getvrtx(si, i);
getvrtx(sj, j);
getvrtx(sk, k);
if (dotTotal == 1) {
if (hff1_tests[k]) {
if (!hff2(s1, sk, si)) {
select_1ik();
projectOnPlane(s1, si, sk, v);
}
else if (!hff2(s1, sk, sj)) {
select_1jk();
projectOnPlane(s1, sj, sk, v);
}
else {
select_1k(); // select region 1i
projectOnLine(s1, sk, v);
}
}
else if (hff1_tests[i]) {
if (!hff2(s1, si, sk)) {
select_1ik();
projectOnPlane(s1, si, sk, v);
}
else {
select_1i(); // select region 1i
projectOnLine(s1, si, v);
}
}
else {
if (!hff2(s1, sj, sk)) {
select_1jk();
projectOnPlane(s1, sj, sk, v);
}
else {
select_1j(); // select region 1i
projectOnLine(s1, sj, v);
}
}
}
else if (dotTotal == 2) {
// Two edges have positive hff1, meaning that for two edges the origin's project fall on the segement.
// Certainly the edge 1,k supports the the point of minimum norm, and so hff1_1k is positive
if (hff1_tests[i]) {
if (!hff2(s1, sk, si))
if (!hff2(s1, si, sk)) {
select_1ik(); // select region 1ik
projectOnPlane(s1, si, sk, v);
}
else {
select_1k(); // select region 1k
projectOnLine(s1, sk, v);
}
else {
if (!hff2(s1, sk, sj)) {
select_1jk(); // select region 1jk
projectOnPlane(s1, sj, sk, v);
}
else {
select_1k(); // select region 1k
projectOnLine(s1, sk, v);
}
}
}
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
projectOnPlane(s1, sj, sk, v);
}
else {
select_1j(); // select region 1j
projectOnLine(s1, sj, v);
}
else {
if (!hff2(s1, sk, si)) {
select_1ik(); // select region 1ik
projectOnPlane(s1, si, sk, v);
}
else {
select_1k(); // select region 1k
projectOnLine(s1, sk, v);
}
}
}
else {
// ERROR;
}
}
else if (dotTotal == 3) {
// MM : ALL THIS HYPHOTESIS IS FALSE
// sk is s.t. hff3 for sk < 0. So, sk must support the origin because there are 2 triangles facing the origin.
int hff2_ik = hff2(s1,si,sk);
int hff2_jk = hff2(s1,sj,sk);
int hff2_ki = hff2(s1,sk,si);
int hff2_kj = hff2(s1,sk,sj);
if (hff2_ki == 0 && hff2_kj == 0){
mexPrintf("\n\n UNEXPECTED VALUES!!! \n\n");
}
if (hff2_ki == 1 && hff2_kj == 1){
select_1k();
projectOnLine(s1, sk, v);
}
else if (hff2_ki) {
// discard i
if (hff2_jk){
// discard k
select_1j();
projectOnLine(s1, sj, v);
}
else{
select_1jk();
projectOnPlane(s1, sk, sj, v);
}
}
else {
// discard j
if (hff2_ik){
// discard k
select_1i();
projectOnLine(s1, si, v);
}
else{
select_1ik();
projectOnPlane(s1, sk, si, v);
}
}
}
break;
case 0:
// The origin is outside all 3 triangles
if (dotTotal == 1) {
// Here si is set such that hff(s1,si) > 0
if (testLineThree) {
k = 2;
i = 1; // s3
j = 0;
}
else if (testLineFour) {
k = 1; // s3
i = 0;
j = 2;
}
else {
k = 0;
i = 2; // s2
j = 1;
}
getvrtx(si, i);
getvrtx(sj, j);
getvrtx(sk, k);
if (!hff2(s1, si, sj)) {
select_1ij();
projectOnPlane(s1, si, sj, v);
}
else if (!hff2(s1, si, sk)) {
select_1ik();
projectOnPlane(s1, si, sk, v);
}
else {
select_1i();
projectOnLine(s1, si, v);
}
}
else if (dotTotal == 2) {
// Here si is set such that hff(s1,si) < 0
s->nvrtx = 3;
if (!testLineThree) {
k = 2;
i = 1; // s3
j = 0;
}
else if (!testLineFour) {
k = 1;
i = 0; // s4
j = 2;
}
else {
k = 0;
i = 2; // s2
j = 1;
}
getvrtx(si, i);
getvrtx(sj, j);
getvrtx(sk, k);
if (!hff2(s1, sj, sk)) {
if (!hff2(s1, sk, sj)) {
select_1jk(); // select region 1jk
projectOnPlane(s1, sj, sk, v);
}
else if (!hff2(s1, sk, si)) {
select_1ik();
projectOnPlane(s1, sk, si, v);
}
else {
select_1k();
projectOnLine(s1, sk, v);
}
}
else if (!hff2(s1, sj, si)) {
select_1ij();
projectOnPlane(s1, si, sj, v);
}
else {
select_1j();
projectOnLine(s1, sj, v);
}
}
break;
default:
mexPrintf("\nERROR:\tunhandled");
}
}
inline static void support(struct bd *body, const double *v) {
double s, maxs;
double *vrt;
int better = -1;
maxs = dotProduct(body->s, v);
for (int i = 0; i < body->numpoints; ++i) {
vrt = body->coord[i];
s = dotProduct(vrt, v);
if (s > maxs) {
maxs = s;
better = i;
}
}
if (better != -1) {
body->s[0] = body->coord[better][0];
body->s[1] = body->coord[better][1];
body->s[2] = body->coord[better][2];
}
}
inline static void subalgorithm(struct simplex *s, double *v) {
switch (s->nvrtx) {
case 4:
S3D(s, v);
break;
case 3:
S2D(s, v);
break;
case 2:
S1D(s, v);
break;
default:
mexPrintf("\nERROR:\t invalid simplex\n");
}
}
double gjk(struct bd bd1, struct bd bd2, struct simplex *s) {
int k = 0; /**< Iteration counter */
int i; /**< General purpose counter */
int mk = 5000; /**< Maximum number of iterations of the GJK algorithm */
int absTestin;
double norm2Wmax = 0;
double tesnorm;
double v[3]; /**< Search direction */
double vminus[3]; /**< Search direction * -1 */
double w[3]; /**< Vertex on CSO boundary given by the difference of support functions on both bodies */
double eps_rel = eps_rel22; /**< Tolerance on relative */
double eps_rel2 = eps_rel * eps_rel;
double eps_tot = eps_tot22;
int exeedtol_rel = 0; /**< Flag for 1st exit condition */
int nullV = 0;
#ifdef DEBUG
mexPrintf("Num points A = %i \n", bd1.numpoints);
mexPrintf("Num points B = %i \n", bd2.numpoints);
for (i = 0; i < bd1.numpoints; ++i) {
for (int j = 0; j < 3; j++) {
mexPrintf("%.4f ", bd1.coord[i][j]);
}
mexPrintf("\n");
}
for (i = 0; i < bd2.numpoints; ++i) {
for (int j = 0; j < 3; j++) {
mexPrintf("%.4f ", bd2.coord[i][j]);
}
mexPrintf("\n");
}
#endif
/* Initialise search direction */
v[0] = bd1.coord[0][0] - bd2.coord[0][0];
v[1] = bd1.coord[0][1] - bd2.coord[0][1];
v[2] = bd1.coord[0][2] - bd2.coord[0][2];
/* 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)
bd1.s[t] = bd1.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];
/* Support function */
support(&bd1, vminus);
support(&bd2, v);
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) */
exeedtol_rel = (norm2(v) - dotProduct(v, w)) <= eps_rel2 * norm2(v);
if (exeedtol_rel) {
break;
}
nullV = norm2(v) < eps_rel2;
if (nullV) {
break;
}
/* Add new vertex to simplex */
i = s->nvrtx;
for (int t = 0; t < 3; ++t)
s->vrtx[i][t] = w[t];
s->nvrtx++;
/* Invoke distance sub-algorithm */
subalgorithm(s, v);
/* Test */
for (int jj = 0; jj < s->nvrtx; jj++) {
tesnorm = norm2(s->vrtx[jj]);
if (tesnorm > norm2Wmax) {
norm2Wmax = tesnorm;
}
}
absTestin = (norm2(v) <= (eps_tot * eps_tot * norm2Wmax));
if (absTestin)
break;
} while ((s->nvrtx != 4) && (k != mk));
if (k == mk) {
mexPrintf("\n * * * * * * * * * * * * MAXIMUM ITERATION NUMBER REACHED!!! * * * * * * * * * * * * * * \n");
}
return sqrt(norm2(v));
}
#ifdef MATLABDOESMEXSTUFF
/**
* @brief Mex function for Matlab.
*/
void mexFunction(int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
double *inCoordsA;
double *inCoordsB;
size_t nCoordsA;
size_t nCoordsB;
int i;
double *distance;
int c = 3;
int count = 0;
double**arr1;
double**arr2;
/**************** PARSE INPUTS AND OUTPUTS **********************/
/*----------------------------------------------------------------*/
/* Examine input (right-hand-side) arguments. */
if (nrhs != 2) {
mexErrMsgIdAndTxt("MyToolbox:gjk:nrhs", "Two inputs required.");
}
/* Examine output (left-hand-side) arguments. */
if (nlhs != 1) {
mexErrMsgIdAndTxt("MyToolbox:gjk:nlhs", "One output required.");
}
/* make sure the two input arguments are any numerical type */
/* .. first input */
if (!mxIsNumeric(prhs[0])) {
mexErrMsgIdAndTxt("MyToolbox:gjk:notNumeric", "Input matrix must be type numeric.");
}
/* .. second input */
if (!mxIsNumeric(prhs[1])) {
mexErrMsgIdAndTxt("MyToolbox:gjk:notNumeric", "Input matrix must be type numeric.");
}
/* make sure the two input arguments have 3 columns */
/* .. first input */
if (mxGetM(prhs[0]) != 3) {
mexErrMsgIdAndTxt("MyToolbox:gjk:notColumnVector", "First input must have 3 columns.");
}
/* .. second input */
if (mxGetM(prhs[1]) != 3) {
mexErrMsgIdAndTxt("MyToolbox:gjk:notColumnVector", "Second input must have 3 columns.");
}
/*----------------------------------------------------------------*/
/* CREATE DATA COMPATIBLE WITH MATALB */
/* create a pointer to the real data in the input matrix */
inCoordsA = mxGetPr(prhs[0]);
inCoordsB = mxGetPr(prhs[1]);
/* get the length of each input vector */
nCoordsA = mxGetN(prhs[0]);
nCoordsB = mxGetN(prhs[1]);
/* Create output */
plhs[0] = mxCreateDoubleMatrix(1, 1, mxREAL);
/* get a pointer to the real data in the output matrix */
distance = mxGetPr(plhs[0]);
/* Copy data from Matlab's vectors into two new arrays */
arr1 = (double **)mxMalloc(sizeof(double *) * (int)nCoordsA);
arr2 = (double **)mxMalloc(sizeof(double *) * (int)nCoordsB);
for (i = 0; i < nCoordsA; i++)
arr1[i] = &inCoordsA[i * 3];
for (i = 0; i < nCoordsB; i++)
arr2[i] = &inCoordsB[i * 3];
/*----------------------------------------------------------------*/
/* POPULATE BODIES' STRUCTURES */
struct bd bd1; /* Structure of body A */
struct bd bd2; /* Structure of body B */
/* Assign number of vertices to each body */
bd1.numpoints = (int)nCoordsA;
bd2.numpoints = (int)nCoordsB;
bd1.coord = arr1;
bd2.coord = arr2;
/*----------------------------------------------------------------*/
/*CALL COMPUTATIONAL ROUTINE */
struct simplex s;
s.nvrtx = 0;
/* Compute squared distance using GJK algorithm */
distance[0] = gjk(bd1, bd2, &s);
mxFree(arr1);
mxFree(arr2);
}
#endif
/**
* @brief Invoke this function from C# applications
*/
double csFunction(int nCoordsA, double *inCoordsA, int nCoordsB, double *inCoordsB)
{
double distance = 0;
int i, j;
/*----------------------------------------------------------------*/
/* POPULATE BODIES' STRUCTURES */
struct bd bd1; /* Structure of body A */
struct bd bd2; /* Structure of body B */
/* Assign number of vertices to each body */
bd1.numpoints = (int)nCoordsA;
bd2.numpoints = (int)nCoordsB;
double **pinCoordsA = (double **)malloc(bd1.numpoints * sizeof(double *));
for (i = 0; i < bd1.numpoints; i++)
pinCoordsA[i] = (double *)malloc(3 * sizeof(double));
for (i = 0; i < 3; i++)
for (j = 0; j < bd1.numpoints; j++)
pinCoordsA[j][i] = inCoordsA[i*bd1.numpoints + j];
double **pinCoordsB = (double **)malloc(bd2.numpoints * sizeof(double *));
for (i = 0; i < bd2.numpoints; i++)
pinCoordsB[i] = (double *)malloc(3 * sizeof(double));
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;
/*----------------------------------------------------------------*/
/*CALL COMPUTATIONAL ROUTINE */
struct simplex s;
/* Initialise simplex as empty */
s.nvrtx = 0;
/* Compute squared distance using GJK algorithm */
distance = gjk(bd1, bd2, &s);
for (i = 0; i < bd1.numpoints; i++)
free(pinCoordsA[i]);
free(pinCoordsA);
for (i = 0; i < bd2.numpoints; i++)
free(pinCoordsB[i]);
free(pinCoordsB);
return distance;
}
Reference in New Issue View Git Blame Copy Permalink