/******************************************************************************\ Copyright 1993 The Regents of the University of California. Modification Copyright 1995 The University of North Carolina at Chapel Hill. All Rights Reserved. Permission to use, copy, modify and distribute this software and its documentation for educational, research and non-profit purposes, without fee, and without a written agreement is hereby granted, provided that the above copyright notice and the following three paragraphs appear in all copies. IN NO EVENT SHALL THE UNIVERSITY OF CALIFORNIA OR THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF CALIFORNIA OR THE UNIVERSITY OF N. CAROLINA HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES. Permission to use, copy, modify and distribute this software and its documentation for educational, research and non-profit purposes, without fee, and without a written agreement is hereby granted, provided that the above copyright notice and the following three paragraphs appear in all copies. THE UNIVERSITY OF CALIFORNIA AND THE UNIVERSITY OF NORTH CAROLINA SPECIFICALLY DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF CALIFORNIA AND THE UNIVERSITY OF N. CAROLINA HAVE NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. The authors may be contacted via: US Mail: Brian Mirtich J. Cohen/M. Lin/D. Manocha/K. Ponamgi 387 Soda Hall Department of Computer Science Computer Science Division Sitterson Hall, CB #3175 University of California University of N. Carolina Berkeley, CA 94720 Chapel Hill, NC 27599-3175 Phone: (510) 642-8149 (919)962-1749 EMail: mirtich@cs.berkeley.edu {cohenj,lin,manocha,ponamgi}@cs.unc.edu \*****************************************************************************/ /*****************************************************************************\ dist.c -- Description : This code implements Ming Lin's algorithm for tracking the closest features between a pair of moving polyhedra. See: Local Methods for Fast Computation of Distance Functions Ming C. Lin, John F. Canny ESRC 92-98 / RAMP 92-11 September 1992 \*****************************************************************************/ /*----------------------------- Local Includes -----------------------------*/ #include #include #include #include #include #include #include #include /*----------------------------- Local Constants -----------------------------*/ #define CONE_EPS 1.0e-7 /* flaring angles These are the angles by which to flare the various planes defining Voronoi cones. Planes of vertex Voronoi regions are flared in, planes of face Voronoi regions are flared out, and planes of edge Voronoi regions are flared in if they correspond to a vertex on the edge's coboundary, or in if they correspond to a face on the coboundary. Stated simply, the highest dimensional feature is given priority. All these constant are in radians and should be nonnegative. Also, the following inequalities must hold: FACE_FLARE > EDGE_F_FLARE FACE_FLARE > VERT_FLARE EDGE_V_FLARE > VERT_FLARE */ #define VERT_FLARE 0.5e-5 #define EDGE_V_FLARE 1.0e-5 #define EDGE_F_FLARE 1.0e-5 #define FACE_FLARE 2.0e-5 #define MAX_CYCLE_LENGTH 200 /* number of steps above which we assume a walk must be a cycle */ #define MAX_CYCLE_TRIES 5 /* number of times we will try to walk if the linear programming test doesn't come back with INFEASIBLE */ /*------------------------------ Local Macros -------------------------------*/ /* The original Lin/Canny implementation only allowed rigid-body transformations, but many graphics applications, at least, allow scaling in the transformation matrix. Certain operations may be a bit faster if you comment out this #define. */ #define ALLOW_SCALING_IN_POSE /* feature types These are the values stored in the tag fields of the __col_Vertex, __col_Edge, and __col_Face structures. The featTag macro returns the type of an arbitrary feature. */ #define V 1 #define E 2 #define F 3 #define featTag(featurePtr) (*((int *) (featurePtr))) /* memory allocation macros */ #define allocVertex (__col_Vertex *) calloc(1, sizeof(__col_Vertex)) #define allocEdge (__col_Edge *) calloc(1, sizeof(__col_Edge)) #define allocFace (__col_Face *) calloc(1, sizeof(__col_Face)) #define allocPolyhedron (__col_Polyhedron *) calloc(1, sizeof(__col_Polyhedron)) #define allocPnode (__col_PNode *) calloc(1, sizeof(__col_PNode)) #define allocFnode (col_Fnode *) calloc(1, sizeof(col_Fnode)) /* transformation macros These transform a vertex or an edge from one frame to another using the transformation matrix T. Transforming an vertex involves transforming a single set of coordinates. Transforming an edge involves transforming the coordinates of both its endpoints, as well as its direction vector */ #define xformVert(T, v) __col_xformPoint(T, (v)->coords, (v)->xcoords) #ifdef ALLOW_SCALING_IN_POSE #define xformEdge(T, e) \ { \ __col_xformPoint(T, (e)->v1->coords, (e)->v1->xcoords); \ __col_xformPoint(T, (e)->v2->coords, (e)->v2->xcoords); \ __col_vectSub((e)->v2->xcoords, (e)->v1->xcoords, (e)->xu); \ (e)->xlen = __col_vectNorm((e)->xu); \ (e)->xu[0] /= (e)->xlen; \ (e)->xu[1] /= (e)->xlen; \ (e)->xu[2] /= (e)->xlen; \ } #else /* no scaling in pose */ #define xformEdge(T, e) \ { \ __col_xformPoint(T, (e)->v1->coords, (e)->v1->xcoords); \ __col_xformPoint(T, (e)->v2->coords, (e)->v2->xcoords); \ __col_xformVect(T, (e)->u, (e)->xu); \ } #endif /* ALLOW_SCALING_IN_POSE */ /* ============================================================================= miscellaneous macros ============================================================================= */ #define SWAP(x,y,type) {type tmp; tmp = x; x = y; y = tmp;} /*------------------------------- Local Types -------------------------------*/ /*------------------------ Local Function Prototypes ------------------------*/ static int getWord(FILE *fp, char *s); static void addFeature(void *feat, col_Fnode **list); static void reverseFlist(col_Fnode **list); static void featureName(void *f, char *s); static __col_Vertex *findVertex(__col_Polyhedron *p, char *name); static __col_Edge *findEdge(__col_Polyhedron *p, char *name1, char *name2); static __col_Face *findFace(__col_Polyhedron *p, char *name); static int numFeats(__col_Polyhedron *p); static void *nthFeat(__col_Polyhedron *p, int n); static __col_Edge *newEdge(__col_Vertex *v1, __col_Vertex *v2); static void dumpPolyhedron(__col_Polyhedron *p); static void addPlane(__col_PNode *pn, __col_PNode **cone); static void flipPlane(col_Vect3 src, col_Vect3 dest); static void tweakPlaneNorm(col_Vect3 u, __col_Real eps, col_Vect3 nOrig, col_Vect3 nTweak); static void computeVertCone(__col_Vertex *v); static void computeFaceCone(__col_Face *f); static void buildCones(__col_Polyhedron *p); static void dumpCones(__col_Polyhedron *p); static int vertConeChk(__col_Vertex **v, col_Vect3 point, int update); static int edgeConeChk(__col_Edge **e, col_Vect3 point, int update); static int faceConeChk(__col_Face **f, col_Vect3 point, int update); static __col_Real Dvv(__col_Vertex *v1, __col_Vertex *v2); static __col_Real Dev(__col_Edge *e, __col_Vertex *v); static __col_Real Dfv(__col_Face *f, __col_Vertex *v); static __col_Real Dve(__col_Vertex *v, __col_Edge *e); static __col_Real Dee(__col_Edge *e1, __col_Edge *e2); static __col_Real Dfe(__col_Face *f, __col_Edge *e); static __col_Real Dff(__col_Face *f1, __col_Face *f2, col_Mat4 T12, col_Mat4 T21); static __col_Real Distance(void *feat1, void *feat2, col_Mat4 T12, col_Mat4 T21); static void *closestToVert(__col_Vertex *v, __col_Polyhedron *p, col_Mat4 Tvp); static void *closestToEdge(__col_Edge *e, __col_Polyhedron *p, col_Mat4 Tep); static void *closestToFace(__col_Face *f, __col_Polyhedron *p, col_Mat4 Tfp, col_Mat4 Tpf); static void edgeCPs(__col_Edge **e1, __col_Edge **e2, col_Vect3 cp1, col_Vect3 cp2); static void *closestEdgeOrVertOnFace(__col_Face *f, __col_Edge *e); static void closestEdges(__col_Face *f1, __col_Face *f2, col_Mat4 T12, __col_Edge **closest1, __col_Edge **closest2); static void *closestToFacePlane(__col_Face *f1, __col_Face *f2, col_Mat4 T12, col_Mat4 T21); static int polygonCut(__col_Face *f, __col_Edge *e, __col_Real *min, __col_Real *max); static int faceOverlap(__col_Face *f1, __col_Face *f2, col_Mat4 T12, col_Mat4 T21); static __col_Real vertex_vertex(__col_Vertex **v1, __col_Vertex **v2, col_Mat4 T12, col_Mat4 T21); static __col_Real vertex_edge(__col_Vertex **v, __col_Edge **e, col_Mat4 T12, col_Mat4 T21); static __col_Real edge_edge(__col_Edge **e1, __col_Edge **e2, col_Mat4 T12, col_Mat4 T21); static __col_Real vertex_face(__col_Vertex **v, __col_Face **f, col_Mat4 T12, col_Mat4 T21, __col_Polyhedron *facePoly); static __col_Real edge_face(__col_Edge **e, __col_Face **f, col_Mat4 T12, col_Mat4 T21, __col_Polyhedron *facePoly); static __col_Real face_face(__col_Face **f1, __col_Face **f2, col_Mat4 T12, col_Mat4 T21, __col_Polyhedron *face1poly, __col_Polyhedron *face2poly); static __col_Real closestFeaturesInit(__col_Polyhedron *poly1, void **feat1, __col_Polyhedron *poly2, void **feat2); static int LinProgTest(__col_Polyhedron *poly1, __col_Polyhedron *poly2, col_Mat4 T12, col_Mat4 T21); /*------------------------------ Local Globals ------------------------------*/ __col_Polyhedron __col_polyhedronLibrary[1000]; static int polyhedronLibraryCount=0; /*---------------------------------Functions-------------------------------- */ /* ============================================================================= miscellaneous routines These are general functions which are not specifically related to the distance algorithm, but are called by some of the routines. ============================================================================= */ /*****************************************************************************\ @ getWord() ----------------------------------------------------------------------------- description : Get next word on the line; skip over spaces and tabs. input : output : return true if we read a word, false if we hit \n first. notes : \*****************************************************************************/ static int getWord(FILE *fp, char *s) { char c; do fscanf(fp, "%c", &c); while ((c == ' ') || c == '\t'); if (c == '\n') return 0; ungetc(c, fp); fscanf(fp, "%s", s); return 1; } /** End of getWord() **/ /*****************************************************************************\ @ __col_randomInt() ----------------------------------------------------------------------------- description : return random # from {0, 1, ..., n-1} input : output : notes : \*****************************************************************************/ int __col_randomInt(int n) { double x; x = ((double) rand()) / 0x7FFFFFFF; /* x = random # in interval [0, 1) */ return (int) floor(x * n); } /** End of __col_randomInt() **/ /* ============================================================================= polyhedra and feature housekeeping These routines are for accessing the polyhedron library, and performing basic operations on polyhedra and their features. ============================================================================= */ /*****************************************************************************\ @ addFeature() ----------------------------------------------------------------------------- description : Add a feature to a feature list. This is a generic routine callable for any type of feature and feature list. input : output : notes : \*****************************************************************************/ static void addFeature(void *feat, col_Fnode **list) { col_Fnode *fn; if (!(fn = allocFnode)) { fprintf(stderr, "addFeature: allocation failed\n"); exit(1); } fn->f.any = feat; fn->next = *list; *list = fn; } /** End of addFeature() **/ /*****************************************************************************\ @ reverseFlist() ----------------------------------------------------------------------------- description : Because of the way feature lists are constructed (in a stack-like manner), it is sometimes necessary to reverse the order of the list. This is necessary, for example, to make the vertices in a face's vertex list appear in the list in CCW order. This routine reverse any feature list. input : output : notes : \*****************************************************************************/ static void reverseFlist(col_Fnode **list) { col_Fnode *last, *fn, *next; for (last = NULL, fn = *list; fn; fn = next) { next = fn->next; fn->next = last; last = fn; } if (last) *list = last; } /** End of reverseFlist() **/ /*****************************************************************************\ @ featureName() ----------------------------------------------------------------------------- description : Get name of an arbitrary feature f, and return in s. Vertices and faces have explicit names; the name of an edge is derived from the names of its endpoint vertices (in alphabetical order), separated by a "." . input : output : notes : \*****************************************************************************/ static void featureName(void *f, char *s) { int type; type = featTag(f); if (type == V) strcpy(s, ((__col_Vertex *)f)->name); else if (type == F) strcpy(s, ((__col_Face *)f)->name); else sprintf(s, "%s.%s", ((__col_Edge *)f)->v1->name, ((__col_Edge *)f)->v2->name); } /** End of featureName() **/ /*****************************************************************************\ @ findVertex() ----------------------------------------------------------------------------- description : Return pointer to a vertex stucture, given the vertex's name and the polyhedron where it lives. \*****************************************************************************/ static __col_Vertex *findVertex(__col_Polyhedron *p, char *name) { col_Fnode *fn; for (fn = p->verts; fn && strcmp(name, fn->f.v->name); fn = fn->next); return (fn) ? fn->f.v : NULL; } /** End of findVertex() **/ /*****************************************************************************\ @ findEdge() ----------------------------------------------------------------------------- description : Return pointer to an edge stucture, given the names of the edges endpoint vertices, and the polyhedron where it lives. The names of the vertices may be passed in either order. \*****************************************************************************/ static __col_Edge *findEdge(__col_Polyhedron *p, char *name1, char *name2) { col_Fnode *fn; if (strcmp(name1, name2) > 0) SWAP(name1, name2, char *); for (fn = p->edges; fn && (strcmp(name1, fn->f.e->v1->name) || strcmp(name2, fn->f.e->v2->name)); fn = fn->next); return (fn) ? fn->f.e : NULL; } /** End of findEdge() **/ /*****************************************************************************\ @ findFace() ----------------------------------------------------------------------------- description : Return pointer to a face stucture, given the face's name and the polyhedron where it lives. \*****************************************************************************/ static __col_Face *findFace(__col_Polyhedron *p, char *name) { col_Fnode *fn; for (fn = p->faces; fn && strcmp(name, fn->f.f->name); fn = fn->next); return (fn) ? fn->f.f : NULL; } /** End of findFace() **/ /*****************************************************************************\ @ numFeats() ----------------------------------------------------------------------------- description : return total number of features in a polyhedron \*****************************************************************************/ static int numFeats(__col_Polyhedron *p) { col_Fnode *fn; int n; n = 0; for (fn = p->verts; fn; fn = fn->next) n++; for (fn = p->edges; fn; fn = fn->next) n++; for (fn = p->faces; fn; fn = fn->next) n++; return n; } /** End of numFeats() **/ /*****************************************************************************\ @ nthFeat() ----------------------------------------------------------------------------- description : Return the nth feature of a polyhedron. The features are numbered starting from 0. Vertices are first, followed by edges, and then faces. \*****************************************************************************/ static void *nthFeat(__col_Polyhedron *p, int n) { col_Fnode *fn; for (fn = p->verts; n > 0 && fn; fn = fn->next) n--; if (fn) return fn->f.any; for (fn = p->edges; n > 0 && fn; fn = fn->next) n--; if (fn) return fn->f.any; for (fn = p->faces; n > 0 && fn; fn = fn->next) n--; return fn->f.any; } /** End of nthFeat() **/ /*****************************************************************************\ @ __col_randFeat() ----------------------------------------------------------------------------- description : Return a random feature from polyhedron. There are equal chances that a vertex, edge, or face will be returned. Within each category, all features have an equal chance of being selected. \*****************************************************************************/ void *__col_randFeat(__col_Polyhedron *p) { int type; col_Fnode *fn, *fn2; int i, n; type = __col_randomInt(3); if (type == 0) fn = p->verts; else if (type == 1) fn = p->edges; else fn = p->faces; /* count number of features in the selected list */ for (fn2 = fn, n = 0; fn2; n++, fn2 = fn2->next); /* pick a random feature */ n = __col_randomInt(n); for (i = 0; i < n; i++) fn = fn->next; return fn->f.any; } /** End of __col_randFeat() **/ /*****************************************************************************\ @ newEdge() ----------------------------------------------------------------------------- description : Create a new edge between vertices v1 and v2. Initialize the edge's vertex pointers, and it's unit direction vector and length fields. \*****************************************************************************/ static __col_Edge *newEdge(__col_Vertex *v1, __col_Vertex *v2) { __col_Edge *e; if (!(e = allocEdge)) { fprintf(stderr, "newEdge: allocation failed\n"); exit(1); } e->tag = E; if (strcmp(v1->name, v2->name) > 0) SWAP(v1, v2, __col_Vertex *); e->v1 = v1; e->v2 = v2; addFeature(e, &v1->edges); addFeature(e, &v2->edges); e->fl = e->fr = NULL; __col_vectSub(v2->coords, v1->coords, e->u); e->len = __col_vectNorm(e->u); e->u[0] /= e->len; e->u[1] /= e->len; e->u[2] /= e->len; #ifndef ALLOW_SCALING_IN_POSE e->xlen = e->len; #endif return e; } /** End of newEdge() **/ /*****************************************************************************\ @ __col_loadPolyhedronLibrary() ----------------------------------------------------------------------------- description : Read the polyhedron library from the file whose name is passed in. The library is stored into the global variable polyhedronLibrary. The global variable polyhedronLibraryCount is also initialized. \*****************************************************************************/ int __col_loadPolyhedronLibrary(char *fname, double scale) { FILE *fp; char s[80]; __col_Polyhedron *p; __col_Vertex *v; __col_Face *f; __col_Edge *e, *e1, *e2; __col_Vertex *start, *last; col_Fnode *fn; int cont; int nameCounter; int n; n = 0; if (!(fp = fopen(fname, "r"))) { fprintf(stderr, "loadPolyhedronLibrary: can't open file %s\n", fname); return -1; } while(1) { do fscanf(fp, "%s", s); while (!feof(fp) && strcmp(s, "polyhedron")); if (feof(fp)) break; p = &__col_polyhedronLibrary[n++]; fscanf(fp, "%s", p->name); __col_mat4Copy(__col_mat4IDENTITY, p->pose); p->verts = p->edges = p->faces = NULL; /* read vertices */ while (1) { fscanf(fp, "%s", s); if (s[0] == '*') break; if (!(v = allocVertex)) { fprintf(stderr, "loadPolyhedronLibrary: vertex allocation failed\n"); exit(1); } v->tag = V; strcpy(v->name, s); v->cone = NULL; if (sizeof v->coords[0] == sizeof(double)) fscanf(fp, "%lf %lf %lf", &v->coords[0], &v->coords[1], &v->coords[2]); else fscanf(fp, "%f %f %f", &v->coords[0], &v->coords[1], &v->coords[2]); v->coords[0] *= scale; v->coords[1] *= scale; v->coords[2] *= scale; v->edges = NULL; addFeature(v, &p->verts); } /* read faces */ nameCounter = 0; while (1) { fscanf(fp, "%s", s); if (s[0] == '*') break; if (s[0] == '-') sprintf(s, "#F_%d", ++nameCounter); if (!(f = allocFace)) { fprintf(stderr, "loadPolyhedronLibrary: face allocation failed\n"); exit(1); } f->tag = F; strcpy(f->name, s); f->verts = f->edges = NULL; f->cone = NULL; addFeature(f, &p->faces); getWord(fp, s); start = last = findVertex(p, s); do { if (cont = getWord(fp, s)) v = findVertex(p, s); else v = start; addFeature(v, &f->verts); if (e = findEdge(p, last->name, v->name)) { if (e->fl) e->fr = f; else e->fl = f; } else { e = newEdge(last, v); if (strcmp(last->name, v->name) < 0) e->fl = f; else e->fr = f; addFeature(e, &p->edges); } addFeature(e, &f->edges); last = v; } while (cont); /* compute face plane coeffs (& outward normal) */ e1 = f->edges->f.e; e2 = f->edges->next->f.e; /* the "not" below is necessary cuz we haven't reversed the vertex list to its proper CCW order yet; right now it's in CW order */ if (!(e1->v1 == e2->v2 || e1->v2 == e2->v1)) __col_vectXprod(e1->u, e2->u, f->plane); else __col_vectXprod(e2->u, e1->u, f->plane); __col_vectNormalize(f->plane, f->plane); f->plane[3] = - __col_vectDotProd(f->plane, f->verts->f.v->coords); } /* clean up lists */ for (fn = p->verts; fn; fn = fn->next) reverseFlist(&fn->f.v->edges); for (fn = p->faces; fn; fn = fn->next) { reverseFlist(&fn->f.f->verts->next); reverseFlist(&fn->f.f->edges); } reverseFlist(&p->verts); reverseFlist(&p->edges); reverseFlist(&p->faces); /* build the voronoi regions for the polyhedron */ buildCones(p); } fclose(fp); polyhedronLibraryCount = n; return n; } /** End of __col_loadPolyhedronLibrary() **/ /*****************************************************************************\ @ __col_createPolyhedron() ----------------------------------------------------------------------------- description : Once the polyhedron library has been initialized, we can create instances of the various polyhedron in the library. Any number of instances of the same polyhedron may exist, at different positions (poses) in space. We create a polyhedron instance by specifying the name of the reference polyhedron in the library (e.g. "cube"), and the name for this particular instance (e.g. "cube-1"). The former name is required; the latter one is optional, and can be NULL. A pointer to the newly instantiated polyhedron is returned. The pose matrix is initialized to the identity. Note that all instances of the same library polyhedron share the same vertex, edge, and face lists and voronoi structure. \*****************************************************************************/ __col_Polyhedron *__col_createPolyhedron(char *libName, char *name) { __col_Polyhedron *newP; int i; for (i = 0; i < polyhedronLibraryCount && strcmp(__col_polyhedronLibrary[i].name, libName) ; i++); if (i == polyhedronLibraryCount) { fprintf(stderr, "createPolyhedron: can't find %s in library\n", libName); return NULL; } if (!(newP = allocPolyhedron)) { fprintf(stderr, "createPolyhedron: polyhedron allocation failed\n"); exit(1); } strcpy(newP->name, name); __col_mat4Copy(__col_mat4IDENTITY, newP->pose); __col_mat4Copy(__col_mat4IDENTITY, newP->inv_pose); newP->verts = __col_polyhedronLibrary[i].verts; newP->edges = __col_polyhedronLibrary[i].edges; newP->faces = __col_polyhedronLibrary[i].faces; return newP; } /** End of __col_createPolyhedron() **/ /*****************************************************************************\ @ dumpPolyhedron() ----------------------------------------------------------------------------- description : print out all the features of a polyhedron \*****************************************************************************/ static void dumpPolyhedron(__col_Polyhedron *p) { col_Fnode *fn, *fn2; char s[80]; printf("polyhedron %s ===================\n", p->name); printf("vertices:\n"); for (fn = p->verts; fn; fn = fn->next) { printf("%-10s (%+6.2f, %+6.2f, %+6.2f)\n", fn->f.v->name, fn->f.v->coords[0], fn->f.v->coords[1], fn->f.v->coords[2]); printf(" edges:"); for (fn2 = fn->f.v->edges; fn2; fn2 = fn2->next) printf(" %s.%s", fn2->f.e->v1->name, fn2->f.e->v2->name); printf("\n"); } printf("edges:\n"); for (fn = p->edges; fn; fn = fn->next) { sprintf(s, "%s.%s", fn->f.e->v1->name, fn->f.e->v2->name); printf("%-15s l=%-10s r=%-10s\n", s, fn->f.e->fl->name, fn->f.e->fr->name); printf(" l = %+7.2f u = (%+6.3f, %+6.3f, %+6.3f)\n", fn->f.e->len, fn->f.e->u[0], fn->f.e->u[1], fn->f.e->u[2]); } printf("faces:\n"); for (fn = p->faces; fn; fn = fn->next) { printf("%-10s (%+6.2f, %+6.2f, %+6.2f) %+6.2f\n", fn->f.f->name, fn->f.f->plane[0], fn->f.f->plane[1], fn->f.f->plane[2], fn->f.f->plane[3]); printf(" vertices:"); for (fn2 = fn->f.f->verts; fn2; fn2 = fn2->next) printf(" %s", fn2->f.v->name); printf("\n edges:"); for (fn2 = fn->f.f->edges; fn2; fn2 = fn2->next) printf(" %s.%s", fn2->f.e->v1->name, fn2->f.e->v2->name); printf("\n"); } } /** End of dumpPolyhedron() **/ /* ============================================================================= voronoi region routines These routines are used for building the voronoi region structures of polyhedra, and for testing for membership of points in these regions. The three membership routines are vertConeChk, edgeConeChk, and faceConeChk, for testing membership of points within vertex, edge, and face voronoi regions respectively. Actually, the faceConeChk only checks against the side planes of the face's voronoi region; the base plane must be checked explicitly. "Points" are simply arrays of 3 doubles. All three membership routines are passed the feature whose cone is being checked as well as the point. These routines return true if the point was in the cone, and false otherwise. In the latter case, the feature pointer passed in will be updated with a new feature (of a different type) which neighbored the old feature, corresponding to the plane which was violated. We do not update the feature based on the first plane which is found to be violated, but rather on the plane which is "most violated." The amount of violation is measured by the distance of the point from the plane, assuming the point is on the negative side of the plane. A point on the positive side does not violated the plane. Hence, even if we find a plane which is violated, we continue to cycle through all planes of the cone, looking for the one which is most violated. The voronoi regions are "sticky" in that we make comparisons against -__COL_EPSILON rather than zero. This provides hysteresis to avoid cycling. The voronoi planes are oriented so that the "signed distance" of a point to a voronoi plane is positive if the point is on the region side of the plane, and negative if the point lies outside the region. N.B. The order in which the edge cone planes are built is important! We add the planes corresponding to vertices (the edge's endpoints) first, followed by the planes corresponding to the neighboring faces. This puts the face planes in the front of the edge's cone list. We depend on this order in the edge_face routine when we're checking that the the faces normal lies between the edge's neigboring faces' normals. ============================================================================= */ /*****************************************************************************\ @ addPlane() ----------------------------------------------------------------------------- description : Add a plane node to a cone list. A cone list is a list of planes describing the voronoi region of a vertex or an edge, or the side planes of the voronoi region of a face. \*****************************************************************************/ static void addPlane(__col_PNode *pn, __col_PNode **cone) { pn->next = *cone; *cone = pn; } /** End of addPlane() **/ /*****************************************************************************\ @ flipPlane() ----------------------------------------------------------------------------- description : Planes have an orientation, since we perform checks to see if points lie on the proper side of the plane. flipPlane negates the four parameters describing the source plane, to produce a destination plane which is identical to the source, except with opposite orientation. \*****************************************************************************/ static void flipPlane(col_Vect4 src, col_Vect4 dest) { dest[0] = - src[0]; dest[1] = - src[1]; dest[2] = - src[2]; dest[3] = - src[3]; } /** End of flipPlane() **/ /*****************************************************************************\ @ tweakPlaneNorm() ----------------------------------------------------------------------------- description : Rotate a plane normal epsilon radians about a given axis u. This should be done before the constant term of the plane vector is computed, since this term is based on the plane normal. u must be a unit vector! \*****************************************************************************/ static void tweakPlaneNorm(col_Vect3 u, __col_Real eps, col_Vect3 nOrig, col_Vect3 nTweak) { col_Mat3 R; __col_Real c, s, v; /* compute rotation matrix for rotating 3-vectors an angle eps about u */ s = sin(eps); c = cos(eps); v = 1.0 - c; R[0][0] = u[0] * u[0] * v + c; R[1][1] = u[1] * u[1] * v + c; R[2][2] = u[2] * u[2] * v + c; R[0][1] = u[0] * u[1] * v - u[2] * s; R[0][2] = u[0] * u[2] * v + u[1] * s; R[1][0] = u[1] * u[0] * v + u[2] * s; R[1][2] = u[1] * u[2] * v - u[0] * s; R[2][0] = u[2] * u[0] * v - u[1] * s; R[2][1] = u[2] * u[1] * v + u[0] * s; __col_xform3(R, nOrig, nTweak); } /** End of tweakPlaneNorm() **/ /*****************************************************************************\ @ computeVertCone() ----------------------------------------------------------------------------- description : Compute the voronoi region of a vertex. This creates a list of plane nodes (one for each incident edge) and points the vertex's cone field to this list. \*****************************************************************************/ static void computeVertCone(__col_Vertex *v) { __col_PNode *pn; col_Fnode *fn; __col_Edge *e; col_Vect3 u, tmpV; for (fn = v->edges; fn; fn = fn->next) { e = fn->f.e; /* compute vector about which to rotate vertex planes REWRITE this vector points toward the right of edge */ __col_vectAdd(e->fl->plane, e->fr->plane, tmpV); __col_vectXprod(e->u, tmpV, u); __col_vectNormalize(u, u); /* construct vertex plane */ if (!(pn = allocPnode)) { fprintf(stderr, "computeVertCone: pnode allocation failed\n"); exit(1); } if (e->v1 == v) { tweakPlaneNorm(u, VERT_FLARE, e->u, pn->plane); __col_vectNeg(pn->plane, pn->plane); } else tweakPlaneNorm(u, -VERT_FLARE, e->u, pn->plane); pn->plane[3] = - __col_vectDotProd(v->coords, pn->plane); pn->nbr = e; addPlane(pn, &v->cone); /* construct edge plane */ if (!(pn = allocPnode)) { fprintf(stderr, "computeVertCone: pnode allocation failed\n"); exit(1); } if (e->v1 == v) tweakPlaneNorm(u, EDGE_V_FLARE, e->u, pn->plane); else { tweakPlaneNorm(u, -EDGE_V_FLARE, e->u, pn->plane); __col_vectNeg(pn->plane, pn->plane); } pn->plane[3] = - __col_vectDotProd(v->coords, pn->plane); pn->nbr = v; addPlane(pn, &e->cone); } } /** End of computeVertCone() **/ /*****************************************************************************\ @ computeFaceCone() ----------------------------------------------------------------------------- description : Compute the voronoi region of a face. This creates a list of plane nodes (one for each edge of the face) and points the faces's cone field to this list. The final plane of the voronoi region is the plane of the face itself. This plane is stored explicitly when the face is created. \*****************************************************************************/ static void computeFaceCone(__col_Face *f) { __col_PNode *pn; __col_Edge *e; __col_Vertex *v; col_Fnode *fnE, *fnV; col_Vect3 norm; /* we only do side planes; base plane is already stored explicitly in f->plane */ for (fnE = f->edges, fnV = f->verts; fnE; fnE = fnE->next, fnV = fnV->next) { e = fnE->f.e; v = fnV->f.v; /* (e->v1 = v) <==> e goes CCW around f; f is left face of e (e->v2 = v) <==> e goes CW around f; f is right face of e */ /* construct face plane */ __col_vectXprod(f->plane, e->u, norm); if (!(pn = allocPnode)) { fprintf(stderr, "computeFaceCone: pnode allocation failed\n"); exit(1); } if (e->v1 == v) tweakPlaneNorm(e->u, FACE_FLARE, norm, pn->plane); else { tweakPlaneNorm(e->u, -FACE_FLARE, norm, pn->plane); __col_vectNeg(pn->plane, pn->plane); } pn->plane[3] = - __col_vectDotProd(v->coords, pn->plane); pn->nbr = e; addPlane(pn, &f->cone); /* construct edge plane */ if (!(pn = allocPnode)) { fprintf(stderr, "computeFaceCone: pnode allocation failed\n"); exit(1); } if (e->v1 == v) { tweakPlaneNorm(e->u, EDGE_F_FLARE, norm, pn->plane); __col_vectNeg(pn->plane, pn->plane); } else tweakPlaneNorm(e->u, -EDGE_F_FLARE, norm, pn->plane); pn->plane[3] = - __col_vectDotProd(v->coords, pn->plane); pn->nbr = f; addPlane(pn, &e->cone); } } /** End of computeFaceCone() **/ /*****************************************************************************\ @ buildCones() ----------------------------------------------------------------------------- description : build the voronoi region structure for an entire polyhedron \*****************************************************************************/ static void buildCones(__col_Polyhedron *p) { col_Fnode *fn; for (fn = p->verts; fn; fn = fn->next) computeVertCone(fn->f.v); for (fn = p->faces; fn; fn = fn->next) computeFaceCone(fn->f.f); } /** End of buildCones() **/ /*****************************************************************************\ @ dumpCones() ----------------------------------------------------------------------------- description : print out the voronoi region structure for an entire polyhedron \*****************************************************************************/ static void dumpCones(__col_Polyhedron *p) { col_Fnode *fn; __col_PNode *pn; printf("cone structure of polyhedron %s ===============\n", p->name); for (fn = p->verts; fn; fn = fn->next) { printf("*** vertex %s\n", fn->f.v->name); for (pn = fn->f.v->cone; pn; pn = pn->next) printf("%+7.3f %+7.3f %+7.3f %+7.3f => %s.%s\n", pn->plane[0], pn->plane[1], pn->plane[2], pn->plane[3], ((__col_Edge *)pn->nbr)->v1->name, ((__col_Edge *)pn->nbr)->v2->name); } for (fn = p->edges; fn; fn = fn->next) { printf("*** edge %s.%s\n", fn->f.e->v1->name, fn->f.e->v2->name); for (pn = fn->f.e->cone; pn; pn = pn->next) printf("%+7.3f %+7.3f %+7.3f %+7.3f => %s\n", pn->plane[0], pn->plane[1], pn->plane[2], pn->plane[3], (featTag(pn->nbr) == V) ? ((__col_Vertex *)pn->nbr)->name : ((__col_Face *)pn->nbr)->name); } for (fn = p->faces; fn; fn = fn->next) { printf("*** face %s\n", fn->f.f->name); for (pn = fn->f.f->cone; pn; pn = pn->next) { printf("%+7.3f %+7.3f %+7.3f %+7.3f => ", pn->plane[0], pn->plane[1], pn->plane[2], pn->plane[3]); if (pn->nbr) printf("%s.%s\n", ((__col_Edge *)pn->nbr)->v1->name, ((__col_Edge *)pn->nbr)->v2->name); else printf("----\n"); } } } /** End of dumpCones() **/ /*****************************************************************************\ @ vertConeChk() ----------------------------------------------------------------------------- description : Return true if a point lies within vertex v's voronoi region, else false. Update v to a neighboring edge in the latter case. \*****************************************************************************/ static int vertConeChk(__col_Vertex **v, col_Vect3 point, int update) { __col_PNode *pn; double min, dot; void *feat; min = __COL_INFINITY; for (pn = (*v)->cone; pn; pn = pn->next) if ((dot = __col_planeDist(pn->plane, point)) < min) { min = dot; feat = pn->nbr; } if (min > -CONE_EPS) return 1; if (update) *v = feat; return 0; } /** End of vertConeChk() **/ /*****************************************************************************\ @ edgeConeChk() ----------------------------------------------------------------------------- description : Return true if a point lies within edge e's voronoi region, else false. Update e to a neighboring vertex or face in the latter case. \*****************************************************************************/ static int edgeConeChk(__col_Edge **e, col_Vect3 point, int update) { __col_PNode *pn; double min, dot; void *feat; min = __COL_INFINITY; for (pn = (*e)->cone; pn; pn = pn->next) if ((dot = __col_planeDist(pn->plane, point)) < min) if (dot < min || featTag(pn->nbr) == F) { min = dot; feat = pn->nbr; } if (min >= -CONE_EPS) return 1; if (update) *e = feat; return 0; } /** End of edgeConeChk() **/ /*****************************************************************************\ @ faceConeChk() ----------------------------------------------------------------------------- description : Return true if a point lies within face f's voronoi region, else false. Update f to a neighboring edge in the latter case. \*****************************************************************************/ static int faceConeChk(__col_Face **f, col_Vect3 point, int update) { __col_PNode *pn; double min, dot; void *feat; min = __COL_INFINITY; for (pn = (*f)->cone; pn; pn = pn->next) if ((dot = __col_planeDist(pn->plane, point)) < min) { min = dot; feat = pn->nbr; } if (min >= -CONE_EPS) return 1; if (update) *f = feat; return 0; } /** End of faceConeChk() **/ /* ============================================================================= quick and dirty distance functions and closestToX functions Occasionally it is necessary to find the closest feature on polyhedron A to a particular feature on polyhedron B. We sometimes have to enumerate over all features on A. This happens, for instance, when a vertex on B lies in the "negative voronoi region" of a face on A. To perform such operations, we use quick and dirty distance functions. These are seven functions which return the distance between two pairs of features of a given type. They are quick and dirty because they sometimes return infinity rather than an actual distance. For instance, the distance between two faces is not even calculated unless the faces are parallel, for if they are not, we know some some other feature pair (possibly involving one of the faces) is closer. Thus Dff returns infinity if the faces aren't parallel. These functions are named "Dxy" where x and y are either "v", "e", or "f", for vertex, edge or face. Thus, Dev returns the distance between an edge and a vertex. We use these routines when searching over polyhedron A for the closest feature to a particular feature on B. For this reason, it is most efficient to transform the feature on B to polyhedron A's frame, rather than transforming every feature on A to B's frame. In most of these routines, it is thus assumed that the second feature passed in (from polyhedron B) has already been transformed to the frame of the first feature's polyhedron (polyhedron A). An exception is Dff (see below). The quick and dirty distance functions are called by the three closestToX functions: closestToVert, closestToEdge, and closestToFace. ClosestToVert finds the closest feature on A to a vertex on B by first transforming the vertex on B into A's frame, and making repeated calls to Dvv, Dev, and Dfv. Likewise, closestToEdge finds thje closest feature on A to an edge on B by transforming the edge to A's frame and calling Dve, Dee, Dfe while enumerating over all features of A. closestToFace works a little differently, since it is more difficult to transform an entire face into another frame. It still enumerates over all features on A, looking for the closest feature to a face on B, however if the current feature being examined on A is a vertex or edge, this feature is transformed into B's frame (rather than transforming the face on B into A's frame). Hence, we still make calls to Dfv and Dfe (Dvf and Def don't exist). If the current feature on A is a face, we call Dff, which assumes each face is in its own frame. This slight inefficiency in the closestToFace case is no big deal, due to the rarity of calling this function. Also note in the closestToX functions, we first enumerate over faces, then edges, then vertices. This gives priority to features of higher dimension, which is what we want. For example, if a face and an edge on A are both closest to the fixed feature on B, we'll return the face. ============================================================================= */ /*****************************************************************************\ @ Dvv() ----------------------------------------------------------------------------- description : Return distance between two vertices; the latter has been xformed into the frame of the former. \*****************************************************************************/ static __col_Real Dvv(__col_Vertex *v1, __col_Vertex *v2) { col_Vect3 offset; __col_vectSub(v1->coords, v2->xcoords, offset); return __col_vectNorm(offset); } /** End of Dvv() **/ /*****************************************************************************\ @ Dev() ----------------------------------------------------------------------------- description : Return distance between an edge and a vertex xformed into the edge's frame. \*****************************************************************************/ static __col_Real Dev(__col_Edge *e, __col_Vertex *v) { col_Vect3 w, offset; __col_Real lambda; __col_vectSub(v->xcoords, e->v1->coords, w); lambda = __col_vectDotProd(w, e->u); if (lambda < 0.0) lambda = 0.0; else if (lambda > e->len) lambda = e->len; __col_displacePoint(e->v1->coords, e->u, lambda, w); __col_vectSub(v->xcoords, w, offset); return __col_vectNorm(offset); } /** End of Dev() **/ /*****************************************************************************\ @ Dfv() ----------------------------------------------------------------------------- description : Return distance between a face and a vertex xformed into the face's frame, or possibly infinity. \*****************************************************************************/ static __col_Real Dfv(__col_Face *f, __col_Vertex *v) { __col_Real dist; __col_PNode *pn; for (pn = f->cone; pn; pn = pn->next) if (__col_planeDist(pn->plane, v->xcoords) < 0.0) return __COL_INFINITY; dist = __col_planeDist(f->plane, v->xcoords); if (dist < 0.0) return __COL_INFINITY; return dist; } /** End of Dfv() **/ /*****************************************************************************\ @ Dve() ----------------------------------------------------------------------------- description : Return distance between a vertex and an edge xformed into the vertex's frame. \*****************************************************************************/ static __col_Real Dve(__col_Vertex *v, __col_Edge *e) { col_Vect3 w, offset; __col_Real lambda; __col_vectSub(v->coords, e->v1->xcoords, w); lambda = __col_vectDotProd(w, e->xu); if (lambda < 0.0) lambda = 0.0; else if (lambda > e->xlen) lambda = e->xlen; __col_displacePoint(e->v1->xcoords, e->xu, lambda, w); __col_vectSub(v->coords, w, offset); return __col_vectNorm(offset); } /** End of Dve() **/ /*****************************************************************************\ @ Dee() ----------------------------------------------------------------------------- description : Return distance between two edges; the latter has been xformed into the frame of the former. \*****************************************************************************/ static __col_Real Dee(__col_Edge *e1, __col_Edge *e2) { col_Vect3 cp1, cp2, offset; edgeCPs(&e2, &e1, cp2, cp1); __col_vectSub(cp2, cp1, offset); return __col_vectNorm(offset); } /** End of Dee() **/ /*****************************************************************************\ @ Dfe() ----------------------------------------------------------------------------- description : Return distance between a face and an edge xformed into the face's frame, or possibly infinity. \*****************************************************************************/ static __col_Real Dfe(__col_Face *f, __col_Edge *e) { __col_Real h1, h2; __col_Real min, max; col_Vect3 minCut, maxCut; if (!polygonCut(f, e, &min, &max)) return __COL_INFINITY; __col_displacePoint(e->v1->xcoords, e->xu, min, minCut); __col_displacePoint(e->v1->xcoords, e->xu, max, maxCut); h1 = __col_planeDist(f->plane, minCut); h2 = __col_planeDist(f->plane, maxCut); if (h1 < 0.0 || h2 < 0.0) return __COL_INFINITY; return (h1 < h2) ? h1 : h2; } /** End of Dfe() **/ /*****************************************************************************\ @ Dff() ----------------------------------------------------------------------------- description : Return distance between two faces, or possibly infinity. The faces are each assumed to be in their own frames, and T12 and T21 are the xformation matrices from f1's frame to f2's frame and vice-versa, resp. If the faces are parallel, then we return the distance of a vertex on one face to the plane of the other. However, we do this computation both ways and take the minimum distance. For the vertex of a huge face might appear to be quite far off the plane of a tiny one due to limited precision. \*****************************************************************************/ static __col_Real Dff(__col_Face *f1, __col_Face *f2, col_Mat4 T12, col_Mat4 T21) { __col_Real k; __col_Real minDist, dist; col_Vect3 w; col_Fnode *fn; __col_Edge *e; __col_xformVect(T21, f2->plane, w); k = __col_vectDotProd(f1->plane, w); if (fabs(k) < 1.0 - __COL_EPSILON) return __COL_INFINITY; /* At this point we know faces are || */ minDist = __COL_INFINITY; /* Find closest edge on f1 to f2 */ for (fn = f1->edges; fn; fn = fn->next) { e = fn->f.e; xformEdge(T12, e); dist = Dfe(f2, e); if (dist < minDist) minDist = dist; } /* now test edges of f2 against f1 */ for (fn = f2->edges; fn; fn = fn->next) { e = fn->f.e; xformEdge(T21, e); dist = Dfe(f1, e); if (dist < minDist) minDist = dist; } return minDist; } /** End of Dff() **/ /*****************************************************************************\ @ closestToVert() ----------------------------------------------------------------------------- description : Return closest feature on polyhedron p to the vertex v. Tvp is the xformation matrix from v's frame to p's frame. \*****************************************************************************/ static void *closestToVert(__col_Vertex *v, __col_Polyhedron *p, col_Mat4 Tvp) { double dist, minDist; void *feat; col_Fnode *fn; /* xform vertex to frame of polyhedron p */ xformVert(Tvp, v); minDist = __COL_INFINITY; for (fn = p->faces; fn; fn = fn->next) { dist = Dfv(fn->f.f, v); if (dist < minDist) { minDist = dist; feat = fn->f.f; } } for (fn = p->edges; fn; fn = fn->next) { dist = Dev(fn->f.e, v); if (dist < minDist) { minDist = dist; feat = fn->f.e; } } for (fn = p->verts; fn; fn = fn->next) { dist = Dvv(fn->f.v, v); if (dist < minDist) { minDist = dist; feat = fn->f.v; } } return feat; } /** End of closestToVert() **/ /*****************************************************************************\ @ closestToEdge() ----------------------------------------------------------------------------- description : Return closest feature on polyhedron p to the edge e. Tep is the xformation matrix from e's frame to p's frame. \*****************************************************************************/ static void *closestToEdge(__col_Edge *e, __col_Polyhedron *p, col_Mat4 Tep) { double dist, minDist; void *feat; col_Fnode *fn; /* xform edge to frame of polyhedron p */ xformEdge(Tep, e); minDist = __COL_INFINITY; for (fn = p->faces; fn; fn = fn->next) { dist = Dfe(fn->f.f, e); if (dist < minDist) { minDist = dist; feat = fn->f.f; } } for (fn = p->edges; fn; fn = fn->next) { dist = Dee(fn->f.e, e); if (dist < minDist) { minDist = dist; feat = fn->f.e; } } for (fn = p->verts; fn; fn = fn->next) { dist = Dve(fn->f.v, e); if (dist < minDist) { minDist = dist; feat = fn->f.v; } } return feat; } /** End of closestToEdge() **/ /*****************************************************************************\ @ closestToFace() ----------------------------------------------------------------------------- description : Return closest feature on polyhedron p to the face f. Tfp is the xformation matrix from f's frame to p's frame, and Tpf is the inverse transformation. \*****************************************************************************/ static void *closestToFace(__col_Face *f, __col_Polyhedron *p, col_Mat4 Tfp, col_Mat4 Tpf) { double dist, minDist; void *feat; col_Fnode *fn; minDist = __COL_INFINITY; for (fn = p->faces; fn; fn = fn->next) { dist = Dff(fn->f.f, f, Tpf, Tfp); if (dist < minDist) { minDist = dist; feat = fn->f.f; } } for (fn = p->edges; fn; fn = fn->next) { xformEdge(Tpf, fn->f.e); dist = Dfe(f, fn->f.e); if (dist < minDist) { minDist = dist; feat = fn->f.e; } } for (fn = p->verts; fn; fn = fn->next) { xformVert(Tpf, fn->f.v); dist = Dfv(f, fn->f.v); if (dist < minDist) { minDist = dist; feat = fn->f.v; } } return feat; } /** End of closestToFace() **/ /* ============================================================================= miscellaneous distance support functions These functions are called by the six basic feature pair checks (i.e. vertex_vertex, vertex_edge, etc.) They perform functions such as computing the closest edge on a face to a given edge, or determining if two faces overlap. ============================================================================= */ /*****************************************************************************\ @ edgeCPs() ----------------------------------------------------------------------------- description : This routine computes closest points between edges e1 and e2, returning them in cp1 and cp2. All possible degenerecies are handled (I think). In cases where the closest points are not unique (this can sometimes happen with parallel edges), individual closest points are still selected. In these cases, we try to choose sensibly, for instance choosing the midpoint of the portion of the edge which is closest to the other edge. We assume e1 has been transformed into e2's frame, and both cp1 and cp2 are computed relative to e2's frame. \*****************************************************************************/ static void edgeCPs(__col_Edge **e1, __col_Edge **e2, col_Vect3 cp1, col_Vect3 cp2) { __col_Real k, lambda, lambda1, lambda2; col_Vect3 offset, w, y; __col_Real dot12, dot21; __col_Real h, t; __col_Real dist, minDist; int changed; k = __col_vectDotProd((*e1)->xu, (*e2)->u); __col_vectSub((*e2)->v1->coords, (*e1)->v1->xcoords, offset); if (fabs(k) > 1.0 - __COL_EPSILON) { /* case I: lines || or anti-|| */ k = (k > 0.0) ? 1.0 : -1.0; lambda1 = lambda2 = __COL_INFINITY; __col_vectSub((*e1)->v1->xcoords, (*e2)->v1->coords, w); dot12 = __col_vectDotProd(w, (*e2)->u); dot21 = - __col_vectDotProd(w, (*e1)->xu); t = dot12; h = dot12 + k * (*e1)->xlen; if (t <= 0.0) { if (h <= 0.0) lambda2 = 0.0; else if (h >= (*e2)->len) lambda2 = (*e2)->len / 2; } else if (t >= (*e2)->len) { if (h <= 0.0) lambda2 = (*e2)->len / 2; else if (h >= (*e2)->len) lambda2 = (*e2)->len; } t = dot21; h = dot21 + k * (*e2)->len; if (t <= 0.0) { if (h <= 0.0) lambda1 = 0.0; else if (h >= (*e1)->xlen) lambda1 = (*e1)->xlen / 2; } else if (t >= (*e1)->xlen) { if (h <= 0.0) lambda1 = (*e1)->xlen / 2; else if (h >= (*e1)->xlen) lambda1 = (*e1)->xlen; } if (lambda1 != (__col_Real) __COL_INFINITY && lambda2 == (__col_Real) __COL_INFINITY) lambda2 = dot12 + k * lambda1; else if (lambda2 != (__col_Real) __COL_INFINITY && lambda1 == (__col_Real) __COL_INFINITY) lambda1 = dot21 + k * lambda2; else if (lambda1 == (__col_Real) __COL_INFINITY) { /* n.b. lambda2 also == __COL_INFINITY */ if (t < 0.0) lambda1 = h / 2; else if (t > (*e1)->xlen) lambda1 = (h + (*e1)->xlen) / 2; else if (h < 0.0) lambda1 = t / 2; else lambda1 = (t + (*e1)->xlen) / 2; lambda2 = dot12 + k * lambda1; } /* compute cp's based on lambdas */ if (lambda1 <= 0.0) __col_vectCopy((*e1)->v1->xcoords, cp1); else if (lambda1 >= (*e1)->xlen) __col_vectCopy((*e1)->v2->xcoords, cp1); else __col_displacePoint((*e1)->v1->xcoords, (*e1)->xu, lambda1, cp1); if (lambda2 <= 0.0) __col_vectCopy((*e2)->v1->coords, cp2); else if (lambda2 >= (*e2)->len) __col_vectCopy((*e2)->v2->coords, cp2); else __col_displacePoint((*e2)->v1->coords, (*e2)->u, lambda2, cp2); } else { /* case II: lines not || */ __col_vectScale((*e2)->u, w, k); __col_vectSub((*e1)->xu, w, w); lambda1 = __col_vectDotProd(w, offset) / (1 - k * k); __col_vectScale((*e1)->xu, w, k); __col_vectSub((*e2)->u, w, w); lambda2 = - __col_vectDotProd(w, offset) / (1 - k * k); if (lambda1 >= 0.0 && lambda1 <= (*e1)->xlen && lambda2 >= 0.0 && lambda2 <= (*e2)->len) { __col_displacePoint((*e1)->v1->xcoords, (*e1)->xu, lambda1, cp1); __col_displacePoint((*e2)->v1->coords, (*e2)->u, lambda2, cp2); return; } /* (lambda1, lambda2) not in allowable region; check boundaries */ minDist = __COL_INFINITY; if (lambda1 < 0.0) { /* check boundary: lambda1 = 0 */ __col_vectSub((*e1)->v1->xcoords, (*e2)->v1->coords, w); lambda = __col_vectDotProd(w, (*e2)->u); if (lambda < 0.0) lambda = 0.0; else if (lambda > (*e2)->len) lambda = (*e2)->len; __col_displacePoint((*e2)->v1->coords, (*e2)->u, lambda, w); __col_vectSub((*e1)->v1->xcoords, w, y); dist = __col_vectNorm(y); if (dist < minDist) { minDist = dist; __col_vectCopy((*e1)->v1->xcoords, cp1); __col_vectCopy(w, cp2); } } else if (lambda1 > (*e1)->xlen) { /* check boundary: lambda1 = length1 */ __col_vectSub((*e1)->v2->xcoords, (*e2)->v1->coords, w); lambda = __col_vectDotProd(w, (*e2)->u); if (lambda < 0.0) lambda = 0.0; else if (lambda > (*e2)->len) lambda = (*e2)->len; __col_displacePoint((*e2)->v1->coords, (*e2)->u, lambda, w); __col_vectSub((*e1)->v2->xcoords, w, y); dist = __col_vectNorm(y); if (dist < minDist) { minDist = dist; __col_vectCopy((*e1)->v2->xcoords, cp1); __col_vectCopy(w, cp2); } } if (lambda2 < 0.0) { /* check boundary: lambda2 = 0 */ __col_vectSub((*e2)->v1->coords, (*e1)->v1->xcoords, w); lambda = __col_vectDotProd(w, (*e1)->xu); if (lambda < 0.0) lambda = 0.0; else if (lambda > (*e1)->xlen) lambda = (*e1)->xlen; __col_displacePoint((*e1)->v1->xcoords, (*e1)->xu, lambda, w); __col_vectSub((*e2)->v1->coords, w, y); dist = __col_vectNorm(y); if (dist < minDist) { minDist = dist; __col_vectCopy((*e2)->v1->coords, cp2); __col_vectCopy(w, cp1); } } else if (lambda2 > (*e2)->len) { /* check boundary: lambda2 = length2 */ __col_vectSub((*e2)->v2->coords, (*e1)->v1->xcoords, w); lambda = __col_vectDotProd(w, (*e1)->xu); if (lambda < 0.0) lambda = 0.0; else if (lambda > (*e1)->xlen) lambda = (*e1)->xlen; __col_displacePoint((*e1)->v1->xcoords, (*e1)->xu, lambda, w); __col_vectSub((*e2)->v2->coords, w, y); dist = __col_vectNorm(y); if (dist < minDist) { minDist = dist; __col_vectCopy((*e2)->v2->coords, cp2); __col_vectCopy(w, cp1); } } } } /** End of edgeCPs() **/ /*****************************************************************************\ @ closestEdgeOrVertOnFace() ----------------------------------------------------------------------------- description : Return the edge or vertex of face f which is closest to edge e. There is a slight bug in Ming's paper. In the paper, we only need to find the closest edge on f to e, but in fact this can lead to infinite looping in certain cases. If the closest point on the closest edge to e is an endpoint of that closest edge, we return the vertex instead of the edge. This solves the problem. notes : We assume e has been transformed to f's frame. \*****************************************************************************/ static void *closestEdgeOrVertOnFace(__col_Face *f, __col_Edge *e) { col_Fnode *fn; __col_Real dist, minDist; __col_Edge *e2, *closest; col_Vect3 cp1, cp2, tmp; minDist = __COL_INFINITY; for (fn = f->edges; fn; fn = fn->next) { e2 = fn->f.e; edgeCPs(&e, &e2, cp1, cp2); __col_vectSub(cp2, cp1, tmp); dist = __col_vectNorm(tmp); if (dist < minDist) { minDist = dist; if (__col_vectEqual(cp2, e2->v1->coords)) closest = (__col_Edge *) e2->v1; else if (__col_vectEqual(cp2, e2->v2->coords)) closest = (__col_Edge *) e2->v2; else closest = e2; } } return closest; } /** End of closestEdgeOrVertOnFace() **/ /*****************************************************************************\ @ closestEdges() ----------------------------------------------------------------------------- description : Find the closest pair of edges between faces f1 and f2; return them in closest1 & closest2. T12 is the transformation matrix from f1's frame to f2's frame. This is just a dumb n^2 algorithm which loops through all pairs of edges. A minor complication is that we favor edge pairs for which the closest points aren't endpoints of the edge. So in addition to computing the distance between two edges, we compute the "subdistance," an integer 0-2 which indicates how many of the closest points between the edge pair are endpoints. When looking for the closest pair, we first check the distance of a pair to the minimum distance found thus far. If these distances are close, we check the subdistances. \*****************************************************************************/ static void closestEdges(__col_Face *f1, __col_Face *f2, col_Mat4 T12, __col_Edge **closest1, __col_Edge **closest2) { col_Fnode *fn1, *fn2; __col_Real dist, minDist; int subDist, minSubDist; __col_Edge *e1, *e2; col_Vect3 cp1, cp2, tmp; minDist = __COL_INFINITY; for (fn1 = f1->edges; fn1; fn1 = fn1->next) { e1 = fn1->f.e; xformEdge(T12, e1); for (fn2 = f2->edges; fn2; fn2 = fn2->next) { e2 = fn2->f.e; edgeCPs(&e1, &e2, cp1, cp2); __col_vectSub(cp2, cp1, tmp); dist = __col_vectNorm(tmp); /* adjustment to favor edges for which closest pts aren't endpoints */ subDist = (__col_vectEqual(cp1, e1->v1->xcoords) || __col_vectEqual(cp1, e1->v2->xcoords)) ? 1 : 0; if (__col_vectEqual(cp2, e2->v1->coords) || __col_vectEqual(cp2, e2->v2->coords)) subDist++; if ((dist < minDist) || (dist < minDist + __COL_EPSILON && subDist < minSubDist)) { minDist = dist; minSubDist = subDist; *closest1 = e1; *closest2 = e2; } } } } /** End of closestEdges() **/ /*****************************************************************************\ @ closestToFacePlane() ----------------------------------------------------------------------------- description : Scan over the edges and vertices of face 2, and return the one which is closest to the plane of f1. T12 & T21 are the usual transformation matrices. Edges are favored over vertices when they are parallel to the face. \*****************************************************************************/ static void *closestToFacePlane(__col_Face *f1, __col_Face *f2, col_Mat4 T12, col_Mat4 T21) { col_Fnode *fn; __col_Edge *e; __col_Real dist, minDist, dist1, dist2; void *feat, *closest; col_Vect3 norm; minDist = __COL_INFINITY; __col_xformVect(T12, f1->plane, norm); for (fn = f2->edges; fn; fn = fn->next) { e = fn->f.e; xformVert(T21, e->v1); dist1 = fabs(__col_planeDist(f1->plane, e->v1->xcoords)); xformVert(T21, e->v2); dist2 = fabs(__col_planeDist(f1->plane, e->v2->xcoords)); if (dist1 < dist2 - __COL_EPSILON) { /* v1 is closer */ dist = dist1; feat = e->v1; } else if (dist2 < dist1 - __COL_EPSILON) { /* v2 is closer */ dist = dist2; feat = e->v2; } else { /* edge is parallel to face */ dist = (dist1 < dist2) ? dist1 : dist2; feat = e; } if ((dist < minDist) || (dist < minDist + __COL_EPSILON && featTag(closest) == V && featTag(feat) == E)) { minDist = dist; closest = feat; } } return closest; } /** End of closestToFacePlane() **/ /*****************************************************************************\ @ polygonCut() ----------------------------------------------------------------------------- description : Determine if edge e intersects face f's cone. Return true if it does, else false. If true, min and max are parameter values which bound the segment of e contained in the cone. Note that 0 <= min < max <= length(e). We assume e has been xformed to f's frame. \*****************************************************************************/ static int polygonCut(__col_Face *f, __col_Edge *e, __col_Real *min, __col_Real *max) { __col_PNode *pn; __col_Real lambda; __col_Real denom; *min = 0.0; *max = e->xlen; for (pn = f->cone; pn; pn = pn->next) { denom = __col_vectDotProd(e->xu, pn->plane); if (denom > 0.0) { lambda = - __col_planeDist(pn->plane, e->v1->xcoords) / denom; if (lambda > *min) { *min = lambda; if (*min > *max) return 0; } } else if (denom < 0.0) { lambda = - __col_planeDist(pn->plane, e->v1->xcoords) / denom; if (lambda < *max) { *max = lambda; if (*max < *min) return 0; } } else { /* denom = 0.0 ; lines are parallel */ if (__col_planeDist(pn->plane, e->v1->xcoords) < 0.0) return 0; } } return 1; } /** End of polygonCut() **/ /*****************************************************************************\ @ faceOverlap() ----------------------------------------------------------------------------- description : Return true if faces overlap, else false. This is only called with pairs of parallel faces, and by "overlap" we mean the projection of one face onto the plane of the other overlaps the other face. We return true iff: some edge of f1 intersects the cone of f2, OR an abritrarily chosen vertex of f2 lies in the cone of f1 \*****************************************************************************/ static int faceOverlap(__col_Face *f1, __col_Face *f2, col_Mat4 T12, col_Mat4 T21) { col_Fnode *fn; __col_Real min, max; for (fn = f1->edges; fn; fn = fn->next) { xformEdge(T12, fn->f.e); if (polygonCut(f2, fn->f.e, &min, &max)) return 1; } /* no dice - maybe f2 lies completely within f1 */ xformVert(T21, f2->verts->f.v); return faceConeChk(&f1, f2->verts->f.v->xcoords, 0); } /** End of faceOverlap() **/ /* ============================================================================= feature pair checks and closest feature routine The six feature pair check routines test if a given pair of features is the closest pair of features between two polyhedra. Each one handles a different combination of feature types. The six routines are: vertex_vertex, vertex_edge, vertex_face, edge_edge, edge_face, and face_face. Each of these routines takes as parameters the two features as well as two transformation matrices for transforming features from one frame to the other and vice versa. T12 is always the transformation matrix from the frame of the first feature in the parameter list to the frame of the second feature in the parameter list; T21 is the inverse transformation. The check routines which involve faces also take pointer(s) to the polyhedron(a) of the face(s), since it is necessary to scan over the entire polyhedron when a face's base plane is violated by a feature from the other polyhedron. If pair of features passed into a check routine is indeed a closest feature pair, the distance between these features is returned. If the features are not a closest feature pair, -__COL_INFINITY is returned, and one or possibly both of the closest features are updated to form a new pair which is guaranteed to be closer than the previous pair. The routine closestFeatures is the main loop of the distance algorithm. It iteratively "walks" around both polyhedra, calling the appropriate feature pair check routines, until it finds the closest pair of features. It then returns this pair. ============================================================================= */ /*****************************************************************************\ @ vertex_vertex() ----------------------------------------------------------------------------- description : check if two vertices are a closest feature pair \*****************************************************************************/ static __col_Real vertex_vertex(__col_Vertex **v1, __col_Vertex **v2, col_Mat4 T12, col_Mat4 T21) { col_Vect3 offset; /* check if v1 lies in v2's cone */ xformVert(T12, *v1); if (!vertConeChk(v2, (*v1)->xcoords, 1)) return -__COL_INFINITY; /* check if v2 lies in v1's cone */ xformVert(T21, *v2); if (!vertConeChk(v1, (*v2)->xcoords, 1)) return -__COL_INFINITY; __col_vectSub((*v1)->coords, (*v2)->xcoords, offset); return __col_vectNorm(offset); } /** End of vertex_vertex() **/ /*****************************************************************************\ @ vertex_edge() ----------------------------------------------------------------------------- description : check if a vertex and an edge are a closest feature pair \*****************************************************************************/ static __col_Real vertex_edge(__col_Vertex **v, __col_Edge **e, col_Mat4 T12, col_Mat4 T21) { col_Vect3 tmp, tmp2, offset; __col_Real lambda; /* check if v lies in e's cone */ xformVert(T12, *v); if (!edgeConeChk(e, (*v)->xcoords, 1)) return -__COL_INFINITY;; /* compute closest point on e to v */ __col_vectSub((*v)->xcoords, (*e)->v1->coords, tmp); lambda = __col_vectDotProd(tmp, (*e)->u); if (lambda < 0.0) lambda = 0.0; else if (lambda > (*e)->len) lambda = (*e)->len; __col_displacePoint((*e)->v1->coords, (*e)->u, lambda, tmp); /* check if closest point on e lies in v's cone */ __col_xformPoint(T21, tmp, tmp2); if (!vertConeChk(v, tmp2, 1)) return -__COL_INFINITY; __col_vectSub((*v)->coords, tmp2, offset); return __col_vectNorm(offset); } /** End of vertex_edge() **/ /*****************************************************************************\ @ edge_edge() ----------------------------------------------------------------------------- description : check if two edges are a closest feature pair \*****************************************************************************/ static __col_Real edge_edge(__col_Edge **e1, __col_Edge **e2, col_Mat4 T12, col_Mat4 T21) { col_Vect3 cp1, cp2, offset, xformed; xformEdge(T12, *e1); edgeCPs(e1, e2, cp1, cp2); /* check if closest point on e1 lies in e2's cone */ if (!edgeConeChk(e2, cp1, 1)) return -__COL_INFINITY; /* check if closest point on e2 lies in e1's cone */ __col_xformPoint(T21, cp2, xformed); if (!edgeConeChk(e1, xformed, 1)) return -__COL_INFINITY; __col_vectSub(cp1, cp2, offset); return __col_vectNorm(offset); } /** End of edge_edge() **/ /*****************************************************************************\ @ vertex_face() ----------------------------------------------------------------------------- description : check if a vertex and a face are a closest feature pair \*****************************************************************************/ static __col_Real vertex_face(__col_Vertex **v, __col_Face **f, col_Mat4 T12, col_Mat4 T21, __col_Polyhedron *facePoly) { col_Vect3 tmp, tmp2; __col_Real dist; /* check if v lies in f's cone */ xformVert(T12, *v); if (!faceConeChk(f, (*v)->xcoords, 1)) return -__COL_INFINITY; dist = __col_planeDist((*f)->plane, (*v)->xcoords); if (dist < 0.0) { *f = closestToVert(*v, facePoly, T12); return -__COL_INFINITY; } /* Compute closest point on f to v, i.e. proj. of v in f's plane. At this point, we know the projected point lies within f */ __col_vectScale((*f)->plane, tmp, dist); __col_vectSub((*v)->xcoords, tmp, tmp); /* check if closest point on f lies in v's cone */ __col_xformPoint(T21, tmp, tmp2); if (!vertConeChk(v, tmp2, 1)) return -__COL_INFINITY; return dist; } /** End of vertex_face() **/ /*****************************************************************************\ @ edge_face() ----------------------------------------------------------------------------- description : check if an edge and a face are a closest feature pair \*****************************************************************************/ static __col_Real edge_face(__col_Edge **e, __col_Face **f, col_Mat4 T12, col_Mat4 T21, __col_Polyhedron *facePoly) { __col_PNode *pn; col_Vect3 w, w1, w2, tmp; __col_Real h1, h2, absH1, absH2; __col_Real dotL, dotR; __col_Real epsilon; col_Vect3 tmpV, tmpV1, tmpV2; __col_Real min, max; __col_Real dist; /* compute distance of v1 and v2 from face */ xformEdge(T12, *e); h1 = __col_planeDist((*f)->plane, (*e)->v1->xcoords); h2 = __col_planeDist((*f)->plane, (*e)->v2->xcoords); absH1 = fabs(h1); absH2 = fabs(h2); /* compute a variable epsilon which will prevent edge-face / vertex-face cycling and edge-face edge-edge cycling (1.5 is a safety factor) */ if (EDGE_F_FLARE > EDGE_V_FLARE /*VERT_FLARE*/) epsilon = /*1.5*/ 1.0e-4 + (*e)->xlen * sin(EDGE_F_FLARE); else epsilon = /*1.5*/ 1.0e-4 + (*e)->xlen * sin(EDGE_V_FLARE /*VERT_FLARE*/); /* fix for edges that would otherwise look || to f if h1 = -h2 */ if (((h1 < 0.0 && h2 > 0.0) || (h1 > 0.0 && h2 < 0.0)) && (fabs(h1 - h2) > epsilon)) { *f = (void *) closestEdgeOrVertOnFace(*f, *e); return -__COL_INFINITY; } if (absH1 < absH2 - epsilon) { /* case I: v1 is closer */ /* check if v1 lies in f's cone */ for (pn = (*f)->cone; pn; pn = pn->next) if (__col_planeDist(pn->plane, (*e)->v1->xcoords) < 0.0) break; if (pn) *f = (void *) closestEdgeOrVertOnFace(*f, *e); else if (h1 < 0.0) *f = closestToEdge(*e, facePoly, T12); else *e = (void *) (*e)->v1; return -__COL_INFINITY; } if (absH2 < absH1 - epsilon) { /* case II: v2 is closer */ /* check if v2 lies in f's cone */ for (pn = (*f)->cone; pn; pn = pn->next) if (__col_planeDist(pn->plane, (*e)->v2->xcoords) < 0.0) break; if (pn) *f = (void *) closestEdgeOrVertOnFace(*f, *e); else if (h2 < 0.0) *f = closestToEdge(*e, facePoly, T12); else *e = (void *) (*e)->v2; return -__COL_INFINITY; } else { /* case III: e is parallel to face */ /* project v1 & v2 into plane of f */ if (!polygonCut(*f, *e, &min, &max)) { *f = (void *) closestEdgeOrVertOnFace(*f, *e); return -__COL_INFINITY; } __col_displacePoint((*e)->v1->xcoords, (*e)->xu, min, tmpV1); h1 = __col_planeDist((*f)->plane, tmpV1); __col_displacePoint((*e)->v1->xcoords, (*e)->xu, max, tmpV2); h2 = __col_planeDist((*f)->plane, tmpV2); if (h1 < 0.0 || h2 < 0.0) { *f = closestToEdge(*e, facePoly, T12); return -__COL_INFINITY; } /* With flared planes, Ming's algo must be changed. We must compare the face's normal directly to the two face plane normals of the edge's cone, rather than comparing it to a vector derived from the edge direction and the normals of the neighboring faces. Note we assume the face planes are the first two in the edge's cone list! */ __col_xformVect(T21, (*f)->plane, w); if (__col_vectDotProd(w, (*e)->cone->plane) > 0.0) { *e = (void *) (*e)->cone->nbr; return -__COL_INFINITY; } if (__col_vectDotProd(w, (*e)->cone->next->plane) > 0.0) { *e = (void *) (*e)->cone->next->nbr; return -__COL_INFINITY; } if (h1 <= h2) { __col_vectScale((*f)->plane, tmp, h1); return h1; } else { __col_vectScale((*f)->plane, tmp, h2); return h2; } } } /** End of edge_face() **/ /*****************************************************************************\ @ face_face() ----------------------------------------------------------------------------- description : check if two faces are a closest feature pair The parallel, overlapping face-face case is rather complicated. We first try to compute the distance from face A to face B by finding the smallest positive dist of any vertex of A from the plane of B or any vertex of B from the plane of A. Negative distances are ignored under the assumption that they are just due to numerical errors that result when the vertex of A is very laterally distant from B (e.g. inifinite planes), unless all distances are negative, in which case we assume faces have interpenetrated. \*****************************************************************************/ static __col_Real face_face(__col_Face **f1, __col_Face **f2, col_Mat4 T12, col_Mat4 T21, __col_Polyhedron *face1poly, __col_Polyhedron *face2poly) { __col_PNode *pn; __col_Edge *e1, *e2, *e; col_Vect3 w, w1, w2, tmp; __col_Real dist1, dist2; __col_Real k, h1, h2; void *feat; __col_Real min1, min2; __col_Real min, max; int penetration; __col_Real dist, minDist; col_Vect3 tmpV, tmpV2; col_Fnode *fn; __col_xformVect(T21, (*f2)->plane, w); k = __col_vectDotProd((*f1)->plane, w); if (fabs(k) > 1.0 - __COL_EPSILON) { /* case I: faces are parallel */ if (faceOverlap(*f1, *f2, T12, T21)) { /* Modified this case. Used to determine if plane A was on (parallel) plane B's inner side if the first vertex of A was on B's negative side. This cause probs when A is very large compared to B - even if planes are roughly parallel, a distant vertex of A could appear to lie quite far into B's negative side. Fix: we require all of A's vertices to lie on B's negative side. In the case described, some of A's vertices will also lie far on the positive side, preventing the erroneous interpenetration deduction. */ minDist = __COL_INFINITY; penetration = 0; /* look at where f2's cone cuts f1's edges */ for (fn = (*f1)->edges; fn; fn = fn->next) { e = fn->f.e; xformEdge(T12, e); if (polygonCut(*f2, e, &min, &max)) { __col_displacePoint(e->v1->xcoords, e->xu, min, tmpV); dist = __col_planeDist((*f2)->plane, tmpV); if (dist < minDist) { if (dist < 0.0) break; /* interpentration */ minDist = dist; __col_vectScale((*f2)->plane, tmpV2, dist); } __col_displacePoint(e->v1->xcoords, e->xu, max, tmpV); dist = __col_planeDist((*f2)->plane, tmpV); if (dist < minDist) { if (penetration = (dist < 0.0)) break; minDist = dist; __col_vectScale((*f2)->plane, tmpV2, dist); } } } if (penetration) { *f2 = closestToFace(*f1, face2poly, T12, T21); return -__COL_INFINITY; } /* look at where f1's cone cuts f2's edges */ for (fn = (*f2)->edges; fn; fn = fn->next) { e = fn->f.e; xformEdge(T21, e); if (polygonCut(*f1, e, &min, &max)) { __col_displacePoint(e->v1->xcoords, e->xu, min, tmpV); dist = __col_planeDist((*f1)->plane, tmpV); if (dist < minDist) { if (dist < 0.0) break; /* interpentration */ minDist = dist; __col_vectScale((*f1)->plane, tmpV2, dist); } __col_displacePoint(e->v1->xcoords, e->xu, max, tmpV); dist = __col_planeDist((*f1)->plane, tmpV); if (dist < minDist) { if (penetration = (dist < 0.0)) break; minDist = dist; __col_vectScale((*f1)->plane, tmpV2, dist); } } } if (penetration) { *f2 = closestToFace(*f1, face2poly, T12, T21); return -__COL_INFINITY; } return minDist; } closestEdges(*f1, *f2, T12, &e1, &e2); *f1 = (void *) e1; *f2 = (void *) e2; return -__COL_INFINITY; } else { /* case II: faces not parallel */ /* return f2 & edge/vert of f1 */ feat = closestToFacePlane(*f2, *f1, T21, T12); if (featTag(feat) == V) { xformVert(T12, (__col_Vertex *)feat); for (pn = (*f2)->cone; pn; pn = pn->next) if (__col_planeDist(pn->plane, ((__col_Vertex *)feat)->xcoords) < 0.0) break; if (!pn) { *f1 = feat; return -__COL_INFINITY; } } else { e = (__col_Edge *)feat; xformEdge(T12, e); if (polygonCut(*f2, e, &min, &max)) { *f1 = feat; return -__COL_INFINITY; } } /* return f1 & edge/vert of f2 */ feat = closestToFacePlane(*f1, *f2, T12, T21); if (featTag(feat) == V) { xformVert(T21, (__col_Vertex *)feat); for (pn = (*f1)->cone; pn; pn = pn->next) if (__col_planeDist(pn->plane, ((__col_Vertex *)feat)->xcoords) < 0.0) break; if (!pn) { *f2 = feat; return -__COL_INFINITY; } } else { e = (__col_Edge *)feat; xformEdge(T21, e); if (polygonCut(*f1, e, &min, &max)) { *f2 = feat; return -__COL_INFINITY; } } closestEdges(*f1, *f2, T12, &e1, &e2); *f1 = (void *) e1; *f2 = (void *) e2; return -__COL_INFINITY; } } /** End of face_face() **/ static __col_Real Distance(void *feat1, void *feat2, col_Mat4 T12, col_Mat4 T21) { int type1, type2; __col_Real distance; type1 = featTag(feat1); type2 = featTag(feat2); switch(type1) { case V: switch(type2) { case V: xformVert(T12, (__col_Vertex *)feat1); distance = Dvv((__col_Vertex *)feat2, (__col_Vertex *)feat1); break; case E: xformVert(T12, (__col_Vertex *)feat1); distance = Dev((__col_Edge *)feat2, (__col_Vertex *)feat1); break; case F: xformVert(T12, (__col_Vertex *)feat1); distance = Dfv((__col_Face *)feat2, (__col_Vertex *)feat1); break; } break; case E: switch(type2) { case V: xformVert(T21, (__col_Vertex *)feat2); distance = Dvv((__col_Vertex *)feat1, (__col_Vertex *)feat2); break; case E: xformEdge(T21, (__col_Edge *)feat2); distance = Dee((__col_Edge *)feat1, (__col_Edge *)feat2); break; case F: xformEdge(T21, (__col_Edge *)feat1); distance = Dfe((__col_Face *)feat2, (__col_Edge *)feat1); break; } break; case F: switch(type2) { case V: xformVert(T21, (__col_Vertex *)feat2); distance = Dfv((__col_Face *)feat1, (__col_Vertex *)feat2); break; case E: xformEdge(T21, (__col_Edge *)feat2); distance = Dfe((__col_Face *)feat1, (__col_Edge *)feat2); break; case F: distance = Dff((__col_Face *)feat1, (__col_Face *)feat2, T12, T21); break; } break; } return distance; } static int LinProgTest(__col_Polyhedron *poly1, __col_Polyhedron *poly2, col_Mat4 T12, col_Mat4 T21) { static double d_vec[4] = {0.0, 0.0, 0.0, 1.0}; static double n_vec[4] = {1.0, 1.0, 1.0, 0.0}; static double opt[4] = {0.0, 0.0, 0.0, 0.0}; static double *halves = 0; static double *work = 0; static int *next = 0; static int *prev = 0; static int max_m = 0; int vert_count1, vert_count2; col_Fnode *fn; int status; int i, m; /* Count vertices in polytope 1 */ for (fn = poly1->verts, vert_count1 = 0; fn; vert_count1++, fn = fn->next); /* Count vertices in polytope 2 */ for (fn = poly2->verts, vert_count2 = 0; fn; vert_count2++, fn = fn->next); /* Transform poly2's vertices to poly1's coordinate system */ for (fn = poly2->verts; fn->next; fn = fn->next) xformVert(T21, fn->f.v); m = vert_count1 + vert_count2; if (m > max_m) { if (halves) free(halves); if (next) free(next); if (prev) free(prev); if (work) free(work); /* allocate array of half spaces */ halves = (double *)calloc(4*m, sizeof(double)); if (!(halves)) { fprintf(stderr, "LinProgTest: halves allocation failed\n"); exit(1); } next = (int *)calloc(m, sizeof(int)); if (!(next)) { fprintf(stderr, "LinProgTest: next allocation failed\n"); exit(1); } prev = (int *)calloc(m, sizeof(int)); if (!(prev)) { fprintf(stderr, "LinProgTest: prev allocation failed\n"); exit(1); } work = (double *)calloc((m+3)*5, sizeof(double)); if (!(work)) { fprintf(stderr, "LinProgTest: work allocation failed\n"); exit(1); } max_m = m; } i = 0; /* fill in half spaces for poly1 */ for (fn = poly1->verts; fn; fn = fn->next) { halves[i++] = fn->f.v->coords[0]; halves[i++] = fn->f.v->coords[1]; halves[i++] = fn->f.v->coords[2]; halves[i++] = 1.0; } /* fill in half spaces for poly2 */ for (fn = poly2->verts; fn; fn = fn->next) { halves[i++] = - fn->f.v->xcoords[0]; halves[i++] = - fn->f.v->xcoords[1]; halves[i++] = - fn->f.v->xcoords[2]; halves[i++] = - 1.0; } for (i = 0; i < m; i++) { next[i] = i + 1; prev[i] = i - 1; } status = linprog(halves,0,m,n_vec,d_vec,3,opt,work,next,prev,m); return status; } /*****************************************************************************\ @ __col_closestFeatures() ----------------------------------------------------------------------------- description : Compute the closest feature pair for polyhedra poly1 & poly2. The initial feature pair passed is passed in via feat1 & feat2, and upon termination these are updated to reflect the new closest feature pair. The routine returns the distance between the pair of closest features. We keep track of the number of iterations of the algorithm (i.e. how many "steps" we take as we walk around the polyhedra, looking for closest features). If this count reaches 100, we assume we are stuck in a cycle, and return the current closest feature pair after printing out a message. Under normal circumstances, this should never occur; the code to check for cycles has mainly been retained for debugging purposes. Cycles have in fact been detected when the two polyhedra overlap, but again this should not occur in a typical collision avoidance application. \*****************************************************************************/ __col_Real __col_closestFeatures(__col_Polyhedron *poly1, void **feat1, __col_Polyhedron *poly2, void **feat2) { static void *feat_cycle[MAX_CYCLE_LENGTH+1][2]; int type1, type2, type3, type4; char name1[20], name2[20]; double dist; col_Mat4 T12, T21, inv; int cycle_detected, cycleChk, cycle_tries, cycle_length; int i; int linprog_status; void *prev_closest_features[2]; /* compute xformation matrices between the two polyhedron frames */ __col_matMultXform(poly2->inv_pose, poly1->pose, T12); __col_matMultXform(poly1->inv_pose, poly2->pose, T21); prev_closest_features[0] = *feat1; prev_closest_features[1] = *feat2; linprog_status = -1; cycleChk = 0; cycle_detected = 0; cycle_tries = 0; do { feat_cycle[cycleChk][0] = *feat1; feat_cycle[cycleChk][1] = *feat2; for (i=0; i MAX_CYCLE_LENGTH) { fprintf(stderr, "closestFeatures: really long walk - cycle assumed\n"); fprintf(stderr, " (you might want to increase MAX_CYCLE_LENGTH)\n"); cycle_detected = 1; cycle_tries++; } if (cycle_detected) { if (linprog_status == -1) linprog_status = LinProgTest(poly1, poly2, T12, T21); switch(linprog_status) { case INFEASIBLE: /* collision */ /* we don't know much about the "closest" features when a collision occurs, so keep the previous closest features */ *feat1 = prev_closest_features[0]; *feat2 = prev_closest_features[1]; return 0.0; break; case MINIMUM: case AMBIGUOUS: if (cycle_tries > MAX_CYCLE_TRIES) { if (cycle_length > 3) /* cycling at actual minimum */ { fprintf(stderr, "LinProgTest: cycle length %d unresolved\n", cycle_length); } return Distance(*feat1, *feat2, T12, T21); } break; case UNBOUNDED: if (cycle_tries > MAX_CYCLE_TRIES) { /* don't know what to do if this comes back */ fprintf(stderr, "LinProgTest returned UNBOUNDED -- reporting no collision\n"); return __COL_INFINITY; } break; } cycleChk = 0; cycle_detected = 0; *feat1 = __col_randFeat(poly1); *feat2 = __col_randFeat(poly2); feat_cycle[0][0] = *feat1; feat_cycle[0][1] = *feat2; } type1 = featTag(*feat1); type2 = featTag(*feat2); if (type1 == V) { if (type2 == V) dist = vertex_vertex((__col_Vertex **)feat1, (__col_Vertex **)feat2, T12, T21); else if (type2 == E) dist = vertex_edge((__col_Vertex **)feat1, (__col_Edge **)feat2, T12, T21); else dist = vertex_face((__col_Vertex **)feat1, (__col_Face **)feat2, T12, T21, poly2); } else if (type1 == E) { if (type2 == V) dist = vertex_edge((__col_Vertex **)feat2, (__col_Edge **)feat1, T21, T12); else if (type2 == E) dist = edge_edge((__col_Edge **)feat1, (__col_Edge **)feat2, T12, T21); else dist = edge_face((__col_Edge **)feat1, (__col_Face **)feat2, T12, T21, poly2); } else { if (type2 == V) dist = vertex_face((__col_Vertex **)feat2, (__col_Face **)feat1, T21, T12, poly1); else if (type2 == E) dist = edge_face((__col_Edge **)feat2, (__col_Face **)feat1, T21, T12, poly1); else dist = face_face((__col_Face **)feat1, (__col_Face **)feat2, T12, T21, poly1, poly2); } } while (dist < 0.0); if (cycle_tries > (MAX_CYCLE_TRIES - 2)) { fprintf(stderr, "Found minimum, but cycle_tries == %d\n", cycle_tries); fprintf(stderr, " MAX_CYCLE_TRIES == %d -- maybe it should be greater\n", MAX_CYCLE_TRIES); } return dist; } /** End of __col_closestFeatures() **/ /*****************************************************************************\ @ closestFeaturesInit() ----------------------------------------------------------------------------- description : Functionally, this is identical to closestFeatures, except we call this routine the first time, when we have not yet initialized feat1 and feat2. This routine simply chooses a starting feature on each of the polyhedra, and then calls closestFeatures. We choose the starting features in a fairly dumb manner, picking the first vertex in each polyhedron's vertex list. This is totally arbitrary. There may be smarter ways of doing it, but it really doesn't matter, since we only perform this operation once, and then all subsequent calls are to closestFeatures. \*****************************************************************************/ static __col_Real closestFeaturesInit(__col_Polyhedron *poly1, void **feat1, __col_Polyhedron *poly2, void **feat2) { *feat1 = poly1->verts->f.v; *feat2 = poly2->verts->f.v; return __col_closestFeatures(poly1, feat1, poly2, feat2); } /** End of closestFeaturesInit() **/ /*****************************************************************************\ \*****************************************************************************/