gjkdemo.c

GJK距离计算算法实现

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <time.h>

/* Demo program for the Oxford University Computing Laboratory 
   implementation of Gilbert, Johnson and Keerthi's algorithm for
   computing the minimum distance between two convex polyhedra.

      (c) Oxford University Computing Laboratory 1996, 1997, 1998.

   Version History:
	0.9	pre-release
	1.0     First public release, December 1996.
	1.1     Modified for GJK v2.1, April 1997.
	1.4	For v2.4, July 1998.

   The program usage is
   
      gjkdemo [-h] [-H] [-Rfactor] [-Tfactor] [-qN] [-Q] [-rrepeats]
      	[-iinstances] [ -sseed ] [-x] n_runs [ n_pts ]

   If the -h flag is present, it simply prints a help message.

   If the -H flag is present, it inhibits the use of hill-climbing.

   If the -x flag is present, it inhibits the use of transformation
   matrices.  Useful for comparing performance with the vertex coordinates
   pre-computed, or defined using a transformation matrix.

   If the -R flag is present, it enables rotations, and the factor
   scales the mean rotation velocity (set to 1 if missing).

   If the -T flag is present, the factor scales the mean translational
   velocity (set to 1 if missing).

   If the -q flag is present then the program runs in quiet mode, in
   which the normal per instance output of the program is suppressed.
   If a numeric argument N is also supplied, then a single dot is printed
   out on standard output for every N runs (default 0, for off).

   If the -Q flag is present, and QHULL is defined, then convex hulls of
   random point sets are used instead of the semi-regular hulls used
   otherwise.

   Each run of GJK is repeated several times in order to improve the
   timing accuracy.  The number of repeats used can be changed with
   the -r<num> option.  (Timing accuracy is likely to be low unless
   this number is large -- say >> 100.)

   The instance count is given by the -i<num> option, and defaults
   to 1.  It says how many times to use any one pair of hulls, moving
   them slightly between calls.

   For each example the program lists the seed integer used in the
   random number generator (as an unsigned string of 8 hexadecimal
   digits), in a form suitable to be passed to the program in the
   -s<seed> argument.  If the seed argument is missing a random seed
   is used, derived from the system clock.

   n_runs gives the number of runs of the program, that is, the number
   of different hull pairs given to GJK to solve.

   Each hull is generated by the number of points given (or DEF_PTS
   if not given).
   */

/*
   A Note on Array Sizes:
   
   Euler's formula relates the numver of vertices, edges and faces in
   a convex polyhedron as
   	V - E + F = 2
   We find it more convenient to replace E by H, the number of half-edges,
   with H = 2E.  Every face is bounded by at least three half-edges,
   giving F <= 3H.  Using this inequality produces
        H <= 6V - 12
   and consequently
        F <= 2V - 4.
   In fact these bounds are tight: consider a generalised diamond shape
   with N vertices, with one at (0,0,1), one at (0,0,-1), and the other
   N-2 arranged on the perimeter of the circle x^2 + y^2 = 1, z = 0.
   Convenient approximations to these inequalities, used below, are
   F <= 2V and H <= 6V.  The ring arrays require an entry for each vertex
   showing where each ring starts, an entry for each vertex marking the
   end of each ring, and an entry for each half-edge, and therefore the
   ring arrays are bounded in size by 8V.
   */
   

   
#include "gjk.h"

int
      gjk_num_g_test,     /* how many times the G-test is performed -- the
                             same as the number of main-loop iterations */
      gjk_num_simplices,  /* how many times the simplex routine
                             was called */
      gjk_num_backups,    /* how many times (if ever!) the GJK backup
                             procedure was called */
      gjk_num_dot_products, /* how many dot-product operations are called */
      gjk_num_support_dp, /* how many dot-product operations are called
			      whilst executing the support function */
      gjk_num_other_ops; /* how many other mults and divides are called */



#define ZETA			(1.0e-10) /* allowance for arithmetic error */

#define	MAX_POINTS		1000 /* maximum number of points per hull */
#define	DEF_PTS			20   /* default number of points */
#define TERMINATOR		-1   /* terminator for edge lists */

/* the following four quantities affect the random hulls and boxes
   generated; see the routines create_polyball(), create_hull() and
   shift_hull_random() for precisely how.
   */
#define	MEAN_RADIUS		50
#define	RADIUS_RATIO		0.2
#define	LENGTH_RATIO		0.6
#define	TRANSLATION_WIDTH	500
#define	TRANSLATION_SPEED	1
#define ROTATION_SINE_AZIMUTH   0.1
#define ROTATION_SINE_INCLIN    0.05

/* definition of local type for a rigid-body transformation */
struct transf { double matrix[DIM][DIM+1]; };


/* this vector is the constant vector by which one of the hulls is
   shifted for each distinct instance.
   */
static const REAL delta_vector[3] = 	{ 1, -0.5, -1 }; 

/* External functions, not declared in gjk.h */
#ifdef QHULL
int sac_qhull( int npts, const double (*input_array)[DIM],
	       double (*points)[DIM], int * size_rings, int rings[],
	       int * num_faces, double (*faces)[DIM+1]);
#endif

/* local functions */

#ifdef QHULL
static void create_hull( int npts, REAL (*pts)[DIM], VertexID * rings);
static int num_outside( int npts, double errin, double * errout,
			REAL (*pts)[DIM], struct transf * t,
			REAL witness[DIM]);
#endif

static void create_polyball( int npts, REAL (*pts)[DIM], VertexID * rings);

static void generate_initial_random_shift( REAL pts[DIM]);
static void add_delta_random_shift( REAL pts[DIM]);
static void generate_small_random_rotation( struct transf * t );
/* apply a transformation to a point vector */
static void transform_point( struct transf *t, REAL * src, REAL * tgt);
/* transform each point in the hull. */
static void transform_hull( int npts, REAL (*src)[DIM], REAL (*tgt)[DIM],
			    struct transf * t);
/* compose two transformations */
static void compose_transformations( struct transf * tgt,
			 struct transf * t2, struct transf * t1);
static int num_further( int npts, double errin, double * errout,
			REAL (*pts)[DIM], struct transf * t,
			REAL direction[DIM], REAL witness[DIM]);

static double dot_product( REAL v1[3], REAL v2[3]);
static double clocktime( void);
static double unit_random( unsigned long * seed);
static void give_help_and_exit( void);
static void mkid( struct transf * t);

static double rotation_factor = 1, translation_factor = 1,
  shift_factor = 1, radius_factor = 1;
static char * prog_name;

static unsigned long seed;    /* current value of the seed for
                                 the random number generator */


void apply_trans(  Transform t, REAL * src, REAL * tgt)
{
  int i;

  if ( t==0 )
    for ( i=0 ; i<DIM ; i++ )
      tgt[i] = src[i];
  else {
    for ( i=0 ; i<DIM ; i++ )
      tgt[i] = t[i][DIM] + OTHER_DOT_PRODUCT( t[i], src);
  }
  return;
}


void
apply_rot_transpose( Transform t, REAL * src, REAL * tgt)
{
  int i;

  if ( t==0 )
    for ( i=0 ; i<DIM ; i++ )
      tgt[i] = src[i];
  else {
    for ( i=0 ; i<DIM ; 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;
}

int main( int argc, char ** argv)
{
   /* vertex arrays: both objects, and a centred form of the second object */
   REAL initial_points1[MAX_POINTS][3], initial_points2[MAX_POINTS][3],
     transformed_points1[MAX_POINTS][3], transformed_points2[MAX_POINTS][3],
     (*ppoints1)[3], (*ppoints2)[3];
   /* edge ring arrays for both objects */
   VertexID fixed_rings1[MAX_RING_SIZE_MULTIPLIER * MAX_POINTS];
   VertexID fixed_rings2[MAX_RING_SIZE_MULTIPLIER * MAX_POINTS];
   VertexID * rings1, * rings2;
   REAL (*tr1)[DIM+1] = 0, (*tr2)[DIM+1] = 0;
   unsigned long this_seed, original_seed; /* random number seeds */
   int num_pts, repeat, num_repeats, instance, num_instances;
   long run, num_runs, num_errors, total_g_test, total_simplices,
     total_vertices, total_dp, total_sdp, total_mult_divide_only,
     total_ops, num_zero_runs, tick_frequency;
   double aver_verts, aver_dp, aver_sdp, aver_ops, aver_time;
   double aver_time_g, aver_time_simp, aver_time_dp, aver_time_op;
   REAL dist, sqdist, dp_error_val, total_dist, aver_dist, aver_zeros;
   REAL wit1[3], wit2[3], one_to_two[3], two_to_one[3], hull2_shift[3];
   struct simplex_point simplex_record, initial_simplex_record;
   double start_time, total_time, initial_time, elapsed_time;
   char outbuff[256]; /* line output buffer */
   int i, d, nbad1, nbad2;
   REAL err, err1, err2;
   /* various flags */
   int quiet = 0, rotating = 0, use_polyballs = 1, allow_hill_climbing = 1;
   int pass_transforms = 1;
   /* various transformations */
   struct transf initial_trf1, initial_trf2, trf2,
     * ptrf1, * ptrf2, delta_rot,current_rot;
   struct Object_structure obj1, obj2;

   num_instances = 10;
   num_repeats = 100;
   tick_frequency = 0;
   prog_name = argv[0];

   /* now deal with setting the intial value of the seed. */
   original_seed = (unsigned long) time( NULL);

   if ( DIM != 3 )
     {
       fprintf( stderr,
		"%s assumes three dimensions, but DIM=%d\n",
		argv[0], DIM);
       exit(1);
     }

   /* Parse the optional arguments */
   while ( argc>=2 && argv[1][0]==(char) '-' )
     {
       if ( !strcmp( "-h", argv[1]) )
	 give_help_and_exit();

       else if ( !strcmp( "-H", argv[1]) )
	   allow_hill_climbing = 0;

       else if ( !strcmp( "-x", argv[1]) )
	   pass_transforms = 0;

       else if ( !strncmp( "-q", argv[1], 2) )
	 {
	   quiet = 1;
	   if ( argv[1][2]!=0 )
	     tick_frequency = atol( argv[1]+2);
	 }       

#ifdef QHULL
       else if ( !strcmp( "-Q", argv[1]) )
	   use_polyballs = 0;
#endif

       else if ( !strncmp( "-R", argv[1], 2) )
	 {
	   rotating = 1;
	   if ( argv[1][2]!=0 )
	     rotation_factor = atof( argv[1]+2);
	 }       
       
       else if ( !strncmp( "-T", argv[1], 2) )
	 {
	   if ( argv[1][2]!=(char) '\0' )
	     shift_factor = atof( argv[1]+2);
	 }       

       else if ( !strncmp( "-r", argv[1], 2) )
	 num_repeats = atoi( argv[1]+2);

       else if ( !strncmp( "-i", argv[1], 2) )
	 num_instances = atoi( argv[1]+2);

       else if ( !strncmp( "-s", argv[1], 2) )
	 /* decode the argument as a long, unsigned hexadecimal string */
	 (void) sscanf( argv[1]+2, "%lx", &original_seed);

       else
	 give_help_and_exit();

       argv++;
       argc--;
     }

   if ( argc<2 || argc>3 )  /* wrong number of arguments */
     give_help_and_exit();

   num_runs = atol( argv[1]);  /* grab number of runs */

   /* set the number of points for each hull */
   num_pts = ( argc>2 ) ? atoi( argv[2]) : DEF_PTS;
   if ( num_pts<4 || num_pts>MAX_POINTS ) {
     fprintf( stderr,
	      "Bad number of points (%d requested, minimum 4, maximum %d)\n",
	      num_pts, MAX_POINTS);
     exit( 1);
   }

   printf(  "Enhanced GJK demo v1.3 (c) OUCL 1996, 1997, 1998\n"
	    "\t%ld runs using %d instances, %d repeats\n"
            "\t%d vertices per %s, seed %08lx, hill-climbing %s\n"
#ifndef GATHER_STATISTICS
	    "\toperation counters NOT in use\n"
#endif
	    ,
            num_runs, num_instances, num_repeats, 
	    num_pts, ( use_polyballs ) ? "polyball" : "hull", original_seed,
	              ( allow_hill_climbing ? "enabled" : "disabled" ) );

   if ( tick_frequency>0 )
     printf( "\tshowing dots every %ld runs\n", tick_frequency);

   num_errors = total_g_test = total_simplices =
     total_vertices = total_dp = total_sdp = total_mult_divide_only = 0;
   num_zero_runs = 0;
   total_dist = 0;
   total_time = 0;

   /* check whether we are doing hill-climbing */
   if ( allow_hill_climbing )
     {
       rings1 = fixed_rings1;
       rings2 = fixed_rings2;
     }
   else
     {
       rings1 = rings2 = 0;
     }

   if ( pass_transforms )
     {
       ptrf1 = &initial_trf1;
       ptrf2 = &        trf2;
       tr1   = ptrf1->matrix;