#include "gjk.h"

/*
 * Implementation of the Gilbert, Johnson and Keerthi routine to compute
 * the minimum distance between two convex polyhedra.
 *
 * Version 2.1, April 1997, (c) Stephen Cameron 1996, 1997
 *
 */
/* Scrambled using scramble 1.0 (September 1996) on Wed Apr  9 18:42:22 1997
 */
#define TWO_TO_DIM (1<<DIM) 
#define DIM_PLUS_ONE (DIM+1)
#define TWICE_TWO_TO_DIM (TWO_TO_DIM+TWO_TO_DIM)
#define EPSILON_SQ (EPSILON*EPSILON)
#define overd( ct) for ( ct=0 ; ct<DIM ; ct++ )
#define card( s) cardinality[s]
#define max_elt( s) max_element[s]
#define elts( s, i) elements[s][i]
#define non_elts( s, i) non_elements[s][i]
#define pred( s, i) predecessor[s][i]
#define succ( s, i) successor[s][i]
#define delta( s, i) delta_values[s][i]
#define prod( i, j) dot_products[i][j]
static int cardinality[TWICE_TWO_TO_DIM];
static int max_element[TWICE_TWO_TO_DIM];
static int elements[TWICE_TWO_TO_DIM][DIM_PLUS_ONE];
static int non_elements[TWICE_TWO_TO_DIM][DIM_PLUS_ONE];
static int predecessor[TWICE_TWO_TO_DIM][DIM_PLUS_ONE];
static int successor[TWICE_TWO_TO_DIM][DIM_PLUS_ONE];
static REAL delta_values[TWICE_TWO_TO_DIM][DIM_PLUS_ONE];
static REAL dot_products[DIM_PLUS_ONE][DIM_PLUS_ONE];
#ifdef GATHER_STATISTICS
int gjk_num_g_test, gjk_num_simplices, gjk_num_backups,
gjk_num_dot_products, gjk_num_support_dp, gjk_num_other_ops;
#define OTHER_DOT_PRODUCT( a, b) \
( gjk_num_dot_products++, DOT_PRODUCT( a, b) )
#define SUPPORT_DOT_PRODUCT( a, b) \
( gjk_num_support_dp++, OTHER_DOT_PRODUCT( a, b) )
#define DO_MULTIPLY( a, b) \
( gjk_num_other_ops++, MULTIPLY( a, b) )
#define DO_DIVIDE( a, b) \
( gjk_num_other_ops++, DIVIDE( a, b) )
#else 
#define OTHER_DOT_PRODUCT( a, b) DOT_PRODUCT( a, b)
#define SUPPORT_DOT_PRODUCT( a, b) DOT_PRODUCT( a, b)
#define DO_MULTIPLY( a, b) MULTIPLY( a, b)
#define DO_DIVIDE( a, b) DIVIDE( a, b)
#endif 
static void initialise_simplex_distance( void);
static INDEX support_function( INDEX, REAL (*pts)[DIM], INDEX *,
INDEX, REAL *, REAL *);
static int simplex_distance( struct simplex_point * simplex);
static int gjk_backup( int size, REAL delta[TWICE_TWO_TO_DIM]);
static void compute_subterms( struct simplex_point * s);
static void compute_point( REAL pt[DIM], int len,
REAL (* vertices)[DIM], REAL lambdas[]);
static void add_simplex_vertex( struct simplex_point * s, int pos,
REAL * v1, REAL (* t1)[DIM_PLUS_ONE], REAL * v2, REAL (* t2)[DIM_PLUS_ONE]);
static void apply_trans( REAL (*)[DIM_PLUS_ONE], REAL *, REAL *);
static void apply_rot_transpose( REAL (*)[DIM_PLUS_ONE], REAL *, REAL *);
REAL gjk_distance(
int npts1, REAL (* pts1)[DIM], INDEX * rings1, REAL (* tr1)[DIM_PLUS_ONE],
int npts2, REAL (* pts2)[DIM], INDEX * rings2, REAL (* tr2)[DIM_PLUS_ONE],
REAL wpt1[DIM], REAL wpt2[DIM],
struct simplex_point * simplex, int use_seed
)
{
int i, p, d, v, compute_both_witnesses;
INDEX maxp, minp;
REAL minus_minv, maxv, sqrd, g_val;
REAL displacementv[DIM], reverse_displacementv[DIM];
REAL local_witness1[DIM], local_witness2[DIM];
REAL local_fdisp[DIM], local_rdisp[DIM], trv[DIM];
REAL * fdisp, * rdisp;
struct simplex_point local_simplex;
#ifdef PARANOID
if ( npts1<1 || npts2<1 )
{
fatal( "Bad arguments to gjk_distance");
}
#endif
compute_both_witnesses = ( wpt1!=0 ) || ( wpt2!=0 ) ||
( tr1!=0 ) || ( tr2!=0 );
if ( wpt1==0 )
wpt1 = local_witness1;
if ( wpt2==0 )
wpt2 = local_witness2;
fdisp = ( tr1==0 ) ? displacementv : local_fdisp;
rdisp = ( tr2==0 ) ? reverse_displacementv : local_rdisp;
if ( simplex==0 )
{
use_seed = 0;
simplex = & local_simplex;
}
if ( use_seed==0 )
{
simplex->simplex1[0] = 0; simplex->simplex2[0] = 0;
simplex->npts = 1; simplex->lambdas[0] = ONE;
simplex->last_best1 = 0; simplex->last_best2 = 0;
add_simplex_vertex( simplex, 0, pts1[0], tr1, pts2[0], tr2);
}
else
{
for ( v=0 ; v<simplex->npts ; v++ )
add_simplex_vertex( simplex, v,
pts1[simplex->simplex1[v]], tr1, pts2[simplex->simplex2[v]], tr2);
simplex->npts = simplex_distance( simplex);
}
#ifdef GATHER_STATISTICS
gjk_num_simplices = ( use_seed ) ? 1 : 0;
gjk_num_g_test = gjk_num_backups =
gjk_num_dot_products = gjk_num_support_dp = gjk_num_other_ops = 0;
for ( ; gjk_num_g_test<npts1 * npts2 ; )
#else
while ( 1 )
#endif
{
if ( compute_both_witnesses )
{
compute_point( wpt1, simplex->npts, simplex->coords1,
simplex->lambdas);
compute_point( wpt2, simplex->npts, simplex->coords2,
simplex->lambdas);
overd( d)
{
displacementv[ d] = wpt2[d] - wpt1[d];
reverse_displacementv[ d] = - displacementv[d];
}
}
else
{
overd( d)
{
displacementv[d] = 0;
for ( p=0 ; p<simplex->npts ; p++ )
displacementv[d] +=
DO_MULTIPLY( simplex->lambdas[p],
simplex->coords2[p][d] - simplex->coords1[p][d]);
reverse_displacementv[ d] = - displacementv[d];
}
}
sqrd = OTHER_DOT_PRODUCT( displacementv, displacementv);
if ( sqrd<EPSILON_SQ || simplex->npts==DIM_PLUS_ONE )
return 0.0; 
if ( tr1!=0 )
apply_rot_transpose( tr1, displacementv, fdisp);
if ( tr2!=0 )
apply_rot_transpose( tr2, reverse_displacementv, rdisp);
maxp = support_function( npts1, pts1, rings1,
( use_seed ? simplex->last_best1 : -1),
&maxv, fdisp
);
minp = support_function( npts2, pts2, rings2,
( use_seed ? simplex->last_best2 : -1),
&minus_minv, rdisp
);
#ifdef GATHER_STATISTICS
gjk_num_g_test++;
#endif
g_val = sqrd + maxv + minus_minv;
if ( tr1!=0 )
{
overd( i) trv[i] = tr1[i][DIM];
g_val += OTHER_DOT_PRODUCT( displacementv, trv);
}
if ( tr2!=0 )
{
overd( i) trv[i] = tr2[i][DIM];
g_val += OTHER_DOT_PRODUCT( reverse_displacementv, trv);
}
if ( g_val <= EPSILON )
{
return sqrd;
}
simplex->simplex1[ simplex->npts] = simplex->last_best1 = maxp;
simplex->simplex2[ simplex->npts] = simplex->last_best2 = minp;
simplex->lambdas[ simplex->npts] = ZERO;
add_simplex_vertex( simplex, simplex->npts,
pts1[maxp], tr1, pts2[minp], tr2);
simplex->npts++;
simplex->npts = simplex_distance( simplex);
}
return 0.0; 
}
static INDEX
support_function(
INDEX npts, REAL (*pts)[DIM], INDEX * rings,
INDEX start, REAL * supportval, REAL * direction)
{
INDEX p, maxp, lastvisited, neighbour, index;
REAL maxv, thisv;
if ( rings==0 )
{
maxv = SUPPORT_DOT_PRODUCT( pts[0], direction);
maxp = 0;
for ( p=1 ; p<npts ; p++ )
{
thisv = SUPPORT_DOT_PRODUCT( pts[p], direction);
if ( thisv>maxv )
{
maxv = thisv;
maxp = p;
}
}
*supportval = maxv;
return maxp;
}
p = lastvisited = -1;
maxp = ( start<0 ) ? 0 : start;
maxv = SUPPORT_DOT_PRODUCT( pts[maxp], direction);
while ( p != maxp )
{
p = maxp;
for ( index = rings[p] ; (neighbour = rings[index]) >= 0 ; index++ )
{
if ( neighbour==lastvisited )
continue;
thisv = SUPPORT_DOT_PRODUCT( pts[neighbour], direction);
if ( thisv>maxv )
{
maxv = thisv;
maxp = neighbour;
#ifdef EAGER_HILL_CLIMBING
break;
#endif
}
}
lastvisited = p;
}
*supportval = maxv;
return p;
}
static int simplex_distance( struct simplex_point * simplex)
{
int s, i, j, k, ok, size;
REAL delta[TWICE_TWO_TO_DIM];
INDEX oldpos;
initialise_simplex_distance();
size = simplex->npts;
#ifdef GATHER_STATISTICS
gjk_num_simplices++;
#endif
#ifdef PARANOID
if ( size<=0 || size>DIM_PLUS_ONE )
{
fatal( "Bad Call in simplex_distance");
}
#endif
if ( size==1 )
{
simplex->lambdas[0] = ONE;
return 1;
}
compute_subterms( simplex);
for ( s=1 ; s < TWICE_TWO_TO_DIM && max_elt( s) < size ; s++ )
{
delta[s] = ZERO; ok=1;
for ( j=0 ; ok && j<card( s) ; j++ )
{
if ( delta( s, elts( s, j))>ZERO )
delta[s] += delta( s, elts( s, j));
else
ok = 0;
}
for ( k=0 ; ok && k < size - card( s) ; k++ )
{
if ( delta( succ( s, k), non_elts( s, k))>0 )
ok = 0;
}
#ifdef TEST_BACKUP_PROCEDURE
ok = 0;
#endif
if ( ok && delta[s]>=TINY ) 
break;
}
if ( !ok )
{
s = gjk_backup( size, delta);
}
for ( j=0 ; j<card( s) ; j++ )
{
oldpos = elts( s, j);
if ( oldpos!=j )
{
simplex->simplex1[j] = simplex->simplex1[oldpos];
simplex->simplex2[j] = simplex->simplex2[oldpos];
overd(i ) {
simplex->coords1[j][i] = simplex->coords1[oldpos][i];
simplex->coords2[j][i] = simplex->coords2[oldpos][i];
}
}
simplex->lambdas[j] = DO_DIVIDE( delta( s, elts( s, j)), delta[s]);
}
simplex->npts = card( s);
return card( s);
}
static int gjk_backup( int size, REAL delta[TWICE_TWO_TO_DIM])
{
int s, i, j, k, bests;
REAL distsq_num[TWICE_TWO_TO_DIM], distsq_den[TWICE_TWO_TO_DIM];
#ifdef GATHER_STATISTICS
gjk_num_backups++;
#endif
bests = 0;
for ( s=1 ; s < TWICE_TWO_TO_DIM && max_elt( s) < size ; s++ )
{
if ( delta[s] <= ZERO )
continue;
for ( i=0 ; i<card( s) ; i++ )
if ( delta( s, elts( s, i))<=ZERO )
break;
if ( i < card( s) )
continue;
distsq_num[s] = ZERO;
for ( j=0 ; j<card( s) ; j++ )
for ( k=0 ; k<card( s) ; k++ )
distsq_num[s] += DO_MULTIPLY( 
DO_MULTIPLY( delta( s, elts( s, j)), delta( s, elts( s, k))),
prod( elts( s, j), elts( s, k))
);
distsq_den[s] = DO_MULTIPLY( delta[s], delta[s]);
if ( (bests < 1) ||
( DO_MULTIPLY( distsq_num[s], distsq_den[bests]) <
DO_MULTIPLY( distsq_num[bests], distsq_den[s]) )
)
bests = s;
}
return bests;
}
static void compute_subterms( struct simplex_point * simp)
{
int i, j, ielt, jelt, s, jsubset, size = simp->npts;
REAL sum, c_space_points[DIM_PLUS_ONE][DIM];
for ( i=0 ; i<size ; i++ )
for ( j=0 ; j<DIM ; j++ )
c_space_points[i][j] =
simp->coords1[i][j] - simp->coords2[i][j];
for ( i=0 ; i<size ; i++ )
for ( j=i ; j<size ; j++ )
prod( i, j) = prod( j, i) =
OTHER_DOT_PRODUCT( c_space_points[i], c_space_points[j]);
for ( s=1 ; s<TWICE_TWO_TO_DIM && max_elt( s) < size ; s++ )
{
if ( card( s)<=1 ) 
{
delta( s, elts( s, 0)) = ONE;
continue;
}
if ( card( s)==2 ) 
{
delta( s, elts( s, 0)) =
prod( elts( s, 1), elts( s, 1)) -
prod( elts( s, 1), elts( s, 0));
delta( s, elts( s, 1)) =
prod( elts( s, 0), elts( s, 0)) -
prod( elts( s, 0), elts( s, 1));
continue;
}
for ( j=0 ; j<card( s) ; j++ )
{
jelt = elts( s, j);
jsubset = pred( s, j);
sum = 0;
for ( i=0 ; i < card( jsubset) ; i++ )
{
ielt = elts( jsubset, i);
sum += DO_MULTIPLY(
delta( jsubset, ielt ),
prod( ielt, elts( jsubset, 0)) - prod( ielt, jelt)
);
}
delta( s, jelt) = sum;
}
}
return;
}
int gjk_extract_point( struct simplex_point *simp,
int whichpoint, REAL vector[])
{
REAL (* coords)[DIM];
if ( whichpoint!=1 && whichpoint !=2 )
{
bad( "Bad indicator argument to gjk_extract_point");
return 0;
}
coords = ( whichpoint==1 ) ? simp->coords1 : simp->coords2;
compute_point( vector, simp->npts, coords, simp->lambdas);
return 1;
}
static void compute_point( REAL pt[DIM], int len, REAL (* vertices)[DIM],
REAL lambdas[]) 
{
int d, i;
overd( d)
{
pt[d] = 0;
for ( i=0 ; i<len ; i++ )
pt[d] += DO_MULTIPLY( vertices[i][d], lambdas[i]);
}
return;
}
static void add_simplex_vertex( struct simplex_point * s, int pos,
REAL * v1, REAL (* t1)[DIM_PLUS_ONE], REAL * v2, REAL (* t2)[DIM_PLUS_ONE])
{
int c;
if ( t1==0 )
overd( c) s->coords1[pos][c] = v1[c];
else
apply_trans( t1, v1, s->coords1[pos]);
if ( t2==0 )
overd( c) s->coords2[pos][c] = v2[c];
else
apply_trans( t2, v2, s->coords2[pos]);
return;
}
static void apply_trans( REAL (* t)[DIM_PLUS_ONE], REAL * src, REAL * tgt)
{
int i;
overd( i)
tgt[i] = t[i][DIM] + OTHER_DOT_PRODUCT( t[i], src);
return;
}
static void
apply_rot_transpose( REAL (* t)[DIM_PLUS_ONE], REAL * src, REAL * tgt)
{
int i;
overd( i)
tgt[i] = DO_MULTIPLY( t[0][i], src[0]) + DO_MULTIPLY( t[1][i], src[1])
+ DO_MULTIPLY( t[2][i], src[2]);
return;
}
static int simplex_distance_initialised = 0;
static void initialise_simplex_distance( void)
{
int power, d, s, e, two_to_e, next_elt, next_non_elt, pr;
int num_succ[TWICE_TWO_TO_DIM];
if ( simplex_distance_initialised )
return;
power = 1;
for ( d=0 ; d<DIM ; d++ )
power *= 2;
if ( power != TWO_TO_DIM )
{
fatal( "TWO_TO_DIM and DIM disagree!");
}
for ( s=0 ; s<TWICE_TWO_TO_DIM ; s++ )
num_succ[s] = 0;
for ( s=1 ; s<TWICE_TWO_TO_DIM ; s++ )
{
two_to_e = 1;
next_elt = next_non_elt = 0;
for ( e=0 ; e<DIM_PLUS_ONE ; e++ )
{
if ( (s/two_to_e) % 2 == 1 )
{
elts( s, next_elt) = e;
pr = s - two_to_e;
pred( s, next_elt) = pr;
succ( pr, num_succ[pr]) = s;
num_succ[ pr]++;
next_elt++;
}
else
non_elts( s, next_non_elt++) = e;
two_to_e *= 2;
}
card( s) = next_elt;
max_elt( s) = elts( s, next_elt-1 );
}
card( 0) = 0; max_elt( 0) = -1;
for ( e=0 ; e<DIM_PLUS_ONE ; e++ )
non_elts( 0, e) = e;
simplex_distance_initialised = 1;
return;
}