#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<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 ; vnpts ; 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_testnpts, 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 ; pnpts ; 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 ( sqrdnpts==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 ; pmaxv ) { 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 && jZERO ) 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 ; jsimplex1[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 ; inpts; REAL sum, c_space_points[DIM_PLUS_ONE][DIM]; for ( i=0 ; icoords1[i][j] - simp->coords2[i][j]; for ( i=0 ; icoords1 : 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 ; icoords1[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