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

   Version History:
	0.9	pre-release
	1.0     First public release, December 1996.

   The program usage is
   
      gjkdemo [-h] [-H] [-Rfactor] [-Tfactor] [-q] [-Q] [-rrepeats]
      	[-iinstances] [ -sseed ] 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 -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 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"

#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

/* 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,
	const REAL (*pts)[DIM], 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 add_delta_random_rotation( int npts, REAL (*pts)[DIM]);
static void shift_and_copy_hull( int npts, const REAL (*src)[DIM],
				 REAL (*pts)[DIM], const REAL delta[DIM]);
static int num_further( int npts, double * err,
	REAL (*pts)[DIM], 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 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 points1[MAX_POINTS][3], points2[MAX_POINTS][3],
     centred_points2[MAX_POINTS][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;

   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;
   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 d, nbad1, nbad2;
   REAL err1, err2;
   /* various flags */
   int quiet = 0, rotating = 0, use_polyballs = 1, allow_hill_climbing = 1;
   
   num_instances = 10;
   num_repeats = 100;
   prog_name = argv[0];

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

   /* 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( "-q", argv[1]) )
	   quiet = 1;

#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.0 (c) OUCL 1996\n"
	    "\t%d runs using %d instances, %d repeats\n"
            "\t%d vertices per %s, seed %08lx, hill-climbing %s\n",
            num_runs, num_instances, num_repeats, 
	    num_pts, ( use_polyballs ) ? "polyball" : "hull", original_seed,
	              ( allow_hill_climbing ? "enabled" : "disabled" ) );

   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;
     }

   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 ( instance == 0 )  /* then create new shapes */
         {
	   this_seed = seed;
#ifdef QHULL
	   if ( !use_polyballs )
	     {
	       create_hull( num_pts, points1,         rings1);
	       create_hull( num_pts, centred_points2, rings2);
	     }
	   else
#endif
	     {
	       create_polyball( num_pts, points1,         rings1);
	       create_polyball( num_pts, centred_points2, rings2);
	     }
	   generate_initial_random_shift( hull2_shift);

	 }
       else  /* shift the second hull, and rotate if necessary */
	 {
	   add_delta_random_shift( hull2_shift);
	   if ( rotating )
	     add_delta_random_rotation( num_pts, centred_points2);
	 }
	 
       shift_and_copy_hull( num_pts, centred_points2, points2, hull2_shift);

       /* Only print ot a report if quiet is not set, or if an error.
	  But we don't know whether there's an error yet, so dump 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, points1, rings1,
				   num_pts, points2, rings2,
                        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, points1, 1, wit1);
      gjk_extract_point( &simplex_record, points2, 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, points1, one_to_two, wit1);
	  nbad2 = num_further( num_pts, &err2, points2, 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, points1, wit1);
	  nbad2 = num_outside( num_pts, &err2, points2, 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;
   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;
   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-q		quiet mode\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;
   }
   
/* Add a random rotation to each point in the given hull
   */      
static void add_delta_random_rotation( int npts, REAL (*pts)[DIM])
{
   int p;
   double temp_pt[DIM];
   double sine_azimuth, cos_azimuth, sine_inclin, cos_inclin, rana, rani;

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

   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 new coordinates */
   for ( p=0 ; p<npts ; p++ )
     {
       temp_pt[0] = pts[p][0] * cos_azimuth + pts[p][1] * sine_azimuth;
       temp_pt[1] = pts[p][1] * cos_azimuth - pts[p][0] * sine_azimuth;
       temp_pt[2] = pts[p][2];

       pts[p][0] = temp_pt[0];
       pts[p][1] = temp_pt[1] * cos_inclin + temp_pt[2] * sine_inclin;
       pts[p][2] = temp_pt[2] * cos_inclin - temp_pt[1] * sine_inclin;
     }

   return;
   }

/* add a given vector to each point in the hull.
   */
static void shift_and_copy_hull( int npts, const REAL (*src)[DIM],
                                 REAL (*pts)[DIM], const REAL delta[DIM])
{
   int d, p;

   for ( p=0 ; p<npts ; p++ )
      for ( d=0 ; d<3 ; d++ )
         pts[p][d] = src[p][d] + delta[d];

   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], REAL direction[DIM], REAL witness[DIM])
{
   double test_val, val, worst;
   int p, nbad;

   test_val = dot_product( witness, direction) + ERROR_EPSILON;

   nbad = 0;
   for ( p=0 ; p<npts ; p++ )
     if ( dot_product( pts[p], direction) > test_val ) {
       val = dot_product( pts[p], 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,
	      const REAL (*pts)[DIM], REAL witness[DIM])
{
   double val, worst;
   int f, nf, nv, nbad;
   double faces[2*MAX_POINTS][4];

   /* construct the hull */
   
   nv = sac_qhull( npts, 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;
   }

/* clocktime should return a double, indicating time passing as a number
   of seconds.
   We use the system function clock, that returns a tick value in
   microseconds on a Sparc, but in units of 18.2/s = CLK_TCK on a PC.
   */
            
#ifndef CLK_TCK
#define CLK_TCK	1000000
#endif

static double clocktime( void)
{
   double num, den;
   num = (double) clock();
   den = (double) CLK_TCK;
   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;
   }