#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.

   Version History:
	0.9	pre-release
	1.0     First public release, December 1996.
	1.1     Modified for GJK v2.1, April 1997.
	1.15	Minor modification to allow triangles, June 1997.

   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"

#ifndef GATHER_STATISTICS
/* then gjk.c will not declare these counters, so we do. */
static 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 */

#endif


#define ERROR_EPSILON		EPSILON /* EPSILON is defined in gjk.h */

#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], INDEX * rings);
static int num_outside( int npts, double * err,
			REAL (*pts)[DIM], struct transf * t,
			REAL witness[DIM]);
#endif

static void create_polyball( int npts, REAL (*pts)[DIM], INDEX * 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 * err,
			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 */


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 */
   INDEX fixed_rings1[MAX_RING_SIZE_MULTIPLIER * MAX_POINTS];
   INDEX fixed_rings2[MAX_RING_SIZE_MULTIPLIER * MAX_POINTS];
   INDEX * 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, errorval, 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, base_time, total_time, initial_time, elapsed_time;
   char outbuff[256]; /* line output buffer */
   int i, j, d, nbad1, nbad2;
   REAL 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;
   REAL delta_translation[DIM];


   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<3 || num_pts>MAX_POINTS )
   {
      fprintf( stderr,
	       "Bad number of points (%d requested, minimum 3, maximum %d)\n",
	       num_pts, MAX_POINTS);
      exit( 1);
      }

   if ( num_pts<4 && allow_hill_climbing ) /* disable hill-climbing */
     {
       printf( "\tHill-climbing not supported for less than 4 points!\n\n");
       allow_hill_climbing = 0;
     }

   printf(  "Enhanced GJK demo v1.15 (c) OUCL 1996, 1997\n"
	    "\t%d 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;
       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);


	   /* 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);
	 }

       /* 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;

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

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

	    /* the call to GJK itself */
	    sqdist = gjk_distance( num_pts, ppoints1, rings1, tr1,
				   num_pts, ppoints2, rings2, 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);

      total_dist += dist;
      if ( sqdist==0.0 )
	num_zero_runs++;

      sprintf( outbuff+strlen( outbuff), " %6.2lf", 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];
      }

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

      if ( sqdist>ZERO )
	{
	  /* 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, &err1, ppoints1, ptrf1,
			       one_to_two, wit1);
	  nbad2 = num_further( num_pts, &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, &err1, ppoints1, ptrf1, wit1);
	  nbad2 = num_outside( num_pts, &err2, ppoints2, ptrf2, wit2);
#else
	  nbad1 = nbad2 = 0; /* can't test, assume OK */
#endif /* QHULL */
	}

      if ( errorval < -ERROR_EPSILON || errorval > ERROR_EPSILON ||
	   ( nbad1>0 || nbad2>0 ) )
      {
         num_errors++;
	 printf( "%s", outbuff);
	 printf( " ERROR=%lf (%lf,%lf), sqdist=%.2lf, nbad=%d,%d\n",
		 errorval, err1, err2, sqdist, nbad1, nbad2);
      }
      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%.2lfus average time timed over %ld repeats, %d errors\n"
	      "\taverage of %.1lf ops, average time per op = %.3lfus\n"
	      "\t%.1lf%% zeros, average dist = %.2lf, %.1lfs 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], INDEX * 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;
}

static int first_time1 = 1, first_time2 = 1;

/* Create a random ball-shaped object
   */
static void create_polyball( int npts, REAL (*pts)[DIM], INDEX * rings)
{
  double s, c, z, xyr;
  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
  if ( first_time1 ) {
    fprintf( stderr, "%d = %d * %d + %d\n", npts, p, q, r);
    first_time1 = 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 * err,
			REAL (*pts)[DIM], struct transf * t,
			REAL direction[DIM], REAL witness[DIM])
{
   double lpt[3], *ptr, test_val, val, worst;
   int p, nbad;

   test_val = dot_product( witness, direction) + ERROR_EPSILON;

   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 + ERROR_EPSILON;
	 if ( ( nbad==0 ) || ( val > worst ) )
	   worst = test_val;
         nbad++;
       }
     }
   if ( err!=0 )
     *err = ( 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 * err,
	      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 > ERROR_EPSILON ) {
	 if ( ( nbad==0 ) || ( val > worst ) )
	   worst = val;
	 nbad++;
       }
     }
   if ( err!=0 )
     *err = ( 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.
   */
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;
   }