gjkdemo.c

GJK距离计算算法实现

       tr2   = ptrf2->matrix;
       ppoints1 = initial_points1;
       ppoints2 = initial_points2;
     }
   else
     {
       tr1 = tr2 = 0;
       ptrf1 = ptrf2 = 0;
       ppoints1 = transformed_points1;
       ppoints2 = transformed_points2;
     }

   initial_time = clocktime();

   /* Now the main loop to run GJK itself */
   
   repeat = 0;  instance = 0;  run = 0;   seed = original_seed;

   while ( run<num_runs )
     {
       if ( tick_frequency>0 && run%tick_frequency==0 )
	 {
	   printf( ".");
	   fflush( stdout);
	 }

       if ( instance == 0 )  /* then create new shapes */
         {
	   this_seed = seed;
#ifdef QHULL
	   if ( !use_polyballs )
	     {
	       create_hull( num_pts, initial_points1, rings1);
	       create_hull( num_pts, initial_points2, rings2);
	     }
	   else
#endif
	     {
	       create_polyball( num_pts, initial_points1, rings1);
	       create_polyball( num_pts, initial_points2, rings2);
	     }

	   /* define initial rotations and translations*/
	   /*
	   generate_small_random_rotation( &initial_trf1);
	   generate_small_random_rotation( &initial_trf2);
	   mkid( &current_rot);
	   generate_initial_random_shift( hull2_shift);
	   */
	   mkid( &initial_trf1);
	   mkid( &initial_trf2);
	   mkid( &current_rot);
	   generate_initial_random_shift( hull2_shift);
	   hull2_shift[0] = hull2_shift[1] = 0;


	   /* If we are not going to pass transformations, we
	      can transform all the points in the first hull now.
	      */
	   if ( !pass_transforms )
	       transform_hull( num_pts, initial_points1, ppoints1,
			       &initial_trf1);
	 }
       else  /* shift the second hull, and rotate if necessary */
	 {
	   if ( rotating )
	     {
	       generate_small_random_rotation( &delta_rot);
	       compose_transformations( &current_rot,
					&delta_rot, &current_rot);
	     }
	   add_delta_random_shift( hull2_shift);
	   hull2_shift[0] = hull2_shift[1] = 0;
	 }

       /* Now construct appropriate transformation for second hull */
       compose_transformations( &trf2, &current_rot, &initial_trf2);
       for ( i=0 ; i<3 ; i++ )
	   trf2.matrix[i][3] += hull2_shift[i];
	 
       /* transform the point in the second hull
	  if we are not passing transformations */
       if ( !pass_transforms )
	 transform_hull( num_pts, initial_points2, ppoints2, &trf2);

       /* Only print out a report if quiet is not set, or if an error.
	  But we don't know whether there's an error yet, so dump the
	  beginning of the message into a line buffer for now */
       sprintf( outbuff, "%6ld: %08lx+%02d", run, this_seed, instance);
       
       /* save the current value of the simplex record */
       initial_simplex_record = simplex_record;

       obj1.numpoints = num_pts;
       obj1.vertices = ppoints1;
       obj1.rings = rings1;

       obj2.numpoints = num_pts;
       obj2.vertices = ppoints2;
       obj2.rings = rings2;

       /* now time num_repeats calls to GJK */
       start_time = clocktime();

       for ( repeat = 0 ; repeat < num_repeats ; repeat++ )
	{
	    simplex_record = initial_simplex_record;

#ifdef GATHER_STATISTICS
	    gjk_num_simplices = ( instance>0 ) ? 1 : 0;
	    gjk_num_g_test = gjk_num_backups = gjk_num_dot_products
	      = gjk_num_support_dp = gjk_num_other_ops = 0;
#endif /* GATHER_STATISTCS */

	    /* the call to GJK itself */
	    sqdist = gjk_distance( &obj1, tr1, &obj2, tr2,
				   wit1, wit2, &simplex_record,
				   (instance>0));
	}

       total_time += clocktime() - start_time;

      /* update running totals */
      total_g_test += gjk_num_g_test;
      total_simplices += gjk_num_simplices;
      total_dp += gjk_num_dot_products;
      total_sdp += gjk_num_support_dp;
      total_mult_divide_only += gjk_num_other_ops;
      total_vertices += 2*num_pts;

      dist = sqrt( sqdist);

      /* Compute the expected width of the error bound */
      err = ( dist>0 ) ? ( simplex_record.error / dist ) : 0.0 ;

      total_dist += dist;
      if ( sqdist<=err )
	num_zero_runs++;

      sprintf( outbuff+strlen( outbuff), " %6.2f", dist);

      /* Now to test the answers.  Compute the displacement vectors
         Don't assume that the witness points were returned. */
      gjk_extract_point( &simplex_record, 1, wit1);
      gjk_extract_point( &simplex_record, 2, wit2);
      for ( d=0 ; d<3 ; d++ )
      {
	two_to_one[d] = wit1[d] - wit2[d];
	one_to_two[d] = - two_to_one[d];
      }

      /* dp_error_val should be zero, to the accuracy of our arithmetic */  
      dp_error_val = sqdist - dot_product( two_to_one, two_to_one);

      if ( sqdist>err*err ) {
	  /* now check, for each hull, how many of its points lie to the
	     wrong side of a plane that lies through the witness point
	     and perpendicular to the displacement vector
	     */
	  nbad1 = num_further( num_pts, err, &err1, ppoints1, ptrf1,
			       one_to_two, wit1);
	  nbad2 = num_further( num_pts, err, &err2, ppoints2, ptrf2,
			       two_to_one, wit2);

	}
      else
	{
	  /* zero was returned, so check that the witness point
	     lies within both hulls. Requires QHULL
	     */
#ifdef QHULL	  
	  nbad1 = num_outside( num_pts, err, &err1, ppoints1, ptrf1, wit1);
	  nbad2 = num_outside( num_pts, err, &err2, ppoints2, ptrf2, wit2);
#else
	  nbad1 = nbad2 = 0; /* can't test, assume OK */
#endif /* QHULL */
	}

      if ( dp_error_val < -ZETA || dp_error_val > ZETA ||
	   ( nbad1>0 || nbad2>0 ) ) {
         num_errors++;
	 printf( "%s", outbuff);
	 printf( " ERROR=%f (%f,%f), sqdist=%.2f, nbad=%d,%d\n",
		 dp_error_val, err1, err2, sqdist, nbad1, nbad2);
      }

      else if ( simplex_record.error > EPSILON ) {
         num_errors++;
	 printf( "%s", outbuff);
	 printf( " Error = %.4g > EPSILON\n", simplex_record.error);
      }

      else if ( !quiet )
	 printf( "%s\n", outbuff);

      instance = ( instance+1 ) % num_instances;
      run++;
     }
      /* end of main loop that calls GJK */

   elapsed_time = clocktime() - initial_time;

   /* scale total_time down to that for just one repeat */
   total_time /= num_repeats;
   
   total_ops = 3 * total_dp + total_mult_divide_only;
   aver_verts = ((double) total_vertices)/num_runs/2;
   aver_dp  = ((double)  total_dp)/num_runs;
   aver_sdp = ((double) total_sdp)/num_runs;
   aver_ops = ((double) total_ops)/num_runs;
   aver_zeros = ((double) num_zero_runs)/num_runs;
   aver_dist = total_dist/num_runs;
	
   aver_time = total_time/num_runs * 1000000.0;
#ifdef GATHER_STATISTICS
   aver_time_g = total_time/total_g_test * 1000000.0;
   aver_time_simp = total_time/total_simplices * 1000000.0;
   aver_time_dp = total_time/total_dp * 1000000.0;
   aver_time_op = total_time/total_ops * 1000000.0;
#else
   aver_time_g = aver_time_simp = aver_time_dp = aver_time_op = 0;
#endif

   printf( "\n\tCompleted %ld runs using %d instances and %d vertex %s\n"
	      "\t%.2fus average time timed over %d repeats, %ld errors\n"
	      "\taverage of %.1f ops, average time per op = %.3fus\n"
	      "\t%.1f%% zeros, average dist = %.2f, %.1fs cpu total\n"
	      ,
	      num_runs, num_instances, num_pts,
        	   ( use_polyballs ? "polyballs" : "hulls" ),
	      aver_time, num_repeats, num_errors,
	      aver_ops, aver_time_op,
	      aver_zeros*100, aver_dist, elapsed_time);
   
   exit( 0);
   }

static void give_help_and_exit( void)
{
  fprintf( stderr,
	   "Usage: %s [optional arguments] number_of_runs [num_points]\n"
	   "  where the optional arguments are\n"
	   "\t-h		gives this message\n"
	   "\t-H		disables hill-climbing\n"
	   "\t-x		don't use transformation matrices\n"
	   "\t-q[N]		quiet mode [show dot every N runs]\n"
	   "\t-Q		use random hulls (if Qhull linked)\n"
	   "\t-R[value]	use relative rotations\n"
	   "\t-T[value]	use relative translations\n"
	   "\t-r<num>		change repeat count\n"
	   "\t-i<num>		define number of instances\n"
	   "\t-s<hex>		define initial seed\n"
	   , prog_name);

  exit( 1);
}

#ifdef QHULL

/* Create a random ball-shaped object
   */
static void create_hull( int npts, REAL (*pts)[DIM], VertexID * rings)
{
   int i, num_vertices, num_rings;
   double azimuth, sine_inclin, cosine_inclin, radius;
   double (* input_points)[DIM];
   double input_array[MAX_POINTS][3];

   if ( rings==0 ) /* then no hill-climbing */
     input_points = pts;
   else
     input_points = input_array;

   /* Compute each of the required points independently.  Each point is
      classified by a radius, and azimuth, and an inclination.  The
      azimuth is like an angle of longitude on the Earth's surface, and
      is choosen from a uniform random variable ranging from 0 to 2pi.
      However as circles of latitude get shorter as we approach the poles,
      we wish to bias the angles of inclination (which measures angle out
      of the equatorial plane) towards the lower absolute values.  Using
      an inverse sinusoidal function provides the correct bias.
      */
   for ( i=0 ; i<npts ; i++ )
   {
      azimuth = 2 * M_PI * unit_random( &seed);
      /* we don't need the inclination as an angle, so derive the sine
         and cosine of it directly */
      sine_inclin = 2 * unit_random( &seed) -1;
      cosine_inclin = sqrt( 1 - sine_inclin * sine_inclin );
      radius = MEAN_RADIUS;

      input_points[i][0] = radius * cosine_inclin * cos( azimuth );
      input_points[i][1] = radius * cosine_inclin * sin( azimuth );
      input_points[i][2] = radius * sine_inclin;
      }

   if ( rings==0 ) /* then no hill-climbing */
     return;

   /* Otherwise, call the convex-hull routine */
   num_vertices = sac_qhull( npts, input_points, pts, &num_rings,
			     rings, (int *) 0, (double (*)[4]) 0);

   return;
   }
      
#endif /* QHULL */


/* compute_sizes is given the number of vertices required in the
   polyball (n), and tries to find the unique values of P, Q and R (to
   which *p, *q and *r are set) so that
      n = PQ + R, R = 0, 1 or 2, P<=Q, and Q-P is minimised.
   The return value is the value of Q-R.
   */

static int compute_sizes( int n, int * p, int * q, int * r)
{
  int i, j, k, m, lp, lq, lr, thisr, thisn;

  /* The `best' solution is bounded by the trivial solution:
     P=1, Q=n-2, R=2 and Q-P=n-3.
     */
  lp = 1;	lq = n-2;	lr = 2;	m = n-3;

  /* set thisr to 0, 1 and 2 */
  for ( thisr=2 ; thisr>=0 ; thisr-- ) {
    thisn = n - thisr;
    /* Now factorise thisn, minimising the difference between the factors.
       The factors will be i and j, and the best difference so far
       will be m */

    /* set i to floor( sqrt(thisn) ). Not efficient, but we don't
       expect to see thisn > 1000, i.e., i should be small */
    for ( i=1 ; i*i<= thisn ; i++ )
      ;
    i--;

    /* initialise k to floor( thisn/i ), and establish ik<=thisn */
    k = thisn / i;

    /* keep trying for this value of i whilst k-i<m,
       and so might just give a better answer */
    while ( k-i < m ) {
      j = k;
      /* hunt for j such that ij=thisn */
      while ( j*i < thisn )
	j++;
      if ( ( j*i==thisn ) && ( j-i<m ) ) {
	/* better solution, so save it */
	m = j-i;	lp = i; lq = j; lr = thisr;
      }
      i = i-1; /* try next value of i */
      k = thisn / i; /* which gives a new value of k */
    }
  }

  *p = lp; *q = lq; *r = lr;
  return m;
}

/* Create a random ball-shaped object
   */
static void create_polyball( int npts, REAL (*pts)[DIM], VertexID * rings)
{
  double s, c, z, xyr;