Merge pull request #4 from JohnnyMudcrab/master
added functionality to calculate witness pointsfixes-turtlebasket
commit
e1e8c71686
|
@ -67,6 +67,8 @@ struct simplex {
|
|||
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. */
|
||||
double p [4][3]; /**< Points of P that form the simplex */
|
||||
double q [4][3]; /**< Points of Q that form the simplex */
|
||||
};
|
||||
|
||||
|
||||
|
|
|
@ -779,6 +779,8 @@ double gjk( struct bd bd1, struct bd bd2, struct simplex *s) {
|
|||
bd1.s[i] = bd1.coord[0][i];
|
||||
bd2.s[i] = bd2.coord[0][i];
|
||||
s->vrtx[0][i] = v[i];
|
||||
s->p[0][i] = bd1.s[i];
|
||||
s->q[0][i] = bd2.s[i];
|
||||
}
|
||||
|
||||
/* Begin GJK iteration */
|
||||
|
@ -818,6 +820,12 @@ double gjk( struct bd bd1, struct bd bd2, struct simplex *s) {
|
|||
/* Invoke distance sub-algorithm */
|
||||
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 */
|
||||
for (i = 0; i < s->nvrtx; i++)
|
||||
{
|
||||
|
|
Loading…
Reference in New Issue