graspit: /opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit

00001 /*<html><pre>  -<a                             href="qh-c.htm#poly"
00002   >-------------------------------</a><a name="TOP">-</a>
00003 
00004    poly.c 
00005    implements polygons and simplices
00006 
00007    see qh-c.htm, poly.h and qhull.h
00008 
00009    infrequent code is in poly2.c 
00010    (all but top 50 and their callers 12/3/95)
00011 
00012    copyright (c) 1993-1998, The Geometry Center
00013 */
00014 
00015 #include "qhull_a.h"
00016 
00017 #ifdef USE_DMALLOC
00018 #include "dmalloc.h"
00019 #endif
00020 
00021 /*======== functions in alphabetical order ==========*/
00022 
00023 /*-<a                             href="qh-c.htm#poly"
00024   >-------------------------------</a><a name="appendfacet">-</a>
00025   
00026   qh_appendfacet( facet )
00027     appends facet to end of qh.facet_list,
00028 
00029   returns:
00030     updates qh.facet_list, facet_tail, newfacet_list, facet_next
00031     increments qh.numfacets
00032   
00033   notes:
00034     assumes qh.facet_list/facet_tail is defined (createsimplex)
00035 
00036 */
00037 void qh_appendfacet(facetT *facet) {
00038   facetT *tail= qh facet_tail;
00039 
00040   if (tail == qh newfacet_list)
00041     qh newfacet_list= facet;
00042   if (tail == qh facet_next)
00043     qh facet_next= facet;
00044   facet->previous= tail->previous;
00045   facet->next= tail;
00046   if (tail->previous)
00047     tail->previous->next= facet;
00048   else
00049     qh facet_list= facet;
00050   tail->previous= facet;
00051   qh num_facets++;
00052   trace4((qh ferr, "qh_appendfacet: append f%d to facet_list\n", facet->id));
00053 } /* appendfacet */
00054 
00055 
00056 /*-<a                             href="qh-c.htm#poly"
00057   >-------------------------------</a><a name="appendvertex">-</a>
00058   
00059   qh_appendvertex( vertex )
00060     appends vertex to end of qh.vertex_list,
00061 
00062   returns:
00063     sets vertex->newlist
00064     updates qh.vertex_list, vertex_tail, newvertex_list
00065     increments qh.num_vertices
00066 
00067   notes:
00068     assumes qh.vertex_list/vertex_tail is defined (createsimplex)
00069 
00070 */
00071 void qh_appendvertex (vertexT *vertex) {
00072   vertexT *tail= qh vertex_tail;
00073 
00074   if (tail == qh newvertex_list)
00075     qh newvertex_list= vertex;
00076   vertex->newlist= True;
00077   vertex->previous= tail->previous;
00078   vertex->next= tail;
00079   if (tail->previous)
00080     tail->previous->next= vertex;
00081   else
00082     qh vertex_list= vertex;
00083   tail->previous= vertex;
00084   qh num_vertices++;
00085   trace4((qh ferr, "qh_appendvertex: append v%d to vertex_list\n", vertex->id));
00086 } /* appendvertex */
00087 
00088 
00089 /*-<a                             href="qh-c.htm#poly"
00090   >-------------------------------</a><a name="attachnewfacets">-</a>
00091   
00092   qh_attachnewfacets( )
00093     attach horizon facets to new facets in qh.newfacet_list
00094     newfacets have neighbor and ridge links to horizon but not vice versa
00095     only needed for qh.ONLYgood
00096 
00097   returns:
00098     set qh.NEWfacets
00099     horizon facets linked to new facets 
00100       ridges changed from visible facets to new facets
00101       simplicial ridges deleted
00102     qh.visible_list, no ridges valid
00103     facet->f.replace is a newfacet (if any)
00104 
00105   design:
00106     delete interior ridges and neighbor sets by
00107       for each visible, non-simplicial facet
00108         for each ridge
00109           if last visit or if neighbor is simplicial
00110             if horizon neighbor
00111               delete ridge for horizon's ridge set
00112             delete ridge
00113         erase neighbor set
00114     attach horizon facets and new facets by
00115       for all new facets
00116         if corresponding horizon facet is simplicial
00117           locate corresponding visible facet {may be more than one}
00118           link visible facet to new facet
00119           replace visible facet with new facet in horizon
00120         else it's non-simplicial
00121           for all visible neighbors of the horizon facet
00122             link visible neighbor to new facet
00123             delete visible neighbor from horizon facet
00124           append new facet to horizon's neighbors
00125           the first ridge of the new facet is the horizon ridge
00126           link the new facet into the horizon ridge
00127 */
00128 void qh_attachnewfacets (void ) {
00129   facetT *newfacet= NULL, *neighbor, **neighborp, *horizon, *visible;
00130   ridgeT *ridge, **ridgep;
00131 
00132   qh NEWfacets= True;
00133   trace3((qh ferr, "qh_attachnewfacets: delete interior ridges\n"));
00134   qh visit_id++;
00135   FORALLvisible_facets {
00136     visible->visitid= qh visit_id;
00137     if (visible->ridges) {
00138       FOREACHridge_(visible->ridges) {
00139         neighbor= otherfacet_(ridge, visible);
00140         if (neighbor->visitid == qh visit_id
00141             || (!neighbor->visible && neighbor->simplicial)) {
00142           if (!neighbor->visible)  /* delete ridge for simplicial horizon */
00143             qh_setdel (neighbor->ridges, ridge);
00144           qh_setfree (&(ridge->vertices)); /* delete on 2nd visit */
00145           qh_memfree (ridge, sizeof(ridgeT));
00146         }
00147       }
00148       SETfirst_(visible->ridges)= NULL;
00149     }
00150     SETfirst_(visible->neighbors)= NULL;
00151   }
00152   trace1((qh ferr, "qh_attachnewfacets: attach horizon facets to new facets\n"));
00153   FORALLnew_facets {
00154     horizon= SETfirstt_(newfacet->neighbors, facetT);
00155     if (horizon->simplicial) {
00156       visible= NULL;
00157       FOREACHneighbor_(horizon) {   /* may have more than one horizon ridge */
00158         if (neighbor->visible) {
00159           if (visible) {
00160             if (qh_setequal_skip (newfacet->vertices, 0, horizon->vertices,
00161                                   SETindex_(horizon->neighbors, neighbor))) {
00162               visible= neighbor;
00163               break;
00164             }
00165           }else
00166             visible= neighbor;
00167         }
00168       }
00169       if (visible) {
00170         visible->f.replace= newfacet;
00171         qh_setreplace (horizon->neighbors, visible, newfacet);
00172       }else {
00173         fprintf (qh ferr, "qhull internal error (qh_attachnewfacets): couldn't find visible facet for horizon f%d of newfacet f%d\n",
00174                  horizon->id, newfacet->id);
00175         qh_errexit2 (qh_ERRqhull, horizon, newfacet);
00176       }
00177     }else { /* non-simplicial, with a ridge for newfacet */
00178       FOREACHneighbor_(horizon) {    /* may hold for many new facets */
00179         if (neighbor->visible) {
00180           neighbor->f.replace= newfacet;
00181           qh_setdelnth (horizon->neighbors,
00182                         SETindex_(horizon->neighbors, neighbor));
00183           neighborp--; /* repeat */
00184         }
00185       }
00186       qh_setappend (&horizon->neighbors, newfacet);
00187       ridge= SETfirstt_(newfacet->ridges, ridgeT);
00188       if (ridge->top == horizon)
00189         ridge->bottom= newfacet;
00190       else
00191         ridge->top= newfacet;
00192       }
00193   } /* newfacets */
00194   if (qh PRINTstatistics) {
00195     FORALLvisible_facets {
00196       if (!visible->f.replace) 
00197         zinc_(Zinsidevisible);
00198     }
00199   }
00200 } /* attachnewfacets */
00201 
00202 /*-<a                             href="qh-c.htm#poly"
00203   >-------------------------------</a><a name="checkflipped">-</a>
00204   
00205   qh_checkflipped( facet, dist, allerror )
00206     checks facet orientation to interior point
00207 
00208     if allerror set,
00209       tests against qh.DISTround
00210     else
00211       tests against 0 since tested against DISTround before
00212 
00213   returns:
00214     False if it flipped orientation (sets facet->flipped)
00215     distance if non-NULL
00216 */
00217 boolT qh_checkflipped (facetT *facet, realT *distp, boolT allerror) {
00218   realT dist;
00219 
00220   if (facet->flipped && !distp)
00221     return False;
00222   zzinc_(Zdistcheck);
00223   qh_distplane(qh interior_point, facet, &dist);
00224   if (distp)
00225     *distp= dist;
00226   if ((allerror && dist > -qh DISTround)|| (!allerror && dist >= 0.0)) {
00227     facet->flipped= True;
00228     zzinc_(Zflippedfacets);
00229     trace0((qh ferr, "qh_checkflipped: facet f%d is flipped, distance= %6.12g during p%d\n",
00230               facet->id, dist, qh furthest_id));
00231     qh_precision ("flipped facet");
00232     return False;
00233   }
00234   return True;
00235 } /* checkflipped */
00236 
00237 /*-<a                             href="qh-c.htm#poly"
00238   >-------------------------------</a><a name="delfacet">-</a>
00239   
00240   qh_delfacet( facet )
00241     removes facet from facet_list and frees up its memory
00242 
00243   notes:
00244     assumes vertices and ridges already freed
00245 */
00246 void qh_delfacet(facetT *facet) {
00247   void **freelistp; /* used !qh_NOmem */
00248 
00249   trace5((qh ferr, "qh_delfacet: delete f%d\n", facet->id));
00250   if (facet == qh tracefacet)
00251     qh tracefacet= NULL;
00252   if (facet == qh GOODclosest)
00253     qh GOODclosest= NULL;
00254   qh_removefacet(facet);
00255   qh_memfree_(facet->normal, qh normal_size, freelistp);
00256   if (qh CENTERtype == qh_ASvoronoi) {   /* uses macro calls */
00257     qh_memfree_(facet->center, qh center_size, freelistp);
00258   }else /* AScentrum */ {
00259     qh_memfree_(facet->center, qh normal_size, freelistp);
00260   }
00261   qh_setfree(&(facet->neighbors));
00262   if (facet->ridges)
00263     qh_setfree(&(facet->ridges));
00264   qh_setfree(&(facet->vertices));
00265   if (facet->outsideset)
00266     qh_setfree(&(facet->outsideset));
00267   if (facet->coplanarset)
00268     qh_setfree(&(facet->coplanarset));
00269   qh_memfree_(facet, sizeof(facetT), freelistp);
00270 } /* delfacet */
00271 
00272 
00273 /*-<a                             href="qh-c.htm#poly"
00274   >-------------------------------</a><a name="deletevisible">-</a>
00275   
00276   qh_deletevisible()
00277     delete visible facets and vertices
00278 
00279   returns:
00280     deletes each facet and removes from facetlist
00281     at exit, qh.visible_list empty (== qh.newfacet_list)
00282 
00283   notes:
00284     ridges already deleted
00285     horizon facets do not reference facets on qh.visible_list
00286     new facets in qh.newfacet_list
00287     uses   qh.visit_id;
00288 */
00289 void qh_deletevisible (void /*qh visible_list*/) {
00290   facetT *visible, *nextfacet;
00291   vertexT *vertex, **vertexp;
00292   int numvisible= 0, numdel= qh_setsize(qh del_vertices);
00293 
00294   trace1((qh ferr, "qh_deletevisible: delete %d visible facets and %d vertices\n",
00295          qh num_visible, numdel));
00296   for (visible= qh visible_list; visible && visible->visible; 
00297                 visible= nextfacet) { /* deleting current */
00298     nextfacet= visible->next;        
00299     numvisible++;
00300     qh_delfacet(visible);
00301   }
00302   if (numvisible != qh num_visible) {
00303     fprintf (qh ferr, "qhull internal error (qh_deletevisible): qh num_visible %d is not number of visible facets %d\n",
00304              qh num_visible, numvisible);
00305     qh_errexit (qh_ERRqhull, NULL, NULL);
00306   }
00307   qh num_visible= 0;
00308   zadd_(Zvisfacettot, numvisible);
00309   zmax_(Zvisfacetmax, numvisible);
00310   zadd_(Zdelvertextot, numdel);
00311   zmax_(Zdelvertexmax, numdel);
00312   FOREACHvertex_(qh del_vertices) 
00313     qh_delvertex (vertex);
00314   qh_settruncate (qh del_vertices, 0);
00315 } /* deletevisible */
00316 
00317 /*-<a                             href="qh-c.htm#poly"
00318   >-------------------------------</a><a name="facetintersect">-</a>
00319   
00320   qh_facetintersect( facetA, facetB, skipa, skipB, prepend )
00321     return vertices for intersection of two simplicial facets
00322     may include 1 prepended entry (if more, need to settemppush)
00323     
00324   returns:
00325     returns set of qh.hull_dim-1 + prepend vertices
00326     returns skipped index for each test and checks for exactly one
00327 
00328   notes:
00329     does not need settemp since set in quick memory
00330   
00331   see also:
00332     qh_vertexintersect and qh_vertexintersect_new
00333     use qh_setnew_delnthsorted to get nth ridge (no skip information)
00334 
00335   design:
00336     locate skipped vertex by scanning facet A's neighbors
00337     locate skipped vertex by scanning facet B's neighbors
00338     intersect the vertex sets
00339 */
00340 setT *qh_facetintersect (facetT *facetA, facetT *facetB,
00341                          int *skipA,int *skipB, int prepend) {
00342   setT *intersect;
00343   int dim= qh hull_dim, i, j;
00344   facetT **neighborsA, **neighborsB;
00345 
00346   neighborsA= SETaddr_(facetA->neighbors, facetT);
00347   neighborsB= SETaddr_(facetB->neighbors, facetT);
00348   i= j= 0;
00349   if (facetB == *neighborsA++)
00350     *skipA= 0;
00351   else if (facetB == *neighborsA++)
00352     *skipA= 1;
00353   else if (facetB == *neighborsA++)
00354     *skipA= 2;
00355   else {
00356     for (i= 3; i < dim; i++) {
00357       if (facetB == *neighborsA++) {
00358         *skipA= i;
00359         break;
00360       }
00361     }
00362   }
00363   if (facetA == *neighborsB++)
00364     *skipB= 0;
00365   else if (facetA == *neighborsB++)
00366     *skipB= 1;
00367   else if (facetA == *neighborsB++)
00368     *skipB= 2;
00369   else {
00370     for (j= 3; j < dim; j++) {
00371       if (facetA == *neighborsB++) {
00372         *skipB= j;
00373         break;
00374       }
00375     }
00376   }
00377   if (i >= dim || j >= dim) {
00378     fprintf (qh ferr, "qhull internal error (qh_facetintersect): f%d or f%d not in others neighbors\n",
00379             facetA->id, facetB->id);
00380     qh_errexit2 (qh_ERRqhull, facetA, facetB);
00381   }
00382   intersect= qh_setnew_delnthsorted (facetA->vertices, qh hull_dim, *skipA, prepend);
00383   trace4((qh ferr, "qh_facetintersect: f%d skip %d matches f%d skip %d\n",
00384           facetA->id, *skipA, facetB->id, *skipB));
00385   return(intersect);
00386 } /* facetintersect */
00387 
00388 /*-<a                             href="qh-c.htm#poly"
00389   >-------------------------------</a><a name="gethash">-</a>
00390   
00391   qh_gethash( hashsize, set, size, firstindex, skipelem )
00392     return hashvalue for a set with firstindex and skipelem
00393 
00394   notes:
00395     assumes at least firstindex+1 elements
00396     assumes skipelem is NULL, in set, or part of hash
00397     
00398     hashes memory addresses which may change over different runs of the same data
00399     using sum for hash does badly in high d
00400 */
00401 unsigned qh_gethash (int hashsize, setT *set, int size, int firstindex, void *skipelem) {
00402   void **elemp= SETelemaddr_(set, firstindex, void);
00403   ptr_intT hash = 0, elem;
00404   int i;
00405 
00406   switch (size-firstindex) {
00407   case 1:
00408     hash= (ptr_intT)(*elemp) - (ptr_intT) skipelem;
00409     break;
00410   case 2:
00411     hash= (ptr_intT)(*elemp) + (ptr_intT)elemp[1] - (ptr_intT) skipelem;
00412     break;
00413   case 3:
00414     hash= (ptr_intT)(*elemp) + (ptr_intT)elemp[1] + (ptr_intT)elemp[2]
00415       - (ptr_intT) skipelem;
00416     break;
00417   case 4:
00418     hash= (ptr_intT)(*elemp) + (ptr_intT)elemp[1] + (ptr_intT)elemp[2]
00419       + (ptr_intT)elemp[3] - (ptr_intT) skipelem;
00420     break;
00421   case 5:
00422     hash= (ptr_intT)(*elemp) + (ptr_intT)elemp[1] + (ptr_intT)elemp[2]
00423       + (ptr_intT)elemp[3] + (ptr_intT)elemp[4] - (ptr_intT) skipelem;
00424     break;
00425   case 6:
00426     hash= (ptr_intT)(*elemp) + (ptr_intT)elemp[1] + (ptr_intT)elemp[2]
00427       + (ptr_intT)elemp[3] + (ptr_intT)elemp[4]+ (ptr_intT)elemp[5]
00428       - (ptr_intT) skipelem;
00429     break;
00430   default:
00431     hash= 0;
00432     i= 3;
00433     do {     /* this is about 10% in 10-d */
00434       if ((elem= (ptr_intT)*elemp++) != (ptr_intT)skipelem) {
00435         hash ^= (elem << i) + (elem >> (32-i));
00436         i += 3;
00437         if (i >= 32)
00438           i -= 32;
00439       }
00440     }while(*elemp);
00441     break;
00442   }
00443   hash %= (ptr_intT) hashsize;
00444   /* hash= 0;   for debugging purposes */
00445   return hash;
00446 } /* gethash */
00447 
00448 /*-<a                             href="qh-c.htm#poly"
00449   >-------------------------------</a><a name="makenewfacet">-</a>
00450   
00451   qh_makenewfacet( vertices, toporient, horizon )
00452     creates a toporient? facet from vertices
00453 
00454   returns:
00455     returns newfacet
00456       adds newfacet to qh.facet_list 
00457       newfacet->neighbor= horizon, but not vice versa
00458       newfacet->vertices= vertices
00459     newvertex_list updated with vertices
00460 */
00461 facetT *qh_makenewfacet(setT *vertices, boolT toporient,facetT *horizon) {
00462   facetT *newfacet;
00463   vertexT *vertex, **vertexp;
00464 
00465   FOREACHvertex_(vertices) {
00466     if (!vertex->newlist) {
00467       qh_removevertex (vertex);
00468       qh_appendvertex (vertex);
00469     }
00470   }
00471   newfacet= qh_newfacet();
00472   newfacet->vertices= vertices;
00473   newfacet->toporient= toporient;
00474   qh_setappend(&(newfacet->neighbors), horizon);
00475   qh_appendfacet(newfacet);
00476   return(newfacet);
00477 } /* makenewfacet */
00478 
00479 
00480 /*-<a                             href="qh-c.htm#poly"
00481   >-------------------------------</a><a name="makenewplanes">-</a>
00482   
00483   qh_makenewplanes()
00484     make new hyperplanes for facets on qh.newfacet_list
00485 
00486   returns:
00487     all facets have hyperplanes or are marked for   merging
00488     doesn't create hyperplane if horizon is coplanar (will merge)
00489     updates qh.min_vertex if qh.JOGGLEmax
00490 
00491   notes:
00492     facet->f.samecycle is defined for facet->mergehorizon facets
00493 */
00494 void qh_makenewplanes (void /* newfacet_list */) {
00495   facetT *newfacet;
00496 
00497   FORALLnew_facets {
00498     if (!newfacet->mergehorizon)
00499       qh_setfacetplane (newfacet);  
00500   }
00501   if (qh JOGGLEmax < REALmax/2)  
00502     minimize_(qh min_vertex, -wwval_(Wnewvertexmax));
00503 } /* makenewplanes */
00504 
00505 /*-<a                             href="qh-c.htm#poly"
00506   >-------------------------------</a><a name="makenew_nonsimplicial">-</a>
00507   
00508   qh_makenew_nonsimplicial( visible, apex, numnew )
00509     make new facets for ridges of a visible facet
00510     
00511   returns:
00512     first newfacet, bumps numnew as needed
00513     attaches new facets if !qh.ONLYgood
00514     marks ridge neighbors for simplicial visible
00515     if (qh.ONLYgood)
00516       ridges on newfacet, horizon, and visible
00517     else
00518       ridge and neighbors between newfacet and   horizon
00519       visible facet's ridges are deleted    
00520 
00521   notes:
00522     qh.visit_id if visible has already been processed
00523     sets neighbor->seen for building f.samecycle
00524       assumes all 'seen' flags initially false
00525     
00526   design:
00527     for each ridge of visible facet
00528       get neighbor of visible facet
00529       if neighbor was already processed
00530         delete the ridge (will delete all visible facets later)
00531       if neighbor is a horizon facet
00532         create a new facet
00533         if neighbor coplanar
00534           adds newfacet to f.samecycle for later merging
00535         else 
00536           updates neighbor's neighbor set
00537           (checks for non-simplicial facet with multiple ridges to visible facet)
00538         updates neighbor's ridge set
00539         (checks for simplicial neighbor to non-simplicial visible facet)
00540           
00541 */
00542 #ifndef qh_NOmerge
00543 facetT *qh_makenew_nonsimplicial (facetT *visible, vertexT *apex, int *numnew) {
00544   void **freelistp; /* used !qh_NOmem */
00545   ridgeT *ridge, **ridgep;
00546   facetT *neighbor, *newfacet= NULL, *samecycle;
00547   setT *vertices;
00548   boolT toporient;
00549   int ridgeid;
00550 
00551   FOREACHridge_(visible->ridges) {
00552     ridgeid= ridge->id;
00553     neighbor= otherfacet_(ridge, visible);
00554     if (neighbor->visible) {
00555       if (!qh ONLYgood) {
00556         if (neighbor->visitid == qh visit_id) {
00557           qh_setfree (&(ridge->vertices));  /* delete on 2nd visit */
00558           qh_memfree_(ridge, sizeof(ridgeT), freelistp);
00559         }
00560       }
00561     }else {  /* neighbor is an horizon facet */
00562       toporient= (ridge->top == visible);
00563       vertices= qh_setnew (qh hull_dim); /* makes sure this is quick */
00564       qh_setappend (&vertices, apex);
00565       qh_setappend_set (&vertices, ridge->vertices);
00566       newfacet= qh_makenewfacet(vertices, toporient, neighbor);
00567       (*numnew)++;
00568       if (neighbor->coplanar) {
00569         newfacet->mergehorizon= True;
00570         if (!neighbor->seen) {
00571           newfacet->f.samecycle= newfacet;
00572           neighbor->f.newcycle= newfacet;
00573         }else {
00574           samecycle= neighbor->f.newcycle;
00575           newfacet->f.samecycle= samecycle->f.samecycle;
00576           samecycle->f.samecycle= newfacet;
00577         }
00578       }
00579       if (qh ONLYgood) {
00580         if (!neighbor->simplicial)
00581           qh_setappend(&(newfacet->ridges), ridge);
00582       }else {  /* qh_attachnewfacets */
00583         if (neighbor->seen) {
00584           if (neighbor->simplicial) {
00585             fprintf (qh ferr, "qhull internal error (qh_makenew_nonsimplicial): simplicial f%d sharing two ridges with f%d\n", 
00586                    neighbor->id, visible->id);
00587             qh_errexit2 (qh_ERRqhull, neighbor, visible);
00588           }
00589           qh_setappend (&(neighbor->neighbors), newfacet);
00590         }else
00591           qh_setreplace (neighbor->neighbors, visible, newfacet);
00592         if (neighbor->simplicial) {
00593           qh_setdel (neighbor->ridges, ridge);
00594           qh_setfree (&(ridge->vertices)); 
00595           qh_memfree (ridge, sizeof(ridgeT));
00596         }else {
00597           qh_setappend(&(newfacet->ridges), ridge);
00598           if (toporient)
00599             ridge->top= newfacet;
00600           else
00601             ridge->bottom= newfacet;
00602         }
00603       trace4((qh ferr, "qh_makenew_nonsimplicial: created facet f%d from v%d and r%d of horizon f%d\n",
00604             newfacet->id, apex->id, ridgeid, neighbor->id));
00605       }
00606     }
00607     neighbor->seen= True;        
00608   } /* for each ridge */
00609   if (!qh ONLYgood)
00610     SETfirst_(visible->ridges)= NULL;
00611   return newfacet;
00612 } /* makenew_nonsimplicial */
00613 #else /* qh_NOmerge */
00614 facetT *qh_makenew_nonsimplicial (facetT *visible, vertexT *apex, int *numnew) {
00615   return NULL;
00616 }
00617 #endif /* qh_NOmerge */
00618 
00619 /*-<a                             href="qh-c.htm#poly"
00620   >-------------------------------</a><a name="makenew_simplicial">-</a>
00621   
00622   qh_makenew_simplicial( visible, apex, numnew )
00623     make new facets for simplicial visible facet and apex
00624 
00625   returns:
00626     attaches new facets if (!qh.ONLYgood)
00627       neighbors between newfacet and horizon
00628 
00629   notes:
00630     nop if neighbor->seen or neighbor->visible (see qh_makenew_nonsimplicial)
00631 
00632   design:
00633     locate neighboring horizon facet for visible facet
00634     determine vertices and orientation
00635     create new facet
00636     if coplanar,
00637       add new facet to f.samecycle
00638     update horizon facet's neighbor list        
00639 */
00640 facetT *qh_makenew_simplicial (facetT *visible, vertexT *apex, int *numnew) {
00641   facetT *neighbor, **neighborp, *newfacet= NULL;
00642   setT *vertices;
00643   boolT flip, toporient;
00644   int horizonskip, visibleskip;
00645 
00646   FOREACHneighbor_(visible) {
00647     if (!neighbor->seen && !neighbor->visible) {
00648       vertices= qh_facetintersect(neighbor,visible, &horizonskip, &visibleskip, 1);
00649       SETfirst_(vertices)= apex;
00650       flip= ((horizonskip & 0x1) ^ (visibleskip & 0x1));
00651       if (neighbor->toporient)         
00652         toporient= horizonskip & 0x1;
00653       else
00654         toporient= (horizonskip & 0x1) ^ 0x1;
00655       newfacet= qh_makenewfacet(vertices, toporient, neighbor);
00656       (*numnew)++;
00657       if (neighbor->coplanar && (qh PREmerge || qh MERGEexact)) {
00658 #ifndef qh_NOmerge
00659         newfacet->f.samecycle= newfacet;
00660         newfacet->mergehorizon= True;
00661 #endif
00662       }
00663       if (!qh ONLYgood)
00664         SETelem_(neighbor->neighbors, horizonskip)= newfacet;
00665       trace4((qh ferr, "qh_makenew_simplicial: create facet f%d top %d from v%d and horizon f%d skip %d top %d and visible f%d skip %d, flip? %d\n",
00666             newfacet->id, toporient, apex->id, neighbor->id, horizonskip,
00667               neighbor->toporient, visible->id, visibleskip, flip));
00668     }
00669   }
00670   return newfacet;
00671 } /* makenew_simplicial */
00672 
00673 /*-<a                             href="qh-c.htm#poly"
00674   >-------------------------------</a><a name="matchneighbor">-</a>
00675   
00676   qh_matchneighbor( newfacet, newskip, hashsize, hashcount )
00677     either match subridge of newfacet with neighbor or add to hash_table
00678 
00679   returns:
00680     duplicate ridges are unmatched and marked by qh_DUPLICATEridge
00681 
00682   notes:
00683     ridge is newfacet->vertices w/o newskip vertex
00684     do not allocate memory (need to free hash_table cleanly)
00685     uses linear hash chains
00686   
00687   see also:
00688     qh_matchduplicates
00689 
00690   design:
00691     for each possible matching facet in qh.hash_table
00692       if vertices match
00693         set ismatch, if facets have opposite orientation
00694         if ismatch and matching facet doesn't have a match
00695           match the facets by updating their neighbor sets
00696         else
00697           indicate a duplicate ridge
00698           set facet hyperplane for later testing
00699           add facet to hashtable
00700           unless the other facet was already a duplicate ridge
00701             mark both facets with a duplicate ridge
00702             add other facet (if defined) to hash table
00703 */
00704 void qh_matchneighbor (facetT *newfacet, int newskip, int hashsize, int *hashcount) {
00705   boolT newfound= False;   /* True, if new facet is already in hash chain */
00706   boolT same, ismatch;
00707   int hash, scan;
00708   facetT *facet, *matchfacet;
00709   int skip, matchskip;
00710 
00711   hash= (int)qh_gethash (hashsize, newfacet->vertices, qh hull_dim, 1, 
00712                      SETelem_(newfacet->vertices, newskip));
00713   trace4((qh ferr, "qh_matchneighbor: newfacet f%d skip %d hash %d hashcount %d\n",
00714           newfacet->id, newskip, hash, *hashcount));
00715   zinc_(Zhashlookup);
00716   for (scan= hash; (facet= SETelemt_(qh hash_table, scan, facetT)); 
00717        scan= (++scan >= hashsize ? 0 : scan)) {
00718     if (facet == newfacet) {
00719       newfound= True;
00720       continue;
00721     }
00722     zinc_(Zhashtests);
00723     if (qh_matchvertices (1, newfacet->vertices, newskip, facet->vertices, &skip, &same)) {
00724       if (SETelem_(newfacet->vertices, newskip) == 
00725           SETelem_(facet->vertices, skip)) {
00726         qh_precision ("two facets with the same vertices");
00727         fprintf (qh ferr, "qhull precision error: Vertex sets are the same for f%d and f%d.  Can not force output.\n",
00728           facet->id, newfacet->id);
00729         qh_errexit2 (qh_ERRprec, facet, newfacet);
00730       }
00731       ismatch= (same == (newfacet->toporient ^ facet->toporient));
00732       matchfacet= SETelemt_(facet->neighbors, skip, facetT);
00733       if (ismatch && !matchfacet) {
00734         SETelem_(facet->neighbors, skip)= newfacet;
00735         SETelem_(newfacet->neighbors, newskip)= facet;
00736         (*hashcount)--;
00737         trace4((qh ferr, "qh_matchneighbor: f%d skip %d matched with new f%d skip %d\n",
00738            facet->id, skip, newfacet->id, newskip));
00739         return;
00740       }
00741       if (!qh PREmerge && !qh MERGEexact) {
00742         qh_precision ("a ridge with more than two neighbors");
00743         fprintf (qh ferr, "qhull precision error: facets f%d, f%d and f%d meet at a ridge with more than 2 neighbors.  Can not continue.\n",
00744                  facet->id, newfacet->id, getid_(matchfacet));
00745         qh_errexit2 (qh_ERRprec, facet, newfacet);
00746       }
00747       SETelem_(newfacet->neighbors, newskip)= qh_DUPLICATEridge;
00748       newfacet->dupridge= True;
00749       if (!newfacet->normal)
00750         qh_setfacetplane (newfacet);
00751       qh_addhash (newfacet, qh hash_table, hashsize, hash);
00752       (*hashcount)++;
00753       if (!facet->normal)
00754         qh_setfacetplane (facet);
00755       if (matchfacet != qh_DUPLICATEridge) {
00756         SETelem_(facet->neighbors, skip)= qh_DUPLICATEridge;
00757         facet->dupridge= True;
00758         if (!facet->normal)
00759           qh_setfacetplane (facet);
00760         if (matchfacet) {
00761           matchskip= qh_setindex (matchfacet->neighbors, facet);
00762           SETelem_(matchfacet->neighbors, matchskip)= qh_DUPLICATEridge;
00763           matchfacet->dupridge= True;
00764           if (!matchfacet->normal)
00765             qh_setfacetplane (matchfacet);
00766           qh_addhash (matchfacet, qh hash_table, hashsize, hash);
00767           *hashcount += 2;
00768         }
00769       }
00770       trace4((qh ferr, "qh_matchneighbor: new f%d skip %d duplicates ridge for f%d skip %d matching f%d ismatch %d at hash %d\n",
00771            newfacet->id, newskip, facet->id, skip, 
00772            (matchfacet == qh_DUPLICATEridge ? -2 : getid_(matchfacet)), 
00773            ismatch, hash));
00774       return; /* end of duplicate ridge */
00775     }
00776   }
00777   if (!newfound) 
00778     SETelem_(qh hash_table, scan)= newfacet;  /* same as qh_addhash */
00779   (*hashcount)++;
00780   trace4((qh ferr, "qh_matchneighbor: no match for f%d skip %d at hash %d\n",
00781            newfacet->id, newskip, hash));
00782 } /* matchneighbor */
00783 
00784 
00785 /*-<a                             href="qh-c.htm#poly"
00786   >-------------------------------</a><a name="matchnewfacets">-</a>
00787   
00788   qh_matchnewfacets()
00789     match newfacets in qh.newfacet_list to their newfacet neighbors
00790 
00791   returns:
00792     qh.newfacet_list with full neighbor sets
00793       get vertices with nth neighbor by deleting nth vertex
00794     if qh.PREmerge/MERGEexact or qh.FORCEoutput 
00795       all facets check for flipped (also prevents point partitioning)
00796     if duplicate ridges and qh.PREmerge/MERGEexact
00797       facet->dupridge set
00798       missing neighbor links identifies extra ridges to be merging
00799 
00800   notes:
00801     newfacets already have neighbor[0] (horizon facet)
00802     assumes qh.hash_table is NULL
00803     vertex->neighbors has not been updated yet
00804     do not allocate memory after qh.hash_table (need to free it cleanly)
00805 
00806   design:
00807     delete neighbor sets for all new facets
00808     initialize a hash table
00809     for all new facets
00810       match facet with neighbors
00811     if unmatched facets (due to duplicate ridges)
00812       for each new facet with a duplicate ridge
00813         match it with a facet
00814     check for flipped facets
00815 */
00816 void qh_matchnewfacets (void) {
00817   int numnew=0, hashcount=0, newskip;
00818   facetT *newfacet, *neighbor;
00819   int dim= qh hull_dim, hashsize, neighbor_i, neighbor_n;
00820   setT *neighbors;
00821 #ifndef qh_NOtrace
00822   int facet_i, facet_n, numfree= 0;
00823   facetT *facet;
00824 #endif
00825   
00826   trace1((qh ferr, "qh_matchnewfacets: match neighbors for new facets.\n"));
00827   FORALLnew_facets {
00828     numnew++;
00829     {  /* inline qh_setzero (newfacet->neighbors, 1, qh hull_dim); */
00830       neighbors= newfacet->neighbors;
00831       neighbors->e[neighbors->maxsize].i= dim+1; /*may be overwritten*/
00832       memset ((char *)SETelemaddr_(neighbors, 1, void), 0, dim * SETelemsize);
00833     }    
00834   }
00835   qh_newhashtable (numnew*(qh hull_dim-1)); /* twice what is normally needed,
00836                                      but every ridge could be DUPLICATEridge */
00837   hashsize= qh_setsize (qh hash_table);
00838   FORALLnew_facets {
00839     for (newskip=1; newskip<qh hull_dim; newskip++) /* furthest/horizon already matched */
00840       qh_matchneighbor (newfacet, newskip, hashsize, &hashcount);
00841 #if 0   /* use the following to trap hashcount errors */
00842     {
00843       int count= 0, k;
00844       facetT *facet, *neighbor;
00845 
00846       count= 0;
00847       FORALLfacet_(qh newfacet_list) {  /* newfacet already in use */
00848         for (k=1; k < qh hull_dim; k++) {
00849           neighbor= SETelemt_(facet->neighbors, k, facetT);
00850           if (!neighbor || neighbor == qh_DUPLICATEridge)
00851             count++;
00852         }
00853         if (facet == newfacet)
00854           break;
00855       }
00856       if (count != hashcount) {
00857         fprintf (qh ferr, "qh_matchnewfacets: after adding facet %d, hashcount %d != count %d\n",
00858                  newfacet->id, hashcount, count);
00859         qh_errexit (qh_ERRqhull, newfacet, NULL);
00860       }
00861     }
00862 #endif  /* end of trap code */
00863   }
00864   if (hashcount) {
00865     FORALLnew_facets {
00866       if (newfacet->dupridge) {
00867         FOREACHneighbor_i_(newfacet) {
00868           if (neighbor == qh_DUPLICATEridge) {
00869             qh_matchduplicates (newfacet, neighbor_i, hashsize, &hashcount);
00870                     /* this may report MERGEfacet */
00871           }
00872         }
00873       }
00874     }
00875   }
00876   if (hashcount) {
00877     fprintf (qh ferr, "qhull internal error (qh_matchnewfacets): %d neighbors did not match up\n",
00878         hashcount);
00879     qh_printhashtable (qh ferr);
00880     qh_errexit (qh_ERRqhull, NULL, NULL);
00881   }
00882 #ifndef qh_NOtrace
00883   if (qh IStracing >= 2) {
00884     FOREACHfacet_i_(qh hash_table) {
00885       if (!facet)
00886         numfree++;
00887     }
00888     fprintf (qh ferr, "qh_matchnewfacets: %d new facets, %d unused hash entries .  hashsize %d\n",
00889              numnew, numfree, qh_setsize (qh hash_table));
00890   }
00891 #endif /* !qh_NOtrace */
00892   qh_setfree (&qh hash_table);
00893   if (qh PREmerge || qh MERGEexact) {
00894     if (qh IStracing >= 4)
00895       qh_printfacetlist (qh newfacet_list, NULL, qh_ALL);
00896     FORALLnew_facets {
00897       if (newfacet->normal)
00898         qh_checkflipped (newfacet, NULL, qh_ALL);
00899     }
00900   }else if (qh FORCEoutput)
00901     qh_checkflipped_all (qh newfacet_list);  /* prints warnings for flipped */
00902 } /* matchnewfacets */
00903 
00904     
00905 /*-<a                             href="qh-c.htm#poly"
00906   >-------------------------------</a><a name="matchvertices">-</a>
00907   
00908   qh_matchvertices( firstindex, verticesA, skipA, verticesB, skipB, same )
00909     tests whether vertices match with a single skip
00910     starts match at firstindex since all new facets have a common vertex
00911 
00912   returns:
00913     true if matched vertices
00914     skip index for each set
00915     sets same iff vertices have the same orientation
00916 
00917   notes:
00918     assumes skipA is in A and both sets are the same size
00919 
00920   design:
00921     set up pointers
00922     scan both sets checking for a match
00923     test orientation
00924 */
00925 boolT qh_matchvertices (int firstindex, setT *verticesA, int skipA, 
00926        setT *verticesB, int *skipB, boolT *same) {
00927   vertexT **elemAp, **elemBp, **skipBp=NULL, **skipAp;
00928 
00929   elemAp= SETelemaddr_(verticesA, firstindex, vertexT);
00930   elemBp= SETelemaddr_(verticesB, firstindex, vertexT);
00931   skipAp= SETelemaddr_(verticesA, skipA, vertexT);
00932   do if (elemAp != skipAp) {
00933     while (*elemAp != *elemBp++) {
00934       if (skipBp)
00935         return False;
00936       skipBp= elemBp;  /* one extra like FOREACH */
00937     }
00938   }while(*(++elemAp));
00939   if (!skipBp)
00940     skipBp= ++elemBp;
00941   *skipB= SETindex_(verticesB, skipB);
00942   *same= !(((ptr_intT)skipA & 0x1) ^ ((ptr_intT)*skipB & 0x1));
00943   trace4((qh ferr, "qh_matchvertices: matched by skip %d (v%d) and skip %d (v%d) same? %d\n",
00944           skipA, (*skipAp)->id, *skipB, (*(skipBp-1))->id, *same));
00945   return (True);
00946 } /* matchvertices */
00947 
00948 /*-<a                             href="qh-c.htm#poly"
00949   >-------------------------------</a><a name="newfacet">-</a>
00950   
00951   qh_newfacet()
00952     return a new facet 
00953 
00954   returns:
00955     all fields initialized or cleared   (NULL)
00956     preallocates neighbors set
00957 */
00958 facetT *qh_newfacet(void) {
00959   facetT *facet;
00960   void **freelistp; /* used !qh_NOmem */
00961   
00962   qh_memalloc_(sizeof(facetT), freelistp, facet, facetT);
00963   memset ((char *)facet, 0, sizeof(facetT));
00964   if (qh facet_id == qh tracefacet_id)
00965     qh tracefacet= facet;
00966   facet->id= qh facet_id++;
00967   facet->neighbors= qh_setnew(qh hull_dim);
00968 #if !qh_COMPUTEfurthest
00969   facet->furthestdist= 0.0;
00970 #endif
00971 #if qh_MAXoutside
00972   if (qh FORCEoutput && qh APPROXhull)
00973     facet->maxoutside= qh MINoutside;
00974   else
00975     facet->maxoutside= qh DISTround;
00976 #endif
00977   facet->simplicial= True;
00978   facet->good= True;
00979   facet->newfacet= True;
00980   trace4((qh ferr, "qh_newfacet: created facet f%d\n", facet->id));
00981   return (facet);
00982 } /* newfacet */
00983 
00984 
00985 /*-<a                             href="qh-c.htm#poly"
00986   >-------------------------------</a><a name="newridge">-</a>
00987   
00988   qh_newridge()
00989     return a new ridge
00990 */
00991 ridgeT *qh_newridge(void) {
00992   ridgeT *ridge;
00993   void **freelistp;   /* used !qh_NOmem */
00994 
00995   qh_memalloc_(sizeof(ridgeT), freelistp, ridge, ridgeT);
00996   memset ((char *)ridge, 0, sizeof(ridgeT));
00997   zinc_(Ztotridges);
00998   if (qh ridge_id == 0xFFFFFF) {
00999     fprintf(qh ferr, "\
01000 qhull warning: more than %d ridges.  Id field overflows and two ridges\n\
01001 may have the same identifier.  Otherwise output ok.\n", 0xFFFFFF);
01002   }
01003   ridge->id= qh ridge_id++;     
01004   trace4((qh ferr, "qh_newridge: created ridge r%d\n", ridge->id));
01005   return (ridge);
01006 } /* newridge */
01007 
01008 
01009 /*-<a                             href="qh-c.htm#poly"
01010   >-------------------------------</a><a name="pointid">-</a>
01011   
01012   qh_pointid(  )
01013     return id for a point, 
01014     returns -3 if null, -2 if interior, or -1 if not known
01015 
01016   alternative code:
01017     unsigned long id;
01018     id= ((unsigned long)point - (unsigned long)qh.first_point)/qh.normal_size;
01019 
01020   notes:
01021     if point not in point array
01022       the code does a comparison of unrelated pointers.
01023 */
01024 int qh_pointid (pointT *point) {
01025   long offset, id;
01026 
01027   if (!point)
01028     id= -3;
01029   else if (point == qh interior_point)
01030     id= -2;
01031   else if (point >= qh first_point
01032   && point < qh first_point + qh num_points * qh hull_dim) {
01033     offset= point - qh first_point;
01034     id= offset / qh hull_dim;
01035   }else if ((id= qh_setindex (qh other_points, point)) != -1)
01036     id += qh num_points;
01037   else
01038     id= -1;
01039   return (int) id;
01040 } /* pointid */
01041   
01042 /*-<a                             href="qh-c.htm#poly"
01043   >-------------------------------</a><a name="removefacet">-</a>
01044   
01045   qh_removefacet( facet )
01046     unlinks facet from qh.facet_list,
01047 
01048   returns:
01049     updates qh.facet_list .newfacet_list .facet_next visible_list
01050     decrements qh.num_facets
01051 */
01052 void qh_removefacet(facetT *facet) {
01053   facetT *next= facet->next, *previous= facet->previous;
01054   
01055   if (facet == qh newfacet_list)
01056     qh newfacet_list= next;
01057   if (facet == qh facet_next)
01058     qh facet_next= next;
01059   if (facet == qh visible_list)
01060     qh visible_list= next; 
01061   if (previous) {
01062     previous->next= next;
01063     next->previous= previous;
01064   }else {  /* 1st facet in qh facet_list */
01065     qh facet_list= next;
01066     qh facet_list->previous= NULL;
01067   }
01068   qh num_facets--;
01069   trace4((qh ferr, "qh_removefacet: remove f%d from facet_list\n", facet->id));
01070 } /* removefacet */
01071 
01072 
01073 /*-<a                             href="qh-c.htm#poly"
01074   >-------------------------------</a><a name="removevertex">-</a>
01075   
01076   qh_removevertex( vertex )
01077     unlinks vertex from qh.vertex_list,
01078 
01079   returns:
01080     updates qh.vertex_list .newvertex_list 
01081     decrements qh.num_vertices
01082 */
01083 void qh_removevertex(vertexT *vertex) {
01084   vertexT *next= vertex->next, *previous= vertex->previous;
01085   
01086   if (vertex == qh newvertex_list)
01087     qh newvertex_list= next;
01088   if (previous) {
01089     previous->next= next;
01090     next->previous= previous;
01091   }else {  /* 1st vertex in qh vertex_list */
01092     qh vertex_list= vertex->next;
01093     qh vertex_list->previous= NULL;
01094   }
01095   qh num_vertices--;
01096   trace4((qh ferr, "qh_removevertex: remove v%d from vertex_list\n", vertex->id));
01097 } /* removevertex */
01098 
01099 
01100 /*-<a                             href="qh-c.htm#poly"
01101   >-------------------------------</a><a name="updatevertices">-</a>
01102   
01103   qh_updatevertices()
01104     update vertex neighbors and delete interior vertices
01105 
01106   returns:
01107     if qh.VERTEXneighbors
01108       updates neighbors for each vertex
01109     interior vertices added to qh.del_vertices for later partitioning
01110 
01111   design:
01112     if qh.VERTEXneighbors
01113       deletes references to visible facets from vertex neighbors
01114       appends new facets to the neighbor list for each vertex
01115       checks all vertices of visible facets
01116         removes visible facets from neighbor lists
01117         marks unused vertices for deletion
01118 */
01119 void qh_updatevertices (void) {
01120   facetT *newfacet= NULL, *neighbor, **neighborp, *visible;
01121   vertexT *vertex, **vertexp;
01122 
01123   trace3((qh ferr, "qh_updatevertices: delete interior vertices and update vertex->neighbors\n"));
01124   if (qh VERTEXneighbors) {
01125     FORALLvertex_(qh newvertex_list) {
01126       FOREACHneighbor_(vertex) {
01127         if (neighbor->visible) 
01128           SETref_(neighbor)= NULL;
01129       }
01130       qh_setcompact (vertex->neighbors);
01131     }
01132     FORALLnew_facets {
01133       FOREACHvertex_(newfacet->vertices)
01134         qh_setappend (&vertex->neighbors, newfacet);
01135     }
01136     FORALLvisible_facets {
01137       FOREACHvertex_(visible->vertices) {
01138         if (!vertex->newlist && !vertex->deleted) {
01139           FOREACHneighbor_(vertex) { /* this can happen under merging */
01140             if (!neighbor->visible)
01141               break;
01142           }
01143           if (neighbor)
01144             qh_setdel (vertex->neighbors, visible);
01145           else {
01146             vertex->deleted= True;
01147             qh_setappend (&qh del_vertices, vertex);
01148             trace2((qh ferr, "qh_updatevertices: delete vertex p%d (v%d) in f%d\n",
01149                   qh_pointid(vertex->point), vertex->id, visible->id));
01150           }
01151         }
01152       }
01153     }
01154   }else {  /* !VERTEXneighbors */
01155     FORALLvisible_facets {
01156       FOREACHvertex_(visible->vertices) {
01157         if (!vertex->newlist && !vertex->deleted) {
01158           vertex->deleted= True;
01159           qh_setappend (&qh del_vertices, vertex);
01160           trace2((qh ferr, "qh_updatevertices: delete vertex p%d (v%d) in f%d\n",
01161                   qh_pointid(vertex->point), vertex->id, visible->id));
01162         }
01163       }
01164     }
01165   }
01166 } /* updatevertices */
01167 
01168 
01169
/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/qhull/poly.c
