/* Header file for the Oxford version of Gilbert, Johnson and Keerthi's minimum distance routine. Version 2.1, April 1997, (c) Stephen Cameron 1996, 1997 */ #include #include #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]; REAL coords1[DIM+1][DIM]; REAL coords2[DIM+1][DIM]; /* calculated coordinates from the last iteration */ 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, * 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, int whichpoint, REAL vector[]); /* declaration of the main distance routine. */ /* The main GJK distance routine. The first 8 parameters are two sets * of four parameters, one for each hull. Each set of four parameters * starts with the number of vertices for that hull, and then an array * of the vertex cooordinates. The other two parameters for each hull * are described below. The 9th and 10th 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 11th 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 12th 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 11th parameter is * NULL then no harm is done.) * * 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 four parameters that describe each hull: the 3rd * parameter is a pointer that, if non-zero, encodes the edge topology * of the hull for use in hill-climbing. The edge topology is encoded * as follows. Consider a hull whose vertices are stored in array * Pts, and edge topology in integer array Ring. Then the indices of * the neighbouring vertices to the vertex with index i are Ring[j], * Ring[j+1], Ring[j+2], etc, where j = Ring[i] and the list of * indices are terminated with a negative number. Thus the contents * of Ring for a tetrahedron could be * * [ 4, 8, 12, 16, 1, 2, 3, -1, 0, 2, 3, -1, 0, 1, 3, -1, 0, 1, 2, -1 ] * * With this information this routine runs in expected constant time * for tracking operations and small relative motions. If the * information is not supplied (indicated by using a null pointer for * Ring) the the routine reverts to using the original GJK technique, * which takes time roughly linear in the number of vertices. (This * difference is not important until you reach more than, say, 20 * vertices per hull.) * * The fourth parameter per hull is a pointer to a transformation * matrix. This may be set to a null pointer, incidicating that an * indentity transformation should be used. (Unnecessary * multiplications are avoided by indicating an identity * transformation this way, as opposed to providing an explicit * identity transformation matrix.) For the `standard' case of * three-dimensional space the transformation matrix should be a 3x4 * or 4x4 matrix, designed so that if M is the matrix, R is the * rotation sub-matrix of the first 3 rows and columns, t is the * translation vector encoded in the fourth column, and x is a column * position vector, then M maps x to (Rx + t). Eqivalently, M maps * x to the vector y, where * y[i] = M[i][0]*x[0] + M[i][1]*x[1] + M[i][2]*x[2] + M[i][3] */ REAL gjk_distance( int npts1, REAL (* pts1)[DIM], INDEX * rings1, REAL (* tr1)[DIM+1], int npts2, REAL (* pts2)[DIM], INDEX * rings2, REAL (* tr2)[DIM+1], REAL wpt1[DIM], REAL wpt2[DIM], struct simplex_point * simplex, int use_seed );