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

added functionality to calculate witness points

fixes-turtlebasket
Magnus Hanses 2019-10-18 10:47:09 +02:00
parent 93b98359e7
commit f71af1563f
2 changed files with 10 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -67,6 +67,8 @@ struct simplex {
double vrtx [4][3]; /**< Coordinates of simplex's vertices. */ double vrtx [4][3]; /**< Coordinates of simplex's vertices. */
int wids [4]; /**< Label of the simplex's vertices. */ int wids [4]; /**< Label of the simplex's vertices. */
double lambdas [4]; /**< Barycentric coordiantes for each vertex. */ double lambdas [4]; /**< Barycentric coordiantes for each vertex. */
double p [4][3]; /**< Points of P that form the simplex */
double q [4][3]; /**< Points of Q that form the simplex */
}; };

8
lib/src/openGJK.c
View File

@ -779,6 +779,8 @@ double gjk( struct bd bd1, struct bd bd2, struct simplex *s) {
bd1.s[i] = bd1.coord[0][i]; bd1.s[i] = bd1.coord[0][i];
bd2.s[i] = bd2.coord[0][i]; bd2.s[i] = bd2.coord[0][i];
s->vrtx[0][i] = v[i]; s->vrtx[0][i] = v[i];
s->p[0][i] = bd1.s[i];
s->q[0][i] = bd2.s[i];
} }
/* Begin GJK iteration */ /* Begin GJK iteration */
@ -818,6 +820,12 @@ double gjk( struct bd bd1, struct bd bd2, struct simplex *s) {
/* Invoke distance sub-algorithm */ /* Invoke distance sub-algorithm */
subalgorithm ( s, v ); subalgorithm ( s, v );
for (i = 0; i < 3; i++)
{
s->p[s->nvrtx - 1][i] = bd1.s[i];
s->q[s->nvrtx - 1][i] = bd2.s[i];
}
/* 3rd exit condition */ /* 3rd exit condition */
for (i = 0; i < s->nvrtx; i++) for (i = 0; i < s->nvrtx; i++)
{ {