gjk.c

GJK距离计算算法实现

      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,
			  obj1, FirstVertex( obj1), tr1,
			  obj2, FirstVertex( obj2), tr2);
   }
   else {
      /* If we are being told to use this seed point, there
         is a good chance that the near point will be on
         the current simplex.  Besides, if we don't confirm
         that the seed point given satisfies the invariant
         (that the witness points given are the closest points
         on the current simplex) things can and will fall down.
         */
      for ( v=0 ; v<simplex->npts ; v++ )
	add_simplex_vertex( simplex, v,
          obj1, simplex->simplex1[v], tr1,
	  obj2, simplex->simplex2[v], tr2);
   }

   /* Now the main loop.  We first compute the distance between the
      current simplicies, the check whether this gives the globally
      correct answer, and if not construct new simplices and try again.
      */

   max_iterations = NumVertices( obj1)*NumVertices( obj2) ;
      /* in practice we never see more than about 6 iterations. */

   /* Counting the iterations in this way should not be necessary;
      a while( 1) should do just as well. */
   while ( max_iterations-- > 0 ) {

     if ( simplex->npts==1 ) { /* simple case */
       simplex->lambdas[0] = ONE;
     }
     else { /* normal case */
       compute_subterms( simplex);
       if ( use_default ) { 
	 use_default = default_distance( simplex);
       }
       if ( !use_default ) {
	 backup_distance( simplex);
       }
     }

     /* compute at least the displacement vectors given by the
	simplex_point structure.  If we are to provide both witness
	points, it's slightly faster to compute those first.
     */
     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 we are using a c-space simplex with DIM_PLUS_ONE
	points, this is interior to the simplex, and indicates
	that the original hulls overlap, as does the distance 
	between them being too small. */
     if ( sqrd<EPSILON ) {
       simplex->error = EPSILON;
       return sqrd;                 
     }

     if ( ! IdentityTransform( tr1) )
       ApplyInverseRotation( tr1,         displacementv, fdisp);

     if ( ! IdentityTransform( tr2) )
       ApplyInverseRotation( tr2, reverse_displacementv, rdisp);

     /* find the point in obj1 that is maximal in the
	direction displacement, and the point in obj2 that
	is minimal in direction displacement;
     */
     maxp = support_function(
		  obj1,
		  ( use_seed ? simplex->last_best1 : InvalidVertexID),
		  &maxv, fdisp
		  );

     minp = support_function(
		  obj2,
		  ( use_seed ? simplex->last_best2 : InvalidVertexID),
		  &minus_minv, rdisp
		  );

     /* Now apply the G-test on this pair of points */

     INCREMENT_G_TEST_COUNTER;

     g_val = sqrd + maxv + minus_minv;

     if ( ! IdentityTransform( tr1) ) {
       ExtractTranslation( tr1, trv);
       g_val += OTHER_DOT_PRODUCT(         displacementv, trv);
     }

     if ( ! IdentityTransform( tr2) ) {
       ExtractTranslation( tr2, trv);
       g_val += OTHER_DOT_PRODUCT( reverse_displacementv, trv);
     }

     if ( g_val < 0.0 )  /* not sure how, but it happens! */
       g_val = 0;

     if ( g_val < EPSILON ) {
       /* then no better points - finish */
       simplex->error = g_val;
       return sqrd;
     }

     /* check for good calculation above */
     if ( (first_iteration || (sqrd < oldsqrd))
	  && (simplex->npts <= DIM ) ) {
       /* Normal case: add the new c-space points to the current
	  simplex, and call simplex_distance() */
       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,
			   obj1, maxp, tr1,
			   obj2, minp, tr2);
       simplex->npts++;
       oldsqrd = sqrd;
       first_iteration = 0;
       use_default = 1;
       continue;
     }

     /* Abnormal cases! */ 
     if ( use_default ) {
       use_default = 0;
     }
     else { /* give up trying! */
       simplex->error = g_val;
       return sqrd;
     }
   } /* end of `while ( 1 )' */

   return 0.0; /* we never actually get here, but it keeps some
                  fussy compilers happy.  */
}

/*
 * A subsidary routine, that given a simplex record, the point arrays to
 * which it refers, an integer 1 or 2, and a pointer to a vector, sets
 * the coordinates of that vector to the coordinates of either the first
 * or second witness point given by the simplex record.
 */
int gjk_extract_point( struct simplex_point *simp,
		       int whichpoint, REAL vector[]) {

  REAL (* coords)[DIM];

  assert( whichpoint==1 || whichpoint==2 );

  coords = ( whichpoint==1 ) ? simp->coords1 : simp->coords2;
  compute_point( vector, simp->npts, coords, simp->lambdas);

  return 1;
}

static REAL delta[TWICE_TWO_TO_DIM];

/* The simplex_distance routine requires the computation of a number of
   delta terms.  These are computed here.
 */
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];
   
   /* compute the coordinates of the simplex as C-space obstacle points */
   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];
            
   /* compute the dot product terms */
   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]);

   /* now compute all the delta terms */
   for ( s=1 ; s<TWICE_TWO_TO_DIM && max_elt( s) < size ; s++ ) {
      if ( card( s)<=1 ) {  /* just record delta(s, elts(s, 0)) */
         delta( s, elts( s, 0)) = ONE;
         continue;
      }
         
      if ( card( s)==2 ) {  /* the base case for the recursion */
         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;
      }

      /* otherwise, card( s)>2, so use the general case */

      /* for each element of this subset s, namely elts( s, j) */
      for ( j=0 ; j<card( s) ; j++ ) {
         jelt = elts( s, j);
         jsubset = pred( s, j);
         sum = 0;
         /* for each element of subset jsubset */   
         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;
}


/*
 * default_distance is our equivalent of GJK's distance subalgorithm.
 * It is given a c-space simplex as indices of size (up to DIM_PLUS_ONE) points
 * in the master point list, and computes a pair of witness points for the
 * minimum distance vector between the simplices.  This vector is indicated
 * by setting the values lambdas[] in the given array, and returning the
 * number of non-zero values of lambda. 
 */
 
static int default_distance( struct simplex_point * simplex) {

   int s, j, k, ok, size;

   size = simplex->npts;

   INCREMENT_SIMPLICES_COUNTER;

   assert( size>0 && size<=DIM_PLUS_ONE );


   /* for every subset s of the given set of points ...
      */
   for ( s=1 ; s < TWICE_TWO_TO_DIM && max_elt( s) < size ; s++ ) {
      /* delta[s] will accumulate the sum of the delta expressions for
         this subset, and ok will remain TRUE whilst this subset can
         still be thought to be a candidate simplex for the shortest
         distance.
         */
      delta[s] = ZERO;	ok=1;

      /* Now the first check is whether the simplex formed by this
         subset holds the foot of the perpendicular from the origin
         to the point/line/plane passing through the simplex. This will
         be the case if all the delta terms for each predecessor subset
         are (strictly) positive.
         */
      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;
      }

      /* If the subset survives the previous test, we still need to check
         whether the true minimum distance is to a larger piece of geometry,
         or indeed to another piece of geometry of the same dimensionality.
         A necessary and sufficient condition for it to fail at this stage
         is if the delta term for any successor subset is positive, as this
         indicates a direction on the appropriate higher dimensional simplex
         in which the distance gets shorter.
         */ 

      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
      /* define TEST_BACKUP_PROCEDURE in gjk.h to test accuracy
         of the the backup procedure */
      ok = 0;
#endif

      if ( ok && delta[s]>=TINY )   /* then we've found a viable subset */
         break;
   }

   if ( ok ) {
     reset_simplex( s, simplex);
     return 1;
   }
   else
     return 0;
}

/* A version of GJK's `Backup Procedure'.
   Note that it requires that the delta[s] entries have been
   computed for all viable s within simplex_distance.
   */
static void backup_distance( struct simplex_point * simplex) {

   int s, i, j, k, bests;
   int size = simplex->npts;
   REAL distsq_num[TWICE_TWO_TO_DIM], distsq_den[TWICE_TWO_TO_DIM];

   INCREMENT_BACKUP_COUNTER;

   /* for every subset s of the given set of points ...
      */