gjkdemo.c

GJK距离计算算法实现

  double radius;
  int p, q, r;
  int i, v, lower, upper, slice, firstv, nextv, nextr, lastv;

  double sines[MAX_POINTS], cosines[MAX_POINTS];

  radius = MEAN_RADIUS *
         ( 1.0 - RADIUS_RATIO*radius_factor / 2.0 
	   + RADIUS_RATIO*radius_factor * unit_random( &seed) );

  (void) compute_sizes( npts, & p, & q, & r);
  /* debugging statements 
    fprintf( stderr, "%d = %d * %d + %d\n", npts, p, q, r);
    first_time1 = 0;
  /**/

  /* Force generation of blocks if n==8 */
  if ( npts==8 ) { p = 4; q = 2; r = 0; }
  /**/

  /* `normal' vertices are indexed in the range [firstv, lastv] (inclusive) */
  firstv = ( r==2 ) ? 1 : 0;
  lastv = ( r>0 ) ? npts-2 : npts-1;

  /* compute vertex coordinates */

  /* bottom vertex, if it exists */
  if ( r==2 ) {
     pts[0][0] = pts[0][1] = 0;
     pts[0][2] = - radius;
   }

  /* normal vertices */

  /* cache sines and cosines first */
   sines[0] = 0;	cosines[0] = 1;
   s = sin( 2*M_PI / p );   c = cos( 2*M_PI / p );
   for ( i = 1 ; i < p ; i++ ) {
       sines[i] = cosines[i-1]*s + sines[i-1]*c;
     cosines[i] = cosines[i-1]*c - sines[i-1]*s;
   }

   nextv = firstv;
   for ( slice=1 ; slice<=q ; slice++ ) {
     z = radius * ((double) 2*slice - ( q+1) ) / (q+1);
     xyr = sqrt( radius * radius - z * z );
     for ( i = 0 ; i < p ; i++ ) {
       pts[nextv][0] = xyr * cosines[i];
       pts[nextv][1] = xyr *   sines[i];
       pts[nextv][2] = z;
       nextv++;
     }
   }

  /* top vertex, if it exists */
  if ( r>0 ) {
     pts[npts-1][0] = pts[npts-1][1] = 0;
     pts[npts-1][2] = radius;
   }
   
  if ( rings==0 ) /* then no hill-climbing */
     return;

   /* otherwise set up the edge lists.
      We process them in vertex number order.
      */
   nextr = npts;

   if ( r==2 ) { /* then form an entry for the first, bottom vertex */
     rings[0] = nextr;
     for ( i=0 ; i<p ; i++ )
       rings[nextr++] = i+1;
     rings[nextr++] = TERMINATOR;
   }

   /* now the `normal' vertices */
   for ( v=firstv ; v<=lastv ; v++ ) {
     rings[v] = nextr;
     slice = (v+p-firstv)/p;
     rings[v] = nextr;
     lower = v-p;
     if ( lower<firstv )
       lower = ( r==2 ) ? 0 : -1;
     if ( lower>=0 )
       rings[nextr++] = lower;
     rings[nextr++] = ( ((v+p+1-firstv)/p)==slice ) ? v+1 : v+1-p;
     upper = v+p;
     if ( upper>lastv )
       upper = ( r>0 ) ? npts-1 : -1;
     if ( upper>=0 )
       rings[nextr++] = upper;
     rings[nextr++] = ( ((v+p-1-firstv)/p)==slice ) ? v-1 : v-1+p;
     rings[nextr++] = TERMINATOR;
     }

   if ( r>0 ) { /* then form an entry for the last, top vertex */
     rings[npts-1] = nextr;
     for ( i=0 ; i<p ; i++ )
       rings[nextr++] = lastv - i;
     rings[nextr++] = TERMINATOR;
   }


   return;
   }

      
/* Compute an initial random shift for a hull
   */      
static void generate_initial_random_shift( REAL pts[DIM])
{
   double ran;
   int d;
        
   for ( d=0 ; d<3 ; d++ )
   {
      ran = unit_random( &seed) - 0.5;
      pts[d] = TRANSLATION_WIDTH * ran * translation_factor;
      }

   return;
   }
   
/* Add a random translation component to the vector given
   */      
static void add_delta_random_shift( REAL pts[DIM])
{
   double ran, fact;
   int d;
        
   ran = unit_random( &seed);
   fact = 1 + ran * ran;

   for ( d=0 ; d<3 ; d++ )
   {
      pts[d] += TRANSLATION_SPEED * fact *
	shift_factor * delta_vector[d];
      }

   return;
   }
   

static void generate_small_random_rotation( struct transf * t )
{
  int i;
  double sine_azimuth, cos_azimuth, sine_inclin, cos_inclin, rana, rani;

  rana = unit_random( &seed);
  rani = 2 * unit_random( &seed) - 1;
/*
rana = rani = 0;
*/

  sine_azimuth = ROTATION_SINE_AZIMUTH * rana * rotation_factor;
  cos_azimuth = sqrt( 1 - sine_azimuth * sine_azimuth );
  sine_inclin = ROTATION_SINE_INCLIN * rana * rotation_factor;
  cos_inclin = sqrt( 1 - sine_inclin * sine_inclin );

  /* now compute the transformation */
  t->matrix[0][0] =  cos_azimuth;
  t->matrix[0][1] = sine_azimuth;
  t->matrix[0][2] = 0;
  t->matrix[1][0] = - sine_azimuth *  cos_inclin;
  t->matrix[1][1] =    cos_azimuth *  cos_inclin;
  t->matrix[1][2] =                  sine_inclin;
  t->matrix[2][0] =   sine_azimuth * sine_inclin;
  t->matrix[2][1] = -  cos_azimuth * sine_inclin;
  t->matrix[2][2] =                   cos_inclin;
  /* and no translation component */
  for ( i=0 ; i<3 ; i++ ) t->matrix[i][3] = 0;

  return;
}

/* apply a transformation to a point vector */
static void transform_point( struct transf *t, REAL * src, REAL * tgt)
{
  REAL tv[3];
  int i;

  for ( i=0 ; i<3 ; i++ )
    tv[i] =
      t->matrix[i][0]*src[0] + t->matrix[i][1]*src[1] + t->matrix[i][2]*src[2];
  for ( i=0 ; i<3 ; i++ )
    tgt[i] = tv[i] + t->matrix[i][3];

  return;
}

/* transform each point in the hull.
   */
static void transform_hull( int npts, REAL (*src)[DIM], REAL (*tgt)[DIM],
			    struct transf * t)
{
  int i;
  for ( i=0 ; i<npts ; i++ )
    transform_point( t, src[i], tgt[i]);
  return;
}

/* compose two transformations */
static void compose_transformations( struct transf * tgt,
			 struct transf * t2, struct transf * t1)
{
  REAL rot[3][3], trl[3];
  int i, j;

  for ( i=0 ; i<3 ; i++ )
    {
      for ( j=0 ; j<3 ; j++ )
	rot[i][j] = t2->matrix[i][0]*t1->matrix[0][j] +
    	            t2->matrix[i][1]*t1->matrix[1][j] +
	            t2->matrix[i][2]*t1->matrix[2][j];
      trl[i] = t2->matrix[i][0]*t1->matrix[0][3] +
	       t2->matrix[i][1]*t1->matrix[1][3] +
               t2->matrix[i][2]*t1->matrix[2][3] +
	       t2->matrix[i][3];
    }


  for ( i=0 ; i<3 ; i++ )
    {
      for ( j=0 ; j<3 ; j++ )
	tgt->matrix[i][j] = rot[i][j];
      tgt->matrix[i][3] = trl[i];
    }

  return;
}
      
  
/* Compute how many points lie further in the direction given than the
   witness point.
   */
static int num_further( int npts, double errin, double * errout,
			REAL (*pts)[DIM], struct transf * t,
			REAL direction[DIM], REAL witness[DIM])
{
   double lpt[3], dirsize, *ptr, test_val, val, worst;
   int p, nbad;

   dirsize = sqrt( dot_product( direction, direction));
   test_val = dot_product( witness, direction) + dirsize * errin + ZETA;

   nbad = 0;
   for ( p=0 ; p<npts ; p++ )
     {
       if ( t==0 )
	 ptr = pts[p];
       else
	 {
	   transform_point( t, pts[p], lpt);
	   ptr = lpt;
	 }
       if ( dot_product( ptr, direction) > test_val ) {
	 val = dot_product( ptr, direction) - test_val - ZETA;
	 if ( ( nbad==0 ) || ( val > worst ) )
	   worst = test_val;
         nbad++;
       }
     }
   *errout = ( nbad>0 ) ? worst : 0.0;

   return nbad;
   }
   
#ifdef QHULL
/* Compute how many planes of the hull of the given set of points
   the given witness point lies outside of.
   */
static int num_outside( int npts, double errin, double * errout,
	      REAL (*pts)[DIM], struct transf * t, REAL witness[DIM])
{
   double val, worst;
   int f, nf, nv, nbad;
   double trans_pts[MAX_POINTS][3];
   double faces[2*MAX_POINTS][4];

   if ( t!=0 )
     transform_hull( npts, pts, trans_pts, t);

   /* construct the hull */
   
   nv = sac_qhull( npts, ( t==0 ) ? pts : trans_pts,
		   (double (*)[3]) 0, (int *) 0, (int *) 0,
		   &nf, faces);

   nbad = 0;
   for ( f=0 ; f<nf ; f++ )
     {
       val = dot_product( witness, faces[f]) + faces[f][3];
       if ( val > errin + ZETA ) {
	 if ( ( nbad==0 ) || ( val > worst ) )
	   worst = val;
	 nbad++;
       }
     }
   *errout = ( nbad>0 ) ? worst : 0.0;

   return nbad;
   }
#endif /* QHULL */
   
/* Vector dot-product function.  We don't use the one defined in gjk.h
   in order to provide some measure of independence.
   */
static double dot_product( REAL v1[3], REAL v2[3])
{
   int i;
   double val = 0;

   for ( i=0 ; i<3 ; i++ )
      val += v1[i]*v2[i];

   return val;
   }

/* Set to an identity transformation */
static void mkid( struct transf * t)
{
  int i, j;
  for ( i=0 ; i<3 ; i++ )
    for ( j=0 ; j<4 ; j++ )
      t->matrix[i][j] = ( i==j ) ? 1 : 0;
  return;
}

/* clocktime should return a double, indicating time passing as a number
   of seconds.
   */
            
#ifndef CLOCKS_PER_SEC /* then why not -- supposed to be ANSI C! */
#ifdef CLK_TCK
#define CLOCKS_PER_SEC CLK_TCK
#else
#define CLOCKS_PER_SEC 1000000
#endif
#endif

static double clocktime( void)
{
   double num, den;
   num = (double) clock();
   den = (double) CLOCKS_PER_SEC;
   return num / den;
   }


/* random number generator. Generate a double between zero and one.
   */
                              
#include <limits.h> /* RAND_MAX should be defined here */
#ifndef RAND_MAX
#define RAND_MAX   UINT_MAX
#endif /* !defined( RAND_MAX) */

static double unit_random( unsigned long * seed)
   /* return a random number in range 0-1 */
{
   srand( (unsigned) * seed);
   * seed = (unsigned) rand();
   return (double)  ( * seed) / RAND_MAX;
   }