gjk.c

GJK距离计算算法实现

#include "gjk.h"

/*
 * Implementation of the Gilbert, Johnson and Keerthi routine to compute
 * the minimum distance between two convex polyhedra.
 *
 * Version 2.4, July 1998, (c) Stephen Cameron 1996, 1997, 1998
 *
 */

/* The code uses some tables that essentially encode the constant
   topology of simplices in DIM-dimensional space.  The original
   version of this program did this by constructing the tables
   each time the code was activated (but not once per run of gjk_distance!).
   In this version of the code we can define the constant USE_CONST_TABLES
   to use pre-defined versions of these tables instead.

   Not defining USE_CONST_TABLES makes the code behave as before.  This
   takes around 5000 integer operations at code initialisation when
   DIM==3.

   Defining USE_CONST_TABLES uses the pre-defined tables.

   Defining DUMP_CONST_TABLES makes the code construct appropriate
   versions of the tables for space of dimension <= DIM on the first
   call of gjk_distance, and to simply dump these to standard ouput and
   immediately call exit(0) to abort the programme.  This may be used if
   either the pre-defined tables have become corrupted, or if you wish
   to use pre-defined tables for a higher dimension.
*/
#define USE_CONST_TABLES
/**/

/* standard definitions, derived from those in gjk.h */

#define TWO_TO_DIM	   (1<<DIM)	/* must have TWO_TO_DIM = 2^DIM */
#define DIM_PLUS_ONE	   (DIM+1)
#define TWICE_TWO_TO_DIM   (TWO_TO_DIM+TWO_TO_DIM)

#define overd( ct)         for ( ct=0 ; ct<DIM ; ct++ )

/* The following #defines are defined to make the code easier to
   read: they are simply standard accesses of the following
   arrays.
   */
#define card( s)	cardinality[s]
#define max_elt( s)	max_element[s]
#define elts( s, i)	elements[s][i]
#define non_elts( s, i)	non_elements[s][i]
#define pred( s, i)	predecessor[s][i]
#define succ( s, i)	successor[s][i]
#define delta( s, i)	delta_values[s][i]
#define prod( i, j)	dot_products[i][j]

/* Decide whether the code the construct the constant tables should be
   linked into the source; this either happens if we have been asked to
   dump the tables to standard output, or if we have not been asked to use
   the pre-defined tables.
*/
#ifdef DUMP_CONST_TABLES
#define CONSTRUCT_TABLES
#else
#ifndef USE_CONST_TABLES
#define CONSTRUCT_TABLES
#endif /* USE_CONST_TABLES */
#endif /* DUMP_CONST_TABLES */

/* The following arrays store the constant subset structure -- see the
   comments in initialise_simplex_distance() for the data-invariants.
   Note that the entries could easily be packed, as say for any subset
   with index s we have only DIM_PLUS_ONE active entries in total for
   both elts( s,) and non_elts( s,), and ditto for prec( s,) and succ( s,).
   We have not bothered here as the tables are small.
   */
#ifdef CONSTRUCT_TABLES
static int cardinality[TWICE_TWO_TO_DIM];
static int max_element[TWICE_TWO_TO_DIM];
static int elements[TWICE_TWO_TO_DIM][DIM_PLUS_ONE];
static int non_elements[TWICE_TWO_TO_DIM][DIM_PLUS_ONE];
static int predecessor[TWICE_TWO_TO_DIM][DIM_PLUS_ONE];
static int successor[TWICE_TWO_TO_DIM][DIM_PLUS_ONE];
#else
/* the output of the routine to dump the six arrays above should be
   interpolated between this line and the following #endif */

#define PRE_DEFINED_TABLE_DIM 3

static const int cardinality[16] = {
	 0,  1,  1,  2,  1,  2,  2,  3,  1,  2,  2,  3,  2,  3,  3,  4};
static const int max_element[16] = {
	-1,  0,  1,  1,  2,  2,  2,  2,  3,  3,  3,  3,  3,  3,  3,  3};
static const int elements[16][4] = {
	{  0,  0,  0,  0},
	{  0,  0,  0,  0},
	{  1,  0,  0,  0},
	{  0,  1,  0,  0},
	{  2,  0,  0,  0},
	{  0,  2,  0,  0},
	{  1,  2,  0,  0},
	{  0,  1,  2,  0},
	{  3,  0,  0,  0},
	{  0,  3,  0,  0},
	{  1,  3,  0,  0},
	{  0,  1,  3,  0},
	{  2,  3,  0,  0},
	{  0,  2,  3,  0},
	{  1,  2,  3,  0},
	{  0,  1,  2,  3} };
static const int non_elements[16][4] = {
	{  0,  1,  2,  3},
	{  1,  2,  3,  0},
	{  0,  2,  3,  0},
	{  2,  3,  0,  0},
	{  0,  1,  3,  0},
	{  1,  3,  0,  0},
	{  0,  3,  0,  0},
	{  3,  0,  0,  0},
	{  0,  1,  2,  0},
	{  1,  2,  0,  0},
	{  0,  2,  0,  0},
	{  2,  0,  0,  0},
	{  0,  1,  0,  0},
	{  1,  0,  0,  0},
	{  0,  0,  0,  0},
	{  0,  0,  0,  0} };
static const int predecessor[16][4] = {
	{  0,  0,  0,  0},
	{  0,  0,  0,  0},
	{  0,  0,  0,  0},
	{  2,  1,  0,  0},
	{  0,  0,  0,  0},
	{  4,  1,  0,  0},
	{  4,  2,  0,  0},
	{  6,  5,  3,  0},
	{  0,  0,  0,  0},
	{  8,  1,  0,  0},
	{  8,  2,  0,  0},
	{ 10,  9,  3,  0},
	{  8,  4,  0,  0},
	{ 12,  9,  5,  0},
	{ 12, 10,  6,  0},
	{ 14, 13, 11,  7} };
static const int successor[16][4] = {
	{  1,  2,  4,  8},
	{  3,  5,  9,  0},
	{  3,  6, 10,  0},
	{  7, 11,  0,  0},
	{  5,  6, 12,  0},
	{  7, 13,  0,  0},
	{  7, 14,  0,  0},
	{ 15,  0,  0,  0},
	{  9, 10, 12,  0},
	{ 11, 13,  0,  0},
	{ 11, 14,  0,  0},
	{ 15,  0,  0,  0},
	{ 13, 14,  0,  0},
	{ 15,  0,  0,  0},
	{ 15,  0,  0,  0},
	{  0,  0,  0,  0} };

#endif /* CONSTRUCT_TABLES */

static REAL delta_values[TWICE_TWO_TO_DIM][DIM_PLUS_ONE];
static REAL dot_products[DIM_PLUS_ONE][DIM_PLUS_ONE];

#ifdef CONSTRUCT_TABLES
static void initialise_simplex_distance( void);
#endif

static VertexID support_function( Object obj,
			       VertexID, REAL *, REAL *);
static VertexID support_simple( Object obj,
			       VertexID, REAL *, REAL *);
static VertexID support_hill_climbing( Object obj,
			       VertexID, REAL *, REAL *);

static int default_distance( struct simplex_point * simplex);
static void backup_distance( struct simplex_point * simplex);
static void reset_simplex( int subset, struct simplex_point * simplex);

static void compute_subterms( struct simplex_point * s);
static void compute_point( REAL pt[DIM], int len,
                   REAL (* vertices)[DIM], REAL lambdas[]);
static void add_simplex_vertex( struct simplex_point * s, int pos,
				Object obj1, VertexID v1, Transform t1,
				Object obj2, VertexID v2, Transform t2);


/* The main GJK distance routine.  This routine implements the routine
 * of Gilbert, Johnson and Keerthi, as described in the paper (GJK88)
 * listed below.  It also optionally runs my speed-up extension to this
 * algorithm, as described in (Cam97).
 
 *
 * The first 4 parameters are two pairs of parameters, one pair for
 * each hull; each pair is an object data-structure, plus a
 * transformation data-structure.  These data-structures are defined
 * to this code in gjk.h, and are designed to be opaque to this code;
 * the data is accessed through selectors, iterators and prediciates,
 * which are discussed below.
 *
 * The 5th and 6th parameters are point arrays, that are set to the
 * coordinates of two witness points (one within each convex hull)
 * that realise the minimum distance between them.
 *
 * The actual return value for the function is the square of the
 * minimum distance between the hulls, which is equal to the (square
 * of the) distance between the witness points.
 *
 * The 7th parameter 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 8th parameter 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 7th parameter is
 * NULL then no harm is done.)
 *
 * Note that with this version one field of the simplex_point structure
 * can be used to pass back the confidence region for the routine: when
 * the routine returns with distance squared equal to D*d, it means that
 * the true distance is known to lie between D-(E/D) and D, where E is
 * the positive value returned in the `error' field of the simplex_point.
 * Equivalently; the true value of the distance squared is less than or equal
 * to the value returned DSQ, with an error bound width of 2E - E*E/DSQ.
 * (In `normal' cases E is very small, and so the error bound width 2E can
 * be sensibly used.)
 *
 * The code will attempt to return with E<=EPSILON, which the user
 * can set, but will in any event return with some value of E.  In particular,
 * the code should return even with EPSILON set to zero.
 *
 * Alternatively, either or both of the pointer values for the witness
 * points can be zero, in which case those witness points are not
 * returned.  The caller can later extract the coordinates of the
 * witness points from the simplex_point structure by using the
 * function gjk_extract_point.
 *
 * Returning to the opaque data-structures used to describe the objects
 * and their transformations.  For an object then the minimum information
 * required is a list of vertices.  The vertices themselves are another
 * opaque type, accessed through the type VertexID.  The following macros
 * are defined for the vertices:
 *
 *  InvalidVertexID	 a VertexID which cannot point to a valid vertex
 *  FirstVertex( obj)	 an arbitrary first vertex for the object
 *  NumVertices( obj)	 the number of vertices in the object
 *  IncrementVertex(o,v) set vertex to the next vertex
 *  ValidVertex( obj, v) says whether the VertexID is valid for obj
 *  LastVertex( obj, v)  is this the last vertex?
 *  SupportValue(o,v,d)  returns support value for v in direction d
 *
 * Optionally, the object data-structures encode the wireframe of the objects;
 * this is used in my extended GJK algorithm to greatly speed up the routine
 * in many cases.  For an object the predicate ValidRing( obj) says whether
 * this information is provided, in which case the edges that surround any
 * can be accessed and traversed with the following:
 *
 *  FirstEdge( obj, vertex)	Returns the first edge (type EdgeID)
 *  IncrementEdge( obj, edge)	Sets edge to the next edge
 *  ValidEdge( obj, edge)      	Indicates whether edge is a real edge
 *  VertexOfEdge( obj, edge)	Returns the (other) vertex of an edge
 *
 * With this information this routine runs in expected constant time
 * for tracking operations and small relative motions.  If the
 * information is not supplied the the routine reverts to using the
 * original GJK technique, which takes time roughly linear in the number
 * of vertices.  (As a rough rule of thumb, this difference becomes
 * measurable at around 10 vertices per hull, and important at about
 * 20 vertices per hull.)
 *
 * Transformations are stored in data-structures given by opaque type
 * Transform, for which the following operations need to be defined:
 *
 *  IdentityTransform( t)	Might t be an identity transformation?
 *  ExtractTranslation( t, v)	Set v to the translation component of t
 *  ApplyTransform( t,o,v,tgt)  Apply transform to vertex v of o, result in tgt
 *  ApplyInverseRotation(t,d,r) Apply inverse of t to direction d, result in r
 *
 * Notes:
 *  + it is OK for IdentityTransform( t) to return false when t is
 *    in fact the identity (with a small time penalty)
 *  + ExtractTranslation equivalently sets v to where the origin is
 *    transformed to by t
 *
 * References:
 * (GJK88) "A Fast Procedure for Computing the Distance between Complex
 * Objects in Three-Dimensional Space" by EG Gilbert, DW Johnson and SS
 * Keerthi, IEEE Trans Robotics and Automation 4(2):193--203, April 1988.
 *
 * (Cam97) "A Comparison of Two Fast Algorithms for Computing the Distance
 * between Convex Polyhedra" by Stephen Cameron, IEEE Trans Robotics and
 * Automation 13(6):915-920, December 1997.
 *
 */
REAL gjk_distance(
   Object obj1, Transform tr1,
   Object obj2, Transform tr2,
   REAL wpt1[DIM], REAL wpt2[DIM],
   struct simplex_point * simplex, int use_seed
   ) {

   VertexID v, p, maxp, minp;
   REAL minus_minv, maxv, sqrd, g_val;
   REAL displacementv[DIM], reverse_displacementv[DIM];
   REAL local_witness1[DIM], local_witness2[DIM];
   REAL local_fdisp[DIM], local_rdisp[DIM], trv[DIM];
   REAL * fdisp, * rdisp;
   struct simplex_point local_simplex;
   int d, compute_both_witnesses, use_default, first_iteration, max_iterations;
   double oldsqrd;

   assert( NumVertices( obj1)>0 && NumVertices( obj2)>0 );

   use_default = first_iteration = 1;
#ifdef CONSTRUCT_TABLES
   initialise_simplex_distance();
		/* will return immediately if already initialised */
#else
   assert( PRE_DEFINED_TABLE_DIM >= DIM );
#endif /* CONSTRUCT_TABLES */

   compute_both_witnesses = ( wpt1!=0 ) || ( wpt2!=0 ) ||
                            (  tr1!=0 ) || (  tr2!=0 );

   if ( wpt1==0 )
       wpt1 = local_witness1;

   if ( wpt2==0 )
       wpt2 = local_witness2;

   fdisp = IdentityTransform( tr1) ?         displacementv : local_fdisp;
   rdisp = IdentityTransform( tr2) ? reverse_displacementv : local_rdisp;

   if ( simplex==0 ) {
      use_seed = 0;