/* Header file for the Oxford University Computing Laboratory version 1.0
   of Gilbert, Johnson and Keerthi's minimum distance routine.

      (c) Oxford University Computing Laboratory 1994.

   */

#include <stdio.h>
#include <stdlib.h>

#define GATHER_STATISTICS

#define DIM		3       /* dimension of space (i.e., x/y/z = 3) */
/* REAL is the type of a coordinate, INDEX of an index into the point arrays */
typedef double	REAL;
typedef int		INDEX;

#ifdef GATHER_STATISTICS
extern 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

/* The simplex_point structure is really designed to be private to the
   main GJK routines.  However the calling routine may wish to allocate
   some to be used to cache information for the distance routines.
   */
struct simplex_point {
      int    npts;   /* number of points in this simplex */
                     /* simplex1 and simplex2 are two arrays of
                        indices into the point arrays, given by the
                        user. */
      INDEX  simplex1[DIM+1];
      INDEX  simplex2[DIM+1];
                     /* and lambdas gives the values of lambda
                        associated with those points.
                        */
      REAL   lambdas[DIM+1];
      INDEX last_best1, last_best2;
                 /* last maximal vertices, used for hill-climbing */
      };

/* Even this algorithm has an epsilon (fudge) factor.  It basically indicates
   how far apart two points have to be to declared different, expressed 
   loosely as a proportion of the `average distance' between the point sets.
 */
#define EPSILON ((REAL) 0.00001)

/* TINY is used in one place, to indicate when a positive number is getting
   so small that we loose confidence in being able to divide a positive
   number smaller than it into it, and still believing the result.
   */
#define TINY	((REAL) 1.0e-20)  /* probably pessimistic! */

/* Fatal message handler. Fatal errors can only occur at the first call,
   and indicate inconsistent internal tables.  The string indicates
   the exact problem that has occured.
   */
#define	fatal( str)	   fprintf( stderr, "<<%s\n>>", str)


/* Error message handler. These are only generated by bad arguments to the
   main routines.  The string indicates the problem that has occured.
   */
#define	bad( str)	   fprintf( stderr, "<<%s\n>>", str)


/* Arithmetic operators on type REAL: defined here to make it
   easy (say) to use fixed point arithmetic. Note that addition
   and subtraction are assumed to work normally.
   */

#define MULTIPLY( a, b)		((a)*(b))
#define DIVIDE( num, den)	((num)/(den))
	/* note that this definition of DOT_PRODUCT depends on DIM */
#define DOT_PRODUCT( a, b)	(     MULTIPLY( (a)[0],(b)[0]) + \
                                 MULTIPLY( (a)[1],(b)[1]) + \
                                 MULTIPLY( (a)[2],(b)[2]) )
#define ZERO			      ((REAL) 0)
                           /* given the need for addition to work ZERO
                              could hardly be anything else ... */
#define ONE			         ((REAL) 1)

/* MAX_RING_SIZE gives an upper bound on the size of the array of rings
 * of edges in terms of the number of vertices.  From the formula
 *   v - e + f = 2
 * and the relationships that there are two half-edges for each edge,
 * and at least 3 half-edges per face, we obtain
 *   h <= 6v - 12
 * Add to this another v entries for the initial pointers to each ring,
 * another v entries to indicate the end of each ring list, and round it
 * up.
 */
#define MAX_RING_SIZE_MULTIPLIER	8

/*
 * A subsidary routine, that given a simplex record, the point arrays to
 * which it refers, an integer 1 or 2, and a pointer to a vector, sets
 * the coordinates of that vector to the coordinates of either the first
 * or second witness point given by the simplex record.
 */
int gjk_extract_point( struct simplex_point *simp, REAL (*pts)[DIM],
		       int whichpoint, REAL vector[]);

/* declaration of the main distance routine. */

/*
 * The main GJK distance routine. It takes two arrays of points, together
 * with counts of the number of points in the arrays, and sets two
 * witness points within the convex hulls of the two point sets that
 * realise the minimum distance between them.  The actual return value
 * is the square of the minimum distance between them.   Alternatively,
 * either or both of these pointer values for the witness points can be
 * zero; if both are zero it will save a few multiplies, but leave the
 * caller to compute the coordinates of the witness points if required.
 *
 * Also provided with each array of points is a pointer that, if non-zero,
 * encodes the edge topology of the hull for use in hill-climbing.
 *
 * The ninth argument is a pointer to a simplex_point structure.
 * If this is non-NULL then a special form of the witness points is
 * stored in the structure by the routine, suitable for passing to
 * this routine as seed points for any further calls involving these
 * two objects. The tenth argument is a flag, which when set tells
 * the routine to use the given simplex_point structure instance as
 * as seed, otherwise it just uses any seed.  (If the ninth argument
 * is NULL then no harm is done.)
 *
 */

REAL gjk_distance(
      int npts1, REAL (* pts1)[DIM], INDEX * rings1,
      int npts2, REAL (* pts2)[DIM], INDEX * rings2,
      REAL wpt1[DIM], REAL wpt2[DIM],
      struct simplex_point * simplex, int use_seed
      );