Cython wrapping trial in Cython folder
Daniel Song
parent
c29c2dee5d
commit
5e29631244
Binary file not shown.
Binary file not shown.
|
|
@ -0,0 +1,945 @@
|
||||||
|
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
|
||||||
|
* ##### # # # *
|
||||||
|
* #### ##### ###### # # # # # # # *
|
||||||
|
* # # # # # ## # # # # # *
|
||||||
|
* # # # # ##### # # # # #### # ### *
|
||||||
|
* # # ##### # # # # # # # # # # *
|
||||||
|
* # # # # # ## # # # # # # *
|
||||||
|
* #### # ###### # # ##### ##### # # *
|
||||||
|
* *
|
||||||
|
* 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;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,52 @@
|
||||||
|
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
|
||||||
|
* ##### # # # *
|
||||||
|
* #### ##### ###### # # # # # # # *
|
||||||
|
* # # # # # ## # # # # # *
|
||||||
|
* # # # # ##### # # # # #### # ### *
|
||||||
|
* # # ##### # # # # # # # # # # *
|
||||||
|
* # # # # # ## # # # # # # *
|
||||||
|
* #### # ###### # # ##### ##### # # *
|
||||||
|
* *
|
||||||
|
* Edward Garemo and Mattia Montanari *
|
||||||
|
* University of Oxford 2019 *
|
||||||
|
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
|
||||||
|
* *
|
||||||
|
* This is the header file for the openGJK.c file. It defines the openGJK *
|
||||||
|
* function and it two important structures: bd and simplex. *
|
||||||
|
* *
|
||||||
|
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
|
||||||
|
|
||||||
|
#ifndef __OPENGJK_H__
|
||||||
|
#define __OPENGJK_H__
|
||||||
|
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include "math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Structure of a body.
|
||||||
|
*/
|
||||||
|
struct bd {
|
||||||
|
int numpoints; /**< Number of points defining the body. */
|
||||||
|
double s[3]; /**< Support mapping computed last. */
|
||||||
|
double **coord; /**< Pointer to pointer to the points' coordinates. */
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Structure for a simplex.
|
||||||
|
*/
|
||||||
|
struct simplex {
|
||||||
|
int nvrtx; /**< Number of simplex's vertices. */
|
||||||
|
double vrtx[4][3]; /**< Coordinates of simplex's vertices. */
|
||||||
|
int wids[4]; /**< Label of the simplex's vertices. */
|
||||||
|
double lambdas[4]; /**< Barycentric coordiantes for each vertex. */
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief The GJK algorithm which returns the minimum distance between
|
||||||
|
* two bodies.
|
||||||
|
*/
|
||||||
|
extern double gjk(struct bd, struct bd, struct simplex *);
|
||||||
|
|
||||||
|
#endif
|
||||||
Binary file not shown.
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,52 @@
|
||||||
|
#cython: language_level=3, boundscheck=False
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
cdef extern from "openGJK.h":
|
||||||
|
struct bd:
|
||||||
|
int numpoints
|
||||||
|
double s[3]
|
||||||
|
double** coord
|
||||||
|
|
||||||
|
struct simplex:
|
||||||
|
int nvrtx
|
||||||
|
double vrtx[4][3]
|
||||||
|
int wids[4]
|
||||||
|
double lambdas[4]
|
||||||
|
|
||||||
|
double gjk(bd bd1, bd bd2, simplex *s)
|
||||||
|
|
||||||
|
def pygjk(bod1, bod2):
|
||||||
|
"""Returns distance between two bodies, input: array of nodal coordinates for each body"""
|
||||||
|
|
||||||
|
cdef:
|
||||||
|
simplex s
|
||||||
|
bd bd1
|
||||||
|
bd bd2
|
||||||
|
double v1[3], v2[3]
|
||||||
|
int i, j
|
||||||
|
|
||||||
|
dummyRow = np.array([0.,0.,0.])
|
||||||
|
|
||||||
|
bd1.numpoints = np.size(bod1, 0)
|
||||||
|
if bd1.numpoints < 2:
|
||||||
|
bod1 = np.vstack([bod1, dummyRow])
|
||||||
|
for i in range(0, bd1.numpoints):
|
||||||
|
v1[0] = bod1[i,0]
|
||||||
|
v1[1] = bod1[i,1]
|
||||||
|
v1[2] = bod1[i,2]
|
||||||
|
bd1.coord[i] = v1
|
||||||
|
|
||||||
|
bd2.numpoints = np.size(bod2, 0)
|
||||||
|
if bd2.numpoints < 2:
|
||||||
|
bod2 = np.vstack([bod2, dummyRow])
|
||||||
|
for j in range(0, bd2.numpoints):
|
||||||
|
v2[0] = bod2[j,0]
|
||||||
|
v2[1] = bod2[j,1]
|
||||||
|
v2[2] = bod2[j,2]
|
||||||
|
bd2.coord[i] = v2
|
||||||
|
|
||||||
|
answer = gjk(bd1, bd2, &s)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
return answer
|
||||||
|
|
@ -0,0 +1,12 @@
|
||||||
|
|
||||||
|
from distutils.core import setup, Extension
|
||||||
|
from Cython.Build import cythonize
|
||||||
|
|
||||||
|
exts = Extension(
|
||||||
|
"openGJKpy",
|
||||||
|
sources = ["openGJK_cython.pyx", "openGJK.c"]
|
||||||
|
)
|
||||||
|
|
||||||
|
setup(ext_modules = cythonize( [exts] ))
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -0,0 +1,181 @@
|
||||||
|
import openGJKpy as opengjk
|
||||||
|
from scipy.spatial.transform import Rotation as R
|
||||||
|
import numpy as np
|
||||||
|
import pytest
|
||||||
|
|
||||||
|
|
||||||
|
def distance_point_to_line_3D(P1, P2, point):
|
||||||
|
"""
|
||||||
|
distance from point to line
|
||||||
|
"""
|
||||||
|
return np.linalg.norm(np.cross(P2-P1, P1-point))/np.linalg.norm(P2-P1)
|
||||||
|
|
||||||
|
|
||||||
|
def distance_point_to_plane_3D(P1, P2, P3, point):
|
||||||
|
"""
|
||||||
|
Distance from point to plane
|
||||||
|
"""
|
||||||
|
return np.abs(np.dot(np.cross(P2-P1, P3-P1) /
|
||||||
|
np.linalg.norm(np.cross(P2-P1, P3-P1)), point-P2))
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.mark.parametrize("delta", [0.1, 1e-12, 0, -2])
|
||||||
|
def test_line_point_distance(delta):
|
||||||
|
line = np.array([[0.1, 0.2, 0.3], [0.5, 0.8, 0.7]], dtype=np.float64) #
|
||||||
|
point_on_line = line[0] + 0.27*(line[1]-line[0])
|
||||||
|
normal = np.cross(line[0], line[1])
|
||||||
|
point = point_on_line + delta * normal
|
||||||
|
distance = opengjk.pygjk(line, point)
|
||||||
|
actual_distance = distance_point_to_line_3D(
|
||||||
|
line[0], line[1], point)
|
||||||
|
print(distance, actual_distance)
|
||||||
|
assert(np.isclose(distance, actual_distance, atol=1e-15))
|
||||||
|
|
||||||
|
|
||||||
|
# @pytest.mark.parametrize("delta", [0.1, 1e-12, 0])
|
||||||
|
# def test_line_line_distance(delta):
|
||||||
|
# line = np.array([[-0.5, -0.7, -0.3], [1, 2, 3]], dtype=np.float64)
|
||||||
|
# point_on_line = line[0] + 0.38*(line[1]-line[0])
|
||||||
|
# normal = np.cross(line[0], line[1])
|
||||||
|
# point = point_on_line + delta * normal
|
||||||
|
# line_2 = np.array([point, [2, 5, 6]], dtype=np.float64)
|
||||||
|
# distance = opengjk.pygjk(line, line_2)
|
||||||
|
# actual_distance = distance_point_to_line_3D(
|
||||||
|
# line[0], line[1], line_2[0])
|
||||||
|
# print(distance, actual_distance)
|
||||||
|
# assert(np.isclose(distance, actual_distance, atol=1e-15))
|
||||||
|
|
||||||
|
|
||||||
|
# @pytest.mark.parametrize("delta", [0.1**(3*i) for i in range(6)])
|
||||||
|
# def test_tri_distance(delta):
|
||||||
|
# tri_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0]], dtype=np.float64)
|
||||||
|
# tri_2 = np.array([[1, delta, 0], [3, 1.2, 0], [
|
||||||
|
# 1, 1, 0]], dtype=np.float64)
|
||||||
|
# P1 = tri_1[2]
|
||||||
|
# P2 = tri_1[1]
|
||||||
|
# point = tri_2[0]
|
||||||
|
# actual_distance = distance_point_to_line_3D(P1, P2, point)
|
||||||
|
# distance = opengjk.pygjk(tri_1, tri_2)
|
||||||
|
# print("Computed distance ", distance, "Actual distance ", actual_distance)
|
||||||
|
|
||||||
|
# assert(np.isclose(distance, actual_distance, atol=1e-15))
|
||||||
|
|
||||||
|
|
||||||
|
# @pytest.mark.parametrize("delta", [0.1*0.1**(3*i) for i in range(6)])
|
||||||
|
# def test_quad_distance2d(delta):
|
||||||
|
# quad_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0],
|
||||||
|
# [1, 1, 0]], dtype=np.float64)
|
||||||
|
# quad_2 = np.array([[0, 1+delta, 0], [2, 2, 0],
|
||||||
|
# [2, 4, 0], [4, 4, 0]], dtype=np.float64)
|
||||||
|
# P1 = quad_1[2]
|
||||||
|
# P2 = quad_1[3]
|
||||||
|
# point = quad_2[0]
|
||||||
|
# actual_distance = distance_point_to_line_3D(P1, P2, point)
|
||||||
|
# distance = opengjk.pygjk(quad_1, quad_2)
|
||||||
|
# print("Computed distance ", distance, "Actual distance ", actual_distance)
|
||||||
|
|
||||||
|
# assert(np.isclose(distance, actual_distance, atol=1e-15))
|
||||||
|
|
||||||
|
|
||||||
|
# @pytest.mark.parametrize("delta", [1*0.5**(3*i) for i in range(7)])
|
||||||
|
# def test_tetra_distance_3d(delta):
|
||||||
|
# tetra_1 = np.array([[0, 0, 0.2], [1, 0, 0.1], [0, 1, 0.3],
|
||||||
|
# [0, 0, 1]], dtype=np.float64)
|
||||||
|
# tetra_2 = np.array([[0, 0, -3], [1, 0, -3], [0, 1, -3],
|
||||||
|
# [0.5, 0.3, -delta]], dtype=np.float64)
|
||||||
|
# actual_distance = distance_point_to_plane_3D(tetra_1[0], tetra_1[1],
|
||||||
|
# tetra_1[2], tetra_2[3])
|
||||||
|
# distance = opengjk.pygjk(tetra_1, tetra_2)
|
||||||
|
# print("Computed distance ", distance, "Actual distance ", actual_distance)
|
||||||
|
|
||||||
|
# assert(np.isclose(distance, actual_distance, atol=1e-15))
|
||||||
|
|
||||||
|
|
||||||
|
# @pytest.mark.parametrize("delta", [(-1)**i*np.sqrt(2)*0.1**(3*i)
|
||||||
|
# for i in range(6)])
|
||||||
|
# def test_tetra_collision_3d(delta):
|
||||||
|
# tetra_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0],
|
||||||
|
# [0, 0, 1]], dtype=np.float64)
|
||||||
|
# tetra_2 = np.array([[0, 0, -3], [1, 0, -3], [0, 1, -3],
|
||||||
|
# [0.5, 0.3, -delta]], dtype=np.float64)
|
||||||
|
# actual_distance = distance_point_to_plane_3D(tetra_1[0], tetra_1[1],
|
||||||
|
# tetra_1[2], tetra_2[3])
|
||||||
|
# distance = opengjk.pygjk(tetra_1, tetra_2)
|
||||||
|
|
||||||
|
# if delta < 0:
|
||||||
|
# assert(np.isclose(distance, 0, atol=1e-15))
|
||||||
|
# else:
|
||||||
|
# print("Computed distance ", distance,
|
||||||
|
# "Actual distance ", actual_distance)
|
||||||
|
# assert(np.isclose(distance, actual_distance, atol=1e-15))
|
||||||
|
|
||||||
|
|
||||||
|
# @pytest.mark.parametrize("delta", [0, -0.1, -0.49, -0.51])
|
||||||
|
# def test_hex_collision_3d(delta):
|
||||||
|
# hex_1 = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0], [1, 1, 0],
|
||||||
|
# [0, 0, 1], [1, 0, 1], [0, 1, 1], [1, 1, 1]],
|
||||||
|
# dtype=np.float64)
|
||||||
|
# P0 = np.array([1.5+delta, 1.5+delta, 0.5], dtype=np.float64)
|
||||||
|
# P1 = np.array([2, 2, 1], dtype=np.float64)
|
||||||
|
# P2 = np.array([2, 1.25, 0.25], dtype=np.float64)
|
||||||
|
# P3 = P1 + P2 - P0
|
||||||
|
# quad_1 = np.array([P0, P1, P2, P3], dtype=np.float64)
|
||||||
|
# n = (np.cross(quad_1[1]-quad_1[0], quad_1[2]-quad_1[0]) /
|
||||||
|
# np.linalg.norm(
|
||||||
|
# np.cross(quad_1[1]-quad_1[0],
|
||||||
|
# quad_1[2]-quad_1[0])))
|
||||||
|
# quad_2 = quad_1 + n
|
||||||
|
# hex_2 = np.zeros((8, 3), dtype=np.float64)
|
||||||
|
# hex_2[:4, :] = quad_1
|
||||||
|
# hex_2[4:, :] = quad_2
|
||||||
|
# actual_distance = np.linalg.norm(
|
||||||
|
# np.array([1, 1, P0[2]], dtype=np.float64)-hex_2[0])
|
||||||
|
# distance = opengjk.pygjk(hex_1, hex_2)
|
||||||
|
|
||||||
|
# if P0[0] < 1:
|
||||||
|
# assert(np.isclose(distance, 0, atol=1e-15))
|
||||||
|
# else:
|
||||||
|
# print("Computed distance ", distance,
|
||||||
|
# "Actual distance ", actual_distance)
|
||||||
|
# assert(np.isclose(distance, actual_distance, atol=1e-15))
|
||||||
|
|
||||||
|
|
||||||
|
# @pytest.mark.parametrize("c0", [0, 1, 2, 3])
|
||||||
|
# @pytest.mark.parametrize("c1", [0, 1, 2, 3])
|
||||||
|
# def test_cube_distance(c0, c1):
|
||||||
|
# cubes = [np.array([[-1, -1, -1], [1, -1, -1], [-1, 1, -1], [1, 1, -1],
|
||||||
|
# [-1, -1, 1], [1, -1, 1], [-1, 1, 1], [1, 1, 1]],
|
||||||
|
# dtype=np.float64)]
|
||||||
|
|
||||||
|
# r = R.from_euler('z', 45, degrees=True)
|
||||||
|
# cubes.append(r.apply(cubes[0]))
|
||||||
|
# r = R.from_euler('y', np.arctan2(1.0, np.sqrt(2)))
|
||||||
|
# cubes.append(r.apply(cubes[1]))
|
||||||
|
# r = R.from_euler('y', 45, degrees=True)
|
||||||
|
# cubes.append(r.apply(cubes[0]))
|
||||||
|
|
||||||
|
# dx = cubes[c0][:,0].max() - cubes[c1][:,0].min()
|
||||||
|
# cube0 = cubes[c0]
|
||||||
|
|
||||||
|
# for delta in [1e8, 1.0, 1e-4, 1e-8, 1e-12]:
|
||||||
|
# cube1 = cubes[c1] + np.array([dx + delta, 0, 0])
|
||||||
|
# distance = opengjk.pygjk(cube0, cube1)
|
||||||
|
# print(distance, delta)
|
||||||
|
# assert(np.isclose(distance, delta))
|
||||||
|
|
||||||
|
# def test_random_objects():
|
||||||
|
# for i in range(1, 8):
|
||||||
|
# for j in range(1, 8):
|
||||||
|
# for k in range(1000):
|
||||||
|
# arr1 = np.random.rand(i, 3)
|
||||||
|
# arr2 = np.random.rand(j, 3)
|
||||||
|
# opengjk.pygjk(arr1, arr2)
|
||||||
|
|
||||||
|
|
||||||
|
# def test_large_random_objects():
|
||||||
|
# for i in range(1, 8):
|
||||||
|
# for j in range(1, 8):
|
||||||
|
# for k in range(1000):
|
||||||
|
# arr1 = 10000.0*np.random.rand(i, 3)
|
||||||
|
# arr2 = 10000.0*np.random.rand(j, 3)
|
||||||
|
# opengjk.pygjk(arr1, arr2)
|
||||||
Loading…
Reference in New Issue