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

Fix to major bugs in sub-algorithm and exit conditions, and support function improved

fixes-turtlebasket
Mattia 2019-07-09 15:10:46 +01:00
parent 646c1d3edc
commit d56fac5562
5 changed files with 407 additions and 449 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
example1_c/main.c
View File

@ -7,7 +7,7 @@
* # # # # # ## # # # # # # *
* #### # ###### # # ##### ##### # # *
* *
* Mattia Montanari | University of Oxford 2018 *
* Mattia Montanari | University of Oxford 2019 *
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
* *
* This file runs an example to illustrate how to invoke the openGJK lib *

2
example2_mex/runme.m
View File

@ -7,7 +7,7 @@
% # # # # # ## # # # # # # %
% #### # ###### # # ##### ##### # # %
% %
% Mattia Montanari | University of Oxford 2018 %
% Mattia Montanari | University of Oxford 2019 %
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - %
% %
% This file compiles a mex function from the openGJK library and runs an %

2
lib/CMakeLists.txt
View File

@ -7,7 +7,7 @@
# # # # # # ## # # # # # # #
# #### # ###### # # ##### ##### # # #
# #
# Mattia Montanari | University of Oxford 2018 #
# Mattia Montanari | University of Oxford 2019 #
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - #

2
lib/include/openGJK/openGJK.h
View File

@ -7,7 +7,7 @@
* # # # # # ## # # # # # # *
* #### # ###### # # ##### ##### # # *
* *
* Mattia Montanari | University of Oxford 2018 *
* Mattia Montanari | University of Oxford 2019 *
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
* *
* This is the header file for the openGJK.c file. It defines the openGJK *

502
lib/src/openGJK.c
View File

@ -7,7 +7,8 @@
* # # # # # ## # # # # # # *
* #### # ###### # # ##### ##### # # *
* *
* Mattia Montanari | University of Oxford 2018 *
* *
* Mattia Montanari | University of Oxford 2019 *
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
* *
* This file implements the GJK algorithm and the Signed Volumes method as *
@ -28,15 +29,14 @@
#define _CRT_HAS_CXX17 0
#include <stdio.h>
#include "openGJK/openGJK.h"
/* If defined, uses an advanced method to compute the determinant of matrices. */
/* If instricuted, uses adaptive floating point arithmetics. */
#ifdef ADAPTIVEFP
#include "predicates.h"
#endif
/* Used for creating a mex function for Matlab. */
/* If instricuted, compile a mex function for Matlab. */
#ifdef MATLABDOESMEXSTUFF
#include "mex.h"
#else
@ -47,80 +47,54 @@
#define _isnan(x) isnan(x)
#endif
#define dotProduct(a, b) (a[0]*b[0]+a[1]*b[1]+a[2]*b[2])
/**
* @brief Computes the norm of a vector.
* @brief Returns squared vector norm
*/
double norm2(double *v) {
double n2;
n2 = 0;
for (int i = 0; i < 3; ++i) {
double n2 = 0;
for (int i = 0; i < 3; ++i)
n2 += v[i] * v[i];
}
return n2;
}
/**
* @brief Computes the dot product between two vectors.
* @brief Finds point of minimum norm of 1-simplex. Robust, but slower, version of algorithm presented in paper.
*/
inline double dotprod(double *a, double *b) {
double dot;
dot = 0;
for (int i = 0; i < 3; ++i) {
dot += a[i] * b[i];
}
return dot;
}
/**
* @brief Function invoked by the Signed Volumes sub-algorithm for 1-simplex.
*/
void S1D( struct simplex * s) {
double a[3],
b[3],
t[3],
nu_test[3],
nu_fabs[3];
double inv_detM = 0,
det_ap = 0,
det_pb = 0,
pt = 0;
int i = 0,
indexI = 1,
FacetsTest[ 2 ];
inline static void S1D(struct simplex * s, double *vv) {
int i = 0;
int indexI = 1;
int FacetsTest[2];
double det_ap = 0;
double det_pb = 0;
double pt = 0;
double leng = 0;
double a[3];
double b[3];
double t[3];
double nu_fabs[3];
for (i = 0; i < 3; ++i) {
b[i] = s->vrtx[0][i];
a[i] = s->vrtx[1][i];
t[i] = b[i] - a[i];
}
for ( i = 0; i < 3; ++i) {
nu_test[i] = a[i] - b[i];
}
for ( i = 0; i < 3; ++i) {
nu_fabs[i] = fabs( nu_test[i] );
leng += t[i];
nu_fabs[i] = fabs(t[i]);
}
if (nu_fabs[0] > nu_fabs[1]) {
if (nu_fabs[0] > nu_fabs[2]){
if (nu_fabs[0] > nu_fabs[2])
indexI = 0;
}
else {
else
indexI = 2;
}
}
else if (nu_fabs[0] < nu_fabs[1]) {
if (nu_fabs[1] > nu_fabs[2]){
if (nu_fabs[1] > nu_fabs[2])
indexI = 1;
}
else {
else
indexI = 2;
}
}
else if (nu_fabs[0] < nu_fabs[2]) {
indexI = 2;
}
@ -128,83 +102,82 @@ void S1D( struct simplex * s) {
indexI = 2;
}
/* Compute inverse of detM */
inv_detM = 1 / nu_test [ indexI ];
/* Project origin onto the 1D simplex - line */
pt = dotprod (b,t) / (dotprod(t,t)) * ( a[indexI] - b[indexI] ) + b[indexI];
pt = dotProduct(b, t) / (dotProduct(t, t)) * (a[indexI] - b[indexI]) + b[indexI];
/* Compute signed determinants */
det_ap = a[indexI] - pt;
det_pb = pt - b[indexI];
/* Test if sign of ABCD is equal to the signes of the auxiliary simplices */
FacetsTest[ 0 ] = SAMESIGN( nu_test [ indexI ] , det_ap );
FacetsTest[ 1 ] = SAMESIGN( nu_test [ indexI ] , det_pb );
/* Compare signs of AB and auxiliary simplices */
FacetsTest[0] = SAMESIGN(t[indexI], -1 * det_ap);
FacetsTest[1] = SAMESIGN(t[indexI], -1 * det_pb);
if (FacetsTest[0] + FacetsTest[1] == 2) {
s->lambdas[0] = det_ap * inv_detM;
/* The origin is between A and B */
s->lambdas[0] = det_ap * -1.0 / t[indexI];
s->lambdas[1] = 1 - s->lambdas[0];
s->wids[0] = 0;
s->wids[1] = 1;
s->nvrtx = 2;
}
else if (FacetsTest[0] == 0) {
/* The origin is beyond A */
s->lambdas[0] = 1;
s->wids[0] = 0;
s->nvrtx = 1;
for (i = 0; i < 3; ++i) {
s->vrtx[0][i] = s->vrtx[1][i];
}
}
else {
/* The origin is behind B */
s->lambdas[0] = 1;
s->wids[0] = 1;
s->nvrtx = 1;
}
for (int j = 0; j < 3; ++j) {
vv[j] = 0;
for (i = 0; i < s->nvrtx; ++i) {
vv[j] += (s->lambdas[i] * s->vrtx[i][j]);
}
}
}
/**
* @brief Function invoked by the Signed Volumes sub-algorithm for 2-simplex.
* @brief Finds point of minimum norm of 2-simplex. Robust, but slower, version of algorithm presented in paper.
*/
static void S2D( struct simplex * s) {
inline static void S2D(struct simplex * s, double *vv) {
int i;
int k;
int l;
int j;
int indexI = 1;
//int stemp[3];
int FacetsTest[3];
int indexJ[2] = { -1 };
double nu_max = 0;
double inv_detM = 0;
double nnorm_sqrd = 0;
double nnnorm = 0;
double dotNA;
double a[3];
double b[3];
double c[3];
double s21[3];
double s31[3];
double nu_test[3];
double nu_fabs[3];
double B[3];
double n[3];
double v[3];
double vtmp[3];
double pp[3 - 1];
double sa[3 - 1];
double sb[3 - 1];
double sc[3 - 1];
double a[3],
b[3],
c[3],
s21[3],
s31[3],
nu_test[3],
nu_fabs[3],
B[ 3 ],
n[3],
v[3],
vtmp[3];
double pp[3-1],
sa[3-1],
sb[3-1],
sc[3-1];
double nu_max = 0,
inv_detM = 0,
nnorm_sqrd = 0,
nnnorm = 0,
dotNA;
int i = 0,
FacetsTest[3],
k,
l,
j;
int indexI = 1,
indexJ[2] = {0, 2},
stemp[3];
for (i = 0; i < 3; ++i) {
c[i] = s->vrtx[0][i];
@ -214,6 +187,7 @@ static void S2D( struct simplex * s) {
s31[i] = c[i] - a[i];
}
/* Find best axis for projection */
k = 1; l = 2;
for (i = 0; i < 3; ++i) {
nu_test[i] = pow(-1.0, i) * (b[k] * c[l] + a[k] * b[l] + c[k] * a[l] - b[k] * a[l] - c[k] * b[l] - a[k] * c[l]);
@ -267,7 +241,8 @@ static void S2D( struct simplex * s) {
n[i] = n[i] * nnnorm;
}
dotNA = dotprod(n,a);
dotNA = dotProduct(n, a);
pp[0] = dotNA * n[indexJ[0]];
pp[1] = dotNA * n[indexJ[1]];
@ -304,12 +279,12 @@ static void S2D( struct simplex * s) {
#endif
/* Test if sign of ABC is equal to the signes of the auxiliary simplices */
for ( int m = 0; m < 3 ; ++m) {
FacetsTest[ m ] = SAMESIGN( nu_max , B[ m ] );
for (i = 0; i < 3; ++i) {
FacetsTest[i] = SAMESIGN(nu_max, B[i]);
}
if ( FacetsTest[0] +FacetsTest[1] + FacetsTest[2] == 0 || isnan(n[0]) ){
// The nan check was not included originally and will be removed in v2.0
if (FacetsTest[1] + FacetsTest[2] == 0 || isnan(n[0])) {
struct simplex sTmp;
sTmp.nvrtx = 2;
@ -323,8 +298,8 @@ static void S2D( struct simplex * s) {
s->vrtx[1][i] = s->vrtx[2][i];
}
S1D(&sTmp);
S1D(s);
S1D(&sTmp, v);
S1D(s, v);
for (j = 0; j < 3; ++j) {
vtmp[j] = 0;
@ -335,7 +310,8 @@ static void S2D( struct simplex * s) {
}
}
if( dotprod(v,v) < dotprod(vtmp,vtmp) ){
if (dotProduct(v, v) < dotProduct(vtmp, vtmp)) {
/* Keep simplex. Need to update sID only*/
for (i = 1; i < s->nvrtx; ++i) {
s->wids[i] = s->wids[i] + 1;
}
@ -370,60 +346,67 @@ static void S2D( struct simplex * s) {
s->vrtx[0][i] = s->vrtx[1][i];
s->vrtx[1][i] = s->vrtx[2][i];
}
S1D(s);
S1D(s, v);
}
else if (FacetsTest[1] == 0) {
/* The origin projection P faces the segment AC */
stemp[0] = 0; stemp[1] = 2;
s->nvrtx = 2;
for (i = 0; i < 3; ++i) {
s->vrtx[0][i] = s->vrtx[ stemp[0] ][i];
s->vrtx[1][i] = s->vrtx[ stemp[1] ][i];
s->vrtx[0][i] = s->vrtx[0][i];
s->vrtx[1][i] = s->vrtx[2][i];
}
S1D(s);
S1D(s, v);
for (i = 1; i < s->nvrtx; ++i) {
s->wids[i] = s->wids[i] + 1;
}
}
else {
/* The origin projection P faces the segment BC */
s->nvrtx = 2;
S1D(s);
}
S1D(s, v);
}
for ( j = 0; j < 3; ++j) {
vv[j] = 0;
for (i = 0; i < s->nvrtx; ++i) {
vv[j] += (s->lambdas[i] * s->vrtx[i][j]);
}
}
}
/**
* @brief Function invoked by the Signed Volumes sub-algorithm for 3-simplex.
*
* @brief Finds point of minimum norm of 3-simplex. Robust, but slower, version of algorithm presented in paper.
*/
static void S3D( struct simplex * s) {
inline static void S3D(struct simplex * s, double *vv) {
int FacetsTest[4] = { 1,1,1,1 };
int sID[4] = { 0, 0, 0, 0 };
int TrianglesToTest[9] = { 3, 3, 3, 1, 2, 2, 0, 0, 1 };
int i;
int j;
int k;
int l;
int vtxid;
int ndoubts = 0;
int nclosestS = 0;
int firstaux = 0;
int secondaux = 0;
int Flag_sAuxused = 0;
double a[3];
double b[3];
double c[3];
double d[3];
#ifdef ADAPTIVEFP
double o[3] = {0};
#endif
double vtmp[3];
double v[3];
double B[4];
double tmplamda[4] = { 0, 0, 0, 0 };
double tmpscoo1[4][3] = { 0 };
double sqdist_tmp = 0;
double detM;
double inv_detM;
double tmplamda[4] = {0, 0, 0, 0};
double tmpscoo1[4][3] = {0};
int i, j, k, l,
Flag_sAuxused = 0,
firstaux = 0,
secondaux = 0;
int FacetsTest[ 4 ] = { 1,1,1,1 },
sID[4] = {0, 0, 0, 0},
nclosestS = 0;
int TrianglesToTest[9] = { 3, 3, 3, 1, 2, 2, 0, 0, 1 };
int ndoubts = 0,
vtxid;
#ifdef ADAPTIVEFP
double o[3] = { 0 };
#endif
for (i = 0; i < 3; ++i) {
d[i] = s->vrtx[0][i];
c[i] = s->vrtx[1][i];
@ -450,44 +433,27 @@ static void S3D( struct simplex * s) {
if (fabs(detM) < eps)
{
if (fabs(B[2]) < eps && fabs(B[3]) < eps)
{
FacetsTest[1] = 0; /* A = B. Test only ACD */
}
else if (fabs(B[1]) < eps && fabs(B[3]) < eps)
{
FacetsTest[2] = 0; /* A = C. Test only ABD */
}
else if (fabs(B[1]) < eps && fabs(B[2]) < eps)
{
FacetsTest[3] = 0; /* A = D. Test only ABC */
}
else if (fabs(B[0]) < eps && fabs(B[3]) < eps)
{
FacetsTest[1] = 0; /* B = C. Test only ACD */
}
else if (fabs(B[0]) < eps && fabs(B[2]) < eps)
{
FacetsTest[1] = 0; /* B = D. Test only ABD */
}
else if (fabs(B[0]) < eps && fabs(B[1]) < eps)
{
FacetsTest[2] = 0; /* C = D. Test only ABC */
}
else
{
else {
for (i = 0; i < 4; i++)
{
FacetsTest[i] = 0;
FacetsTest[i] = 0; /* Any other case. Test ABC, ABD, ACD */
}
}
}
else
{
for (i = 0; i < 4 ; ++i) {
for (i = 0; i < 4; ++i)
FacetsTest[i] = SAMESIGN(detM, B[i]);
}
}
/* Compare signed volumes and compute barycentric coordinates */
if (FacetsTest[0] + FacetsTest[1] + FacetsTest[2] + FacetsTest[3] == 4) {
@ -502,7 +468,6 @@ static void S3D( struct simplex * s) {
s->wids[2] = 2;
s->wids[3] = 3;
s->nvrtx = 4;
return;
}
else if (FacetsTest[1] + FacetsTest[2] + FacetsTest[3] == 0) {
/* There are three facets facing the origin */
@ -513,14 +478,15 @@ static void S3D( struct simplex * s) {
for (i = 0; i < ndoubts; ++i) {
sTmp.nvrtx = 3;
/* Assign coordinates to simplex */
for (k = 0; k < sTmp.nvrtx; ++k) {
vtxid = TrianglesToTest[i + (k * 3)];
for (j = 0; j < 3; ++j) {
sTmp.vrtx[2 - k][j] = s->vrtx[vtxid][j];
}
}
S2D(&sTmp);
/* ... and call S2D */
S2D(&sTmp, v);
/* ... compute aux squared distance */
for (j = 0; j < 3; ++j) {
vtmp[j] = 0;
for (l = 0; l < sTmp.nvrtx; ++l) {
@ -529,15 +495,15 @@ static void S3D( struct simplex * s) {
}
if (i == 0) {
sqdist_tmp = dotprod(vtmp,vtmp);
sqdist_tmp = dotProduct(vtmp, vtmp);
nclosestS = sTmp.nvrtx;
for (l = 0; l < nclosestS; ++l) {
sID[l] = TrianglesToTest[i + (sTmp.wids[l] * 3)];
tmplamda[l] = sTmp.lambdas[l];
}
}
else if (dotprod(vtmp,vtmp) < sqdist_tmp){
sqdist_tmp = dotprod(vtmp,vtmp);
else if (dotProduct(vtmp, vtmp) < sqdist_tmp) {
sqdist_tmp = dotProduct(vtmp, vtmp);
nclosestS = sTmp.nvrtx;
for (l = 0; l < nclosestS; ++l) {
sID[l] = TrianglesToTest[i + (sTmp.wids[l] * 3)];
@ -561,8 +527,6 @@ static void S3D( struct simplex * s) {
s->lambdas[i] = tmplamda[i];
s->wids[nclosestS - 1 - i] = sID[i];
}
return;
}
else if (FacetsTest[1] + FacetsTest[2] + FacetsTest[3] == 1) {
/* There are two facets facing the origin, need to find the closest. */
@ -576,18 +540,21 @@ static void S3D( struct simplex * s) {
sTmp.vrtx[1][i] = s->vrtx[1][i];
sTmp.vrtx[2][i] = s->vrtx[3][i];
}
S2D(&sTmp);
/* ... and call S2D */
S2D(&sTmp, v);
/* ... compute aux squared distance */
for (j = 0; j < 3; ++j) {
vtmp[j] = 0;
for (i = 0; i < sTmp.nvrtx; ++i) {
vtmp[j] += sTmp.lambdas[i] * (sTmp.vrtx[i][j]);
}
}
sqdist_tmp = dotprod(vtmp,vtmp);
sqdist_tmp = dotProduct(vtmp, vtmp);
Flag_sAuxused = 1;
firstaux = 0;
}
if (FacetsTest[2] == 0) {
/* ... Test facet ABD */
if (Flag_sAuxused == 0) {
for (i = 0; i < 3; ++i) {
@ -595,14 +562,16 @@ static void S3D( struct simplex * s) {
sTmp.vrtx[1][i] = s->vrtx[2][i];
sTmp.vrtx[2][i] = s->vrtx[3][i];
}
S2D(&sTmp);
/* ... and call S2D */
S2D(&sTmp, v);
/* ... compute aux squared distance */
for (j = 0; j < 3; ++j) {
vtmp[j] = 0;
for (i = 0; i < sTmp.nvrtx; ++i) {
vtmp[j] += sTmp.lambdas[i] * (sTmp.vrtx[i][j]);
}
}
sqdist_tmp = dotprod(vtmp,vtmp);
sqdist_tmp = dotProduct(vtmp, vtmp);
firstaux = 1;
}
else {
@ -613,7 +582,7 @@ static void S3D( struct simplex * s) {
s->vrtx[2][i] = s->vrtx[3][i];
}
/* ... and call S2D itself */
S2D(s);
S2D(s, v);
secondaux = 1;
}
}
@ -626,25 +595,25 @@ static void S3D( struct simplex * s) {
s->vrtx[1][i] = s->vrtx[2][i];
s->vrtx[2][i] = s->vrtx[3][i];
}
/* ... and call S2D itself */
S2D(s);
/* ... and call S2D */
S2D(s, v);
secondaux = 2;
}
/* Do a loop and compare current outcomes */
/* Compare outcomes */
for (j = 0; j < 3; ++j) {
v[j] = 0;
for (i = 0; i < s->nvrtx; ++i) {
v[j] += s->lambdas[i] * (s->vrtx[i][j]);
}
}
if( dotprod(v,v) < sqdist_tmp){
if (dotProduct(v, v) < sqdist_tmp) {
/* Keep simplex. Need to update sID only*/
for (i = 0; i < s->nvrtx; ++i) {
/* Assume that vertex a is always included in sID. */
s->wids[s->nvrtx - 1 - i] = TrianglesToTest[secondaux + (s->wids[i] * 3)];
}
}
else {
s->nvrtx = sTmp.nvrtx;
for (i = 0; i < s->nvrtx; ++i) {
for (j = 0; j < 3; ++j) {
@ -654,40 +623,38 @@ static void S3D( struct simplex * s) {
s->wids[sTmp.nvrtx - 1 - i] = TrianglesToTest[firstaux + (sTmp.wids[i] * 3)];
}
}
return;
}
else if (FacetsTest[1] + FacetsTest[2] + FacetsTest[3] == 2) {
/* Only one facet is facing the origin */
if (FacetsTest[1] == 0) {
/* The origin projection P faces the facet ACD */
s->nvrtx = 3;
for (i = 0; i < 3; ++i) {
s->vrtx[0][i] = s->vrtx[0][i];
s->vrtx[1][i] = s->vrtx[1][i];
s->vrtx[2][i] = s->vrtx[3][i];
}
S2D(s);
S2D(s, v);
/* Keep simplex. Need to update sID only*/
for (i = 0; i < s->nvrtx; ++i) {
s->wids[i] = s->wids[i];
}
return;
}
else if (FacetsTest[2] == 0) {
/* The origin projection P faces the facet ABD */
/* ... thus, remove the vertex C from the simplex. */
s->nvrtx = 3;
for (i = 0; i < 3; ++i) {
s->vrtx[0][i] = s->vrtx[0][i];
s->vrtx[1][i] = s->vrtx[2][i];
s->vrtx[2][i] = s->vrtx[3][i];
}
S2D(s);
S2D(s, v);
/* Keep simplex. Need to update sID only*/
for (i = 2; i < s->nvrtx; ++i) {
/* Assume that vertex a is always included in sID. */
s->wids[i] = s->wids[i] + 1;
}
return;
}
else if (FacetsTest[3] == 0) {
/* The origin projection P faces the facet ABC */
@ -697,168 +664,159 @@ static void S3D( struct simplex * s) {
s->vrtx[1][i] = s->vrtx[2][i];
s->vrtx[2][i] = s->vrtx[3][i];
}
S2D(s);
return;
S2D(s, v);
}
}
else {
/* The origin projection P faces the facet BCD */
s->nvrtx = 3;
for (i = 0; i < 3; ++i) {
s->vrtx[0][i] = s->vrtx[0][i];
s->vrtx[1][i] = s->vrtx[1][i];
s->vrtx[2][i] = s->vrtx[2][i];
}
S2D(s);
/* ... and call S2D */
S2D(s, v);
/* Keep simplex. Need to update sID only*/
for (i = 0; i < s->nvrtx; ++i) {
/* Assume that vertex a is always included in sID. */
s->wids[i] = s->wids[i] + 1;
}
return;
}
for ( j = 0; j < 3; ++j) {
vv[j] = 0;
for (i = 0; i < s->nvrtx; ++i) {
vv[j] += (s->lambdas[i] * s->vrtx[i][j]);
}
}
}
/**
* @brief Evaluate support function for polytopes.
*/
void support ( struct bd *body, double *v ) {
inline static void support( struct bd *body, double *v ) {
int better = -1;
int i;
double s;
double maxs;
double *vrt;
/* Initialise variables */
maxs = dotprod (body->coord[0],v);
body->s[0] = body->coord[0][0];
body->s[1] = body->coord[0][1];
body->s[2] = body->coord[0][2];
maxs = dotProduct(body->s, v);
for (i = 1; i < body->numpoints; ++i) {
vrt = body->coord[i];
/* Evaluate function */
s = dotprod (vrt,v);
s = dotProduct (vrt, v);
if ( s > maxs ){
maxs = s;
body->s[0] = vrt[0];
body->s[1] = vrt[1];
body->s[2] = vrt[2];
}
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];
}
}
/**
* @brief Signed Volumes method.
*/
void subalgorithm ( struct simplex *s ) {
inline static void subalgorithm( struct simplex *s, double *v ) {
switch ( s->nvrtx ){
case 4:
S3D( s );
S3D( s, v );
break;
case 3:
S2D( s );
S2D( s, v );
break;
case 2:
S1D( s );
break;
default :
s->lambdas[0] = 1.0;
S1D( s, v );
break;
}
}
/**
* @brief ~The GJK algorithm
*
*/
double gjk( struct bd bd1, struct bd bd2, struct simplex *s) {
int i;
int k = 0; /**< Iteration counter */
int i; /**< General purpose counter */
int mk = 100; /**< Maximum number of iterations of the GJK algorithm */
double v[3]; /**< Search direction */
double vminus[3]; /**< Search direction */
double w[3]; /**< Vertex on CSO frontier */
double eps_rel = 1e-10; /**< Tolerance on relative */
double eps_tot = 1e-13;
double dd = -1; /**< Squared distance */
int maxitreached = 0; /**< Flag for maximum iterations */
int origininsimplex = 0; /**< Flag for origin in simples */
int mk = 50; /**< Maximum number of iterations of the GJK algorithm */
int exeedtol_rel = 0; /**< Flag for 1st exit condition */
int exeedtol_tot = 0; /**< Flag for 2nd exit condition */
int nullV = 0; /**< Flag for 3rd exit condition */
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 = 1e-5; /**< Tolerance on relative distance */
double eps_tot = 1e-15; /**< Tolerance on absolute distance */
double norm2Wmax = 0;
double tesnorm = 0;
#ifdef ADAPTIVEFP
exactinit();
#endif
/* Initialise search direction */
for ( i = 0; i < 3; ++i) {
double eps_rel2 = eps_rel * eps_rel ;
/* Initialise */
s->nvrtx = 1;
for ( i = 0; i < 3; i++)
{
v[i] = bd1.coord[0][i] - bd2.coord[0][i];
vminus[i] = -v[i];
bd1.s[i] = bd1.coord[0][i];
bd2.s[i] = bd2.coord[0][i];
s->vrtx[0][i] = v[i];
}
/* Begin GJK iteration */
do {
/* Increment iteration counter */
k++;
/* Support function on polytope A */
support( &bd1 , vminus );
/* Support function on polytope B */
support( &bd2 , v );
/* Minkowski difference */
for ( i = 0; i < 3; ++i) {
w[i] = bd1.s[i] - bd2.s[i];
}
/* Update negative search direction */
vminus[0] = -v[0];
vminus[1] = -v[1];
vminus[2] = -v[2];
/* Test first exit condition (can't move further) */
exeedtol_rel = (norm2(v) - dotprod (v,w) ) <= eps_rel * norm2(v);
/* Support function */
support( &bd1 , vminus );
support( &bd2 , v );
w[0] = bd1.s[0] - bd2.s[0];
w[1] = bd1.s[1] - bd2.s[1];
w[2] = bd1.s[2] - bd2.s[2];
/* 1st exit condition */
exeedtol_rel = (norm2(v) - dotProduct (v,w) ) <= eps_rel2 * norm2(v);
if ( exeedtol_rel ){
break;
}
nullV = norm2(v) < eps_rel;
if (nullV) {
/* 2nd exit condition */
if (norm2(v) < eps_rel2)
break;
}
/* Add support vertex to simplex at the position nvrtx+1 */
/* Add new vertex to simplex */
i = s->nvrtx;
s->vrtx[i][0] = w[0];
s->vrtx[i][1] = w[1];
s->vrtx[i][2] = w[2];
s->nvrtx++;
for ( i = 0; i < 3; ++i) {
s->vrtx[ s->nvrtx - 1 ][i] = w[i];
/* Invoke distance sub-algorithm */
subalgorithm ( s, v );
/* 3rd exit condition */
for (i = 0; i < s->nvrtx; i++)
{
tesnorm = norm2(s->vrtx[i]);
if (tesnorm > norm2Wmax) {
norm2Wmax = tesnorm;
}
/* Invoke sub-distance algorithm */
subalgorithm ( s );
/* Termination tests */
maxitreached = k == mk;
origininsimplex = s->nvrtx == 4;
exeedtol_tot = norm2 (v) <= eps_tot ;
/* Update search direction */
for ( i = 0; i < 3; ++i) {
v[i] = 0;
}
if (norm2(v) <= (eps_tot * eps_tot * norm2Wmax))
break;
for ( i = 0; i < s->nvrtx; ++i) {
v[0] += (s->lambdas[i] * s->vrtx[i][0]);
v[1] += (s->lambdas[i] * s->vrtx[i][1]);
v[2] += (s->lambdas[i] * s->vrtx[i][2]);
}
for ( i = 0; i < 3; ++i) {
vminus[i] = -v[i];
}
/* 4th and 5th exit conditions */
} while((s->nvrtx != 4) && (k != mk));
}while( !maxitreached && !origininsimplex && !exeedtol_tot && !exeedtol_rel );
/* Compute distance */
dd = sqrt ( norm2 (v));
return dd;
/* Compute and return distance */
return sqrt(norm2(v));
}
#ifdef MATLABDOESMEXSTUFF