gjk.c

GJK距离计算算法实现

   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;

      /* otherwise we've found a viable subset */
      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;
      }

   reset_simplex( bests, simplex);

   return;
}

static void reset_simplex( int subset, struct simplex_point * simplex) {

  int i, j, oldpos;

   /* compute the lambda values that indicate exactly where the
      witness points lie.  We also fold back the values stored for the
      indices into the original point arrays, and the transformed
      coordinates, so that these are ready for subsequent calls.
    */
   for ( j=0 ; j<card( subset) ; j++ ) {
     /* rely on elts( subset, j)>=j, which is true as they are
	stored in ascending order.   */
      oldpos = elts( subset, 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( subset, elts( subset, j)), delta[subset]);
   }
   simplex->npts = card( subset);

   return;
}

/*
 * The implementation of the support function.  Given a direction and
 * a hull, this function returns a vertex of the hull that is maximal
 * in that direction, and the value (i.e., dot-product of the maximal
 * vertex and the direction) associated.
 *
 * If there is no topological information given for the hull
 * then an exhaustive search of the vertices is used.  Otherwise,
 * hill-climbing is performed.  If EAGER_HILL_CLIMBING is defined
 * then the hill-climbing moves to a new vertex as soon as a better
 * vertex is found, and if it is not defined then every vertex leading
 * from the current vertex is explored before moving to the best one.
 * Initial conclusions are that fewer vertices are explored with
 * EAGER_HILL_CLIMBING defined, but that the code runs slighty slower!
 * This is presumably due to pipeline bubbles and/or cache misses.
 *
 */

static VertexID
support_function(
		 Object obj,
		 VertexID start, REAL * supportval, REAL * direction) {

  if ( !ValidRings( obj) ) {
    /* then no information for hill-climbing.  Use brute-force instead. */
    return support_simple( obj, start, supportval, direction);
  }
  else {
    return support_hill_climbing( obj, start, supportval, direction);
  }
}

static VertexID
support_simple(
		 Object obj,
		 VertexID start, REAL * supportval, REAL * direction) {

  VertexID p, maxp;
  REAL maxv, thisv;

  /* then no information for hill-climbing.  Use brute-force instead. */

  p = maxp = FirstVertex( obj);
  maxv = SupportValue( obj, maxp, direction);
  for ( IncrementVertex( obj, p) ;
	!LastVertex( obj, p) ;
	IncrementVertex( obj, p) ) {
    thisv = SupportValue( obj, p, direction);
    if ( thisv>maxv ) {
      maxv = thisv;
      maxp = p;
    }
  }

  *supportval = maxv;
  return maxp;
}

static VertexID
support_hill_climbing(
		 Object obj,
		 VertexID start, REAL * supportval, REAL * direction) {

  VertexID p, maxp, lastvisited, neighbour;
  EdgeID index;
  REAL maxv, thisv;

  /* Use hill-climbing */
  p = lastvisited = InvalidVertexID;
  maxp = ( !ValidVertex( obj, start) ) ? FirstVertex( obj) : start;
  maxv = SupportValue( obj, maxp, direction);

  while ( p != maxp ) {

    p = maxp;
    /* Check each neighbour of the current point. */
    for ( index = FirstEdge( obj, p) ;
	  ValidEdge( obj, index) ;
	  IncrementEdge( obj, index) ) {

      neighbour = VertexOfEdge( obj, index);
      
      /* Check that we haven't already visited this one in the
	 last outer iteration.  This is to avoid us calculating
	 the dot-product with vertices we've already looked at.
      */
      if ( neighbour==lastvisited )
	continue;
      thisv = SupportValue( obj, neighbour, direction);
      if ( thisv>maxv ) {
	maxv = thisv;
	maxp = neighbour;
#ifdef EAGER_HILL_CLIMBING
	/* The next line gives Gilbert & Ong's eager behaviour. */
	break;
#endif
      }
    }
      lastvisited = p;
  }

  *supportval = maxv;

  return p;
}


/* Computes the coordinates of a simplex point.
   Takes an array into which the stuff the result, the number of vertices
   that make up a simplex point, one of the point arrays, the indices of
   which of the points are used in the for the simplex points, and an
   array of the lambda values.
   */
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,
  Object obj1, VertexID v1, Transform t1, Object obj2, VertexID v2,
				Transform t2) {

  ApplyTransform( t1, obj1, v1, s->coords1[pos]);

  ApplyTransform( t2, obj2, v2, s->coords2[pos]);

  return;
}

#ifdef DUMP_CONST_TABLES
#define dump1d( array) \
  printf( "static const int " #array "[%d] = {\n\t%2d", \
	  TWICE_TWO_TO_DIM, array[0]); \
  for ( s=1 ; s<TWICE_TWO_TO_DIM ; s++ ) printf( ", %2d", array[s]); \
  printf( "};\n")

#define dump2d( array) \
  printf( "static const int " #array "[%d][%d] = {\n", \
	  TWICE_TWO_TO_DIM, DIM_PLUS_ONE); \
  for ( s=0 ; s<TWICE_TWO_TO_DIM ; s++ ) { printf( "\t{ %2d", array[s][0]); \
  for ( d=1 ; d<DIM_PLUS_ONE ; d++ ) printf( ", %2d", array[s][d]); \
  printf( (s<TWICE_TWO_TO_DIM-1) ? "},\n" : "} };\n"); }

#endif /* DUMP_CONST_TABLES */

#ifdef CONSTRUCT_TABLES
/* initialise_simplex_distance is called just once per activation of this
   code, to set up some internal tables.  It takes around 5000 integer
   instructions for DIM==3.
   */
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;

   /* check that TWO_TO_DIM is two to the power of DIM */
   power = 1;
   for ( d=0 ; d<DIM ; d++ )
      power *= 2;

   assert( power == TWO_TO_DIM );

   /* initialise the number of successors array */
   for ( s=0 ; s<TWICE_TWO_TO_DIM ; s++ )
        num_succ[s] = 0;

   /* Now the main bit of work.  Simply stated, we wish to encode
      within the matrices listed below information about each possible
      subset of DIM_PLUS_ONE integers e in the range
      0 <= e < DIM_PLUS_ONE.  There are TWICE_TWO_TO_DIM such subsets,
      including the trivial empty subset.  We wish to ensure that the
      subsets are ordered such that all the proper subsets of subset
      indexed s themselves have indexes less than s.  The easiest way
      to do this is to take the natural mapping from integers to
      subsets, namely integer s corresponds to the subset that contains
      element e if and only if there is a one in the e'th position in
      the binary expansion of s.
      
      The arrays set are as follows:
      *  card( s) tells how many elements are in the subset s.
      *  max_elt( s) gives the maximum index of all the elements in
         subset s.
      *  elts( s, i) for 0 <= i < card( s) lists the indices of the
         elements in subset s.
      *  non_elts( s, i) for 0 <= i < DIM_PLUS_ONE-card( s) lists the
         indices of the elements that are not in subset s.
      *  pred( s, i) for 0 <= i < card( s) lists the indices of the
         subsets that are subsets of subset s, but with one fewer
         element, namely the element with index elts( s, i).
      *  succ( s, i) for 0 <= i < DIM_PLUS_ONE-card( s) lists the
         indices of the supersets of subset s that have one extra
         element, namely the element with index non_elts( s, i).

      The elements indexed in each elts( s,) and non_elts( s,) are
      listed in order of increasing index. 
     */  

   /* now for every non-empty subset s (indexed for
      0 < s < TWICE_TWO_TO_DIM ) set the elements of card( s), 
      max_elt( s), elts( s,), pred( s,), and succ( s,).
      */

   for ( s=1 ; s<TWICE_TWO_TO_DIM ; s++ ) {
      /* Now consider every possible element.  Element e is
         in subset s if and only if s DIV 2^e is odd. */
      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 ) {
            /* so e belongs to subset s */
            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 );
   }

   /* for completeness, add the entries for s=0 as well */
   card( 0) = 0;   max_elt( 0) = -1;
   for ( e=0 ; e<DIM_PLUS_ONE ; e++ )
      non_elts( 0, e) = e;
   

   simplex_distance_initialised = 1;

   /* defining USE_CONST_TABLES to be the dimension of space
      causes this routine to be called and to dump the contents
      of the constant integer arrays to standard output.
   */
#ifdef DUMP_CONST_TABLES
   /* Code to dump the arrays to standard output */
     printf( "/* Include following code in gjk.c */\n\n");
     printf( "#define PRE_DEFINED_TABLE_DIM %d\n", DIM);
     dump1d( cardinality);
     dump1d( max_element);
     dump2d( elements);
     dump2d( non_elements);
     dump2d( predecessor);
     dump2d( successor);
     printf( "\n/* End of code to include -"
	     "Exiting gjk program immediately */\n");
     exit( 0);
#endif /* DUMP_CONST_TABLES */

   return;
}
#endif /* CONSTRUCT_TABLES */