Remove compiled files and source coming from library
Mattia Montanari
parent
14a62efb9a
commit
f01cf17ea4
946
Cython/openGJK.c
946
Cython/openGJK.c
|
|
@ -1,946 +0,0 @@
|
|||
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
|
||||
* ##### # # # *
|
||||
* #### ##### ###### # # # # # # # *
|
||||
* # # # # # ## # # # # # *
|
||||
* # # # # ##### # # # # #### # ### *
|
||||
* # # ##### # # # # # # # # # # *
|
||||
* # # # # # ## # # # # # # *
|
||||
* #### # ###### # # ##### ##### # # *
|
||||
* *
|
||||
* 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;
|
||||
}
|
||||
|
|
@ -1,52 +0,0 @@
|
|||
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
|
||||
* ##### # # # *
|
||||
* #### ##### ###### # # # # # # # *
|
||||
* # # # # # ## # # # # # *
|
||||
* # # # # ##### # # # # #### # ### *
|
||||
* # # ##### # # # # # # # # # # *
|
||||
* # # # # # ## # # # # # # *
|
||||
* #### # ###### # # ##### ##### # # *
|
||||
* *
|
||||
* 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
|
||||
BIN
Cython/openGJK.o
BIN
Cython/openGJK.o
Binary file not shown.
21630
Cython/openGJK_cython.c
21630
Cython/openGJK_cython.c
File diff suppressed because it is too large
Load Diff
Loading…
Reference in New Issue