#include "gjk.h"

/*
 * Implementation of the Gilbert, Johnson and Keerthi routine to compute
 * the minimum distance between two convex polyhedra.
 *
 * Version 2.0, September 1996, (c) Stephen Cameron
 *
 */
/* Scrambled using scramble 1.0 (September 1996) on Fri Sep 27 11:37:58 1996
 */
#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 jkfob( ct) for ( ct=0 ; ct<DIM ; ct++ )
#define tevfy( s) cvhux[s]
#define vbtir( s) xuqyk[s]
#define txqcq( s, i) nhsvl[s][i]
#define aryov( s, i) nxxvf[s][i]
#define hhhcv( s, i) bgijk[s][i]
#define alfic( s, i) rqmer[s][i]
#define jdgei( s, i) xjmgd[s][i]
#define gblzj( i, j) lgaeg[i][j]
static int cvhux[TWICE_TWO_TO_DIM];
static int xuqyk[TWICE_TWO_TO_DIM];
static int nhsvl[TWICE_TWO_TO_DIM][DIM_PLUS_ONE];
static int nxxvf[TWICE_TWO_TO_DIM][DIM_PLUS_ONE];
static int bgijk[TWICE_TWO_TO_DIM][DIM_PLUS_ONE];
static int rqmer[TWICE_TWO_TO_DIM][DIM_PLUS_ONE];
static REAL xjmgd[TWICE_TWO_TO_DIM][DIM_PLUS_ONE];
static REAL lgaeg[DIM_PLUS_ONE][DIM_PLUS_ONE];
static REAL (* krnem)[DIM], (* zumyp)[DIM];
static int rzxai, htyvz;
static int * qvieb, * msjow;
#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 pandf( a, b) \
( gjk_num_dot_products++, DOT_PRODUCT( a, b) )
#define cezoc( a, b) \
( gjk_num_support_dp++, pandf( a, b) )
#define qfdnh( a, b) \
( gjk_num_other_ops++, MULTIPLY( a, b) )
#define lucms( a, b) \
( gjk_num_other_ops++, DIVIDE( a, b) )
#else 
#define pandf( a, b) DOT_PRODUCT( a, b)
#define cezoc( a, b) DOT_PRODUCT( a, b)
#define qfdnh( a, b) MULTIPLY( a, b)
#define lucms( a, b) DIVIDE( a, b)
#endif 
static void cenkk( void);
static INDEX qycao( INDEX, REAL (*pts)[DIM], INDEX *,
INDEX, REAL *, REAL *);
static int yppms( struct simplex_point * grloj);
static int uqyte( int vrrmz, REAL jdgei[TWICE_TWO_TO_DIM]);
static void cjlfy( int vrrmz, INDEX abgrd[], INDEX knnkz[]);
static void vtsxf( REAL esybg[DIM], int nzsdb,
REAL (* xyppg)[DIM], INDEX pippf[], REAL lambdas[]);
int gjk_extract_point( struct simplex_point *pippf, REAL (*pts)[DIM],
int whichpoint, REAL vector[]);
REAL gjk_distance(
int mblzm, REAL (* bwhqk)[DIM], INDEX * gdqui,
int vqmyq, REAL (* ckaaf)[DIM], INDEX * opbio,
REAL * ivqtw, REAL * epoqp,
struct simplex_point * grloj, int yqvas
)
{
int p, d, ahvqb;
INDEX vexsk, umhvk;
REAL hbfhc, lvbfa, eczvt, fmbuc, g_val;
REAL ggsjp[DIM], gsmyn[DIM];
REAL icezv[DIM], tfohv[DIM];
REAL yfnvf[DIM_PLUS_ONE][DIM];
struct simplex_point gwodo;
#ifdef PARANOID
if ( mblzm<1 || vqmyq<1 )
{
fatal( "Bad arguments to gjk_distance");
}
#endif
krnem = bwhqk; zumyp = ckaaf;
rzxai = mblzm; htyvz = vqmyq;
qvieb = gdqui; msjow = opbio;
ahvqb = ( ivqtw!=0 ) || ( epoqp!=0 );
if ( ivqtw==0 )
ivqtw = icezv;
if ( epoqp==0 )
epoqp = tfohv;
if ( grloj==0 )
{
yqvas = 0;
grloj = & gwodo;
}
if ( yqvas==0 )
{
grloj->simplex1[0] = 0; grloj->simplex2[0] = 0;
grloj->npts = 1; grloj->lambdas[0] = ONE;
grloj->last_best1 = 0; grloj->last_best2 = 0;
}
else
grloj->npts = yppms( grloj);
#ifdef GATHER_STATISTICS
gjk_num_simplices = ( yqvas ) ? 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<rzxai * htyvz ; )
#else
while ( 1 )
#endif
{
if ( ahvqb )
{
vtsxf( ivqtw, grloj->npts, krnem,
grloj->simplex1, grloj->lambdas);
vtsxf( epoqp, grloj->npts, zumyp,
grloj->simplex2, grloj->lambdas);
jkfob( d)
{
ggsjp[ d] = epoqp[d] - ivqtw[d];
gsmyn[ d] = - ggsjp[d];
}
}
else
{
jkfob( d)
{
ggsjp[d] = 0;
for ( p=0 ; p<grloj->npts ; p++ )
ggsjp[d] +=
qfdnh( grloj->lambdas[p],
zumyp[ grloj->simplex2[p]][d] -
krnem[ grloj->simplex1[p]][d]);
gsmyn[ d] = - ggsjp[d];
}
}
fmbuc = pandf( ggsjp, ggsjp);
if ( fmbuc<EPSILON_SQ || grloj->npts==DIM_PLUS_ONE )
return 0.0; 
vexsk = qycao( rzxai, krnem, qvieb,
( yqvas ? grloj->last_best1 : -1),
&eczvt, ggsjp
);
umhvk = qycao( htyvz, zumyp, msjow,
( yqvas ? grloj->last_best2 : -1),
&lvbfa, gsmyn
);
gjk_num_g_test++;
g_val = fmbuc + eczvt + lvbfa;
if ( g_val <= EPSILON )
{
return fmbuc;
}
grloj->simplex1[ grloj->npts] = grloj->last_best1 = vexsk;
grloj->simplex2[ grloj->npts] = grloj->last_best2 = umhvk;
grloj->lambdas[ grloj->npts] = ZERO;
grloj->npts++;
grloj->npts = yppms( grloj);
}
return 0.0; 
}
static INDEX
qycao(
INDEX npts, REAL (*pts)[DIM], INDEX * akxhj,
INDEX rjxjt, REAL * msnas, REAL * sargj)
{
INDEX p, vexsk, mgocc, wiakl, oexnp;
REAL eczvt, hbfhc;
#ifdef TIGHT_LOOP
INDEX *p_nextv;
INDEX szhfz;
#endif
if ( akxhj==0 )
{
eczvt = cezoc( pts[0], sargj);
vexsk = 0;
for ( p=1 ; p<npts ; p++ )
{
hbfhc = cezoc( pts[p], sargj);
if ( hbfhc>eczvt )
{
eczvt = hbfhc;
vexsk = p;
}
}
*msnas = eczvt;
return vexsk;
}
#ifndef TIGHT_LOOP
p = mgocc = -1;
vexsk = ( rjxjt<0 ) ? 0 : rjxjt;
eczvt = cezoc( pts[vexsk], sargj);
while ( p != vexsk )
{
p = vexsk;
for ( oexnp = akxhj[p] ; (wiakl = akxhj[oexnp]) >= 0 ; oexnp++ )
{
if ( wiakl==mgocc )
continue;
hbfhc = cezoc( pts[wiakl], sargj);
if ( hbfhc>eczvt )
{
eczvt = hbfhc;
vexsk = wiakl;
#ifdef EAGER_HILL_CLIMBING
break;
#endif
}
}
mgocc = p;
}
*msnas = eczvt;
return p;
#else 
#ifndef EAGER_HILL_CLIMBING
vexsk =
#endif
mgocc = -1;
p = ( rjxjt<0 ) ? 0 : rjxjt;
p_nextv = akxhj+akxhj[p];
eczvt = cezoc( pts[p], sargj);
szhfz = *p_nextv;
#ifdef EAGER_HILL_CLIMBING
while ( szhfz>=0 )
{
if ( ( szhfz == mgocc ) ||
( (hbfhc = cezoc( pts[szhfz], sargj))<=eczvt )
)
{
p_nextv++;
szhfz = *p_nextv;
continue;
}
eczvt = hbfhc;
mgocc = p;
p = szhfz;
p_nextv = akxhj+akxhj[p];
szhfz = *p_nextv;
}
#else 
while ( szhfz>=0 )
{
if ( ( szhfz == mgocc ) ||
( (hbfhc = cezoc( pts[szhfz], sargj))<=eczvt )
)
{
p_nextv++;
szhfz = *p_nextv;
if ( szhfz<0 && vexsk>=0 )
{
mgocc = p;
p = vexsk;
p_nextv = akxhj+akxhj[p];
szhfz = *p_nextv;
vexsk = -1;
}
continue;
}
eczvt = hbfhc;
vexsk = szhfz;
p_nextv++;
szhfz = *p_nextv;
if ( szhfz<0 )
p = vexsk;
}
#endif 
*msnas = eczvt;
return p;
#endif 
}
static int yppms( struct simplex_point * grloj)
{
int s, i, j, k, chmzo, vrrmz;
INDEX abgrd[DIM_PLUS_ONE], knnkz[DIM_PLUS_ONE];
REAL jdgei[TWICE_TWO_TO_DIM];
cenkk();
vrrmz = grloj->npts;
#ifdef GATHER_STATISTICS
gjk_num_simplices++;
#endif
#ifdef PARANOID
if ( vrrmz<=0 || vrrmz>DIM_PLUS_ONE )
{
fatal( "Bad Call in yppms");
}
#endif
if ( vrrmz==1 )
{
grloj->lambdas[0] = ONE;
return 1;
}
for ( i=0 ; i<vrrmz ; i++ )
{
abgrd[i] = grloj->simplex1[i];
knnkz[i] = grloj->simplex2[i];
}
cjlfy( vrrmz, abgrd, knnkz);
for ( s=1 ; s < TWICE_TWO_TO_DIM && vbtir( s) < vrrmz ; s++ )
{
jdgei[s] = ZERO; chmzo=1;
for ( j=0 ; chmzo && j<tevfy( s) ; j++ )
{
if ( jdgei( s, txqcq( s, j))>ZERO )
jdgei[s] += jdgei( s, txqcq( s, j));
else
chmzo = 0;
}
for ( k=0 ; chmzo && k < vrrmz - tevfy( s) ; k++ )
{
if ( jdgei( alfic( s, k), aryov( s, k))>0 )
chmzo = 0;
}
#ifdef TEST_BACKUP_PROCEDURE
chmzo = 0;
#endif
if ( chmzo && jdgei[s]>=TINY ) 
break;
}
if ( !chmzo )
{
s = uqyte( vrrmz, jdgei);
}
for ( j=0 ; j<tevfy( s) ; j++ )
{
grloj->simplex1[j] = abgrd[ txqcq( s, j)];
grloj->simplex2[j] = knnkz[ txqcq( s, j)];
grloj->lambdas[j] = lucms( jdgei( s, txqcq( s, j)), jdgei[s]);
}
grloj->npts = tevfy( s);
return tevfy( s);
}
static int uqyte( int vrrmz, REAL jdgei[TWICE_TWO_TO_DIM])
{
int s, i, j, k, yadxo;
REAL tppcw[TWICE_TWO_TO_DIM], reedd[TWICE_TWO_TO_DIM];
#ifdef GATHER_STATISTICS
gjk_num_backups++;
#endif
yadxo = 0;
for ( s=1 ; s < TWICE_TWO_TO_DIM && vbtir( s) < vrrmz ; s++ )
{
if ( jdgei[s] <= ZERO )
continue;
for ( i=0 ; i<tevfy( s) ; i++ )
if ( jdgei( s, txqcq( s, i))<=ZERO )
break;
if ( i < tevfy( s) )
continue;
tppcw[s] = ZERO;
for ( j=0 ; j<tevfy( s) ; j++ )
for ( k=0 ; k<tevfy( s) ; k++ )
tppcw[s] += qfdnh( 
qfdnh( jdgei( s, txqcq( s, j)), jdgei( s, txqcq( s, k))),
gblzj( txqcq( s, j), txqcq( s, k))
);
reedd[s] = qfdnh( jdgei[s], jdgei[s]);
if ( (yadxo < 1) ||
( qfdnh( tppcw[s], reedd[yadxo]) <
qfdnh( tppcw[yadxo], reedd[s]) )
)
yadxo = s;
}
return yadxo;
}
static void cjlfy( int vrrmz, INDEX simplex1[], INDEX simplex2[])
{
int i, j, uywln, wmqcd, s, tosgy;
REAL osveu, c_space_points[DIM_PLUS_ONE][DIM];
for ( i=0 ; i<vrrmz ; i++ )
for ( j=0 ; j<DIM ; j++ )
c_space_points[i][j] =
krnem[ simplex1[i]][j] - zumyp[ simplex2[i]][j];
for ( i=0 ; i<vrrmz ; i++ )
for ( j=i ; j<vrrmz ; j++ )
gblzj( i, j) = gblzj( j, i) =
pandf( c_space_points[i], c_space_points[j]);
for ( s=1 ; s<TWICE_TWO_TO_DIM && vbtir( s) < vrrmz ; s++ )
{
if ( tevfy( s)<=1 ) 
{
jdgei( s, txqcq( s, 0)) = ONE;
continue;
}
if ( tevfy( s)==2 ) 
{
jdgei( s, txqcq( s, 0)) =
gblzj( txqcq( s, 1), txqcq( s, 1)) -
gblzj( txqcq( s, 1), txqcq( s, 0));
jdgei( s, txqcq( s, 1)) =
gblzj( txqcq( s, 0), txqcq( s, 0)) -
gblzj( txqcq( s, 0), txqcq( s, 1));
continue;
}
for ( j=0 ; j<tevfy( s) ; j++ )
{
wmqcd = txqcq( s, j);
tosgy = hhhcv( s, j);
osveu = 0;
for ( i=0 ; i < tevfy( tosgy) ; i++ )
{
uywln = txqcq( tosgy, i);
osveu += qfdnh(
jdgei( tosgy, uywln ),
gblzj( uywln, txqcq( tosgy, 0)) - gblzj( uywln, wmqcd)
);
}
jdgei( s, wmqcd) = osveu;
}
}
return;
}
int gjk_extract_point( struct simplex_point *pippf, REAL (*pts)[DIM],
int whichpoint, REAL vector[])
{
INDEX * svbya;
if ( whichpoint!=1 && whichpoint !=2 )
{
bad( "Bad indicator argument to gjk_extract_point");
return 0;
}
svbya = ( whichpoint==1 ) ? pippf->simplex1 : pippf->simplex2;
vtsxf( vector, pippf->npts, pts, svbya, pippf->lambdas);
return 1;
}
static void vtsxf( REAL esybg[DIM], int nzsdb, REAL (* xyppg)[DIM],
INDEX pippf[], REAL lambdas[]) 
{
int d, i;
jkfob( d)
{
esybg[d] = 0;
for ( i=0 ; i<nzsdb ; i++ )
esybg[d] += qfdnh( xyppg[ pippf[i]][d], lambdas[i]);
}
return;
}
static int simplex_distance_initialised = 0;
static void cenkk( void)
{
int power, d, s, e, sawbx, hizop, dbmhg, pr;
int nlkmj[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++ )
nlkmj[s] = 0;
for ( s=1 ; s<TWICE_TWO_TO_DIM ; s++ )
{
sawbx = 1;
hizop = dbmhg = 0;
for ( e=0 ; e<DIM_PLUS_ONE ; e++ )
{
if ( (s/sawbx) % 2 == 1 )
{
txqcq( s, hizop) = e;
pr = s - sawbx;
hhhcv( s, hizop) = pr;
alfic( pr, nlkmj[pr]) = s;
nlkmj[ pr]++;
hizop++;
}
else
aryov( s, dbmhg++) = e;
sawbx *= 2;
}
tevfy( s) = hizop;
vbtir( s) = txqcq( s, hizop-1 );
}
tevfy( 0) = 0; vbtir( 0) = -1;
for ( e=0 ; e<DIM_PLUS_ONE ; e++ )
aryov( 0, e) = e;
simplex_distance_initialised = 1;
return;
}