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

00001 /*<html><pre>  -<a                             href="qh-c.htm"
00002   >-------------------------------</a><a name="TOP">-</a>
00003 
00004    geom.c 
00005    geometric routines of qhull
00006 
00007    see qh-c.htm and geom.h
00008 
00009    copyright (c) 1993-1998 The Geometry Center        
00010 
00011    infrequent code goes into geom2.c
00012 */
00013    
00014 #include "qhull_a.h"
00015 
00016 #ifdef USE_DMALLOC
00017 #include "dmalloc.h"
00018 #endif
00019    
00020 /*-<a                             href="qh-c.htm#geom"
00021   >-------------------------------</a><a name="backnormal">-</a>
00022   
00023   qh_backnormal( rows, numrow, numcol, sign, normal, nearzero )
00024     given an upper-triangular rows array and a sign,
00025     solve for normal equation x using back substitution over rows U
00026 
00027   returns:
00028      normal= x
00029       
00030      if will not be able to divzero() when normalized (qh.MINdenom_2 and qh.MINdenom_1_2),
00031        if fails on last row
00032          this means that the hyperplane intersects [0,..,1]
00033          sets last coordinate of normal to sign
00034        otherwise
00035          sets tail of normal to [...,sign,0,...], i.e., solves for b= [0...0]
00036          sets nearzero
00037 
00038   notes:
00039      assumes numrow == numcol-1
00040 
00041      see Golub & van Loan 4.4-9 for back substitution
00042 
00043      solves Ux=b where Ax=b and PA=LU
00044      b= [0,...,0,sign or 0]  (sign is either -1 or +1)
00045      last row of A= [0,...,0,1]
00046 
00047      1) Ly=Pb == y=b since P only permutes the 0's of   b
00048      
00049   design:
00050     for each row from end
00051       perform back substitution
00052       if near zero
00053         use qh_divzero for division
00054         if zero divide and not last row
00055           set tail of normal to 0
00056 */
00057 void qh_backnormal (realT **rows, int numrow, int numcol, boolT sign,
00058         coordT *normal, boolT *nearzero) {
00059   int i, j;
00060   coordT *normalp, *normal_tail, *ai, *ak;
00061   realT diagonal;
00062   boolT waszero;
00063   int zerocol= -1;
00064   
00065   normalp= normal + numcol - 1;
00066   *normalp--= (sign ? -1.0 : 1.0);
00067   for(i= numrow; i--; ) {
00068     *normalp= 0.0;
00069     ai= rows[i] + i + 1;
00070     ak= normalp+1;
00071     for(j= i+1; j < numcol; j++)
00072       *normalp -= *ai++ * *ak++;
00073     diagonal= (rows[i])[i];
00074     if (fabs_(diagonal) > qh MINdenom_2)
00075       *(normalp--) /= diagonal;
00076     else {
00077       waszero= False;
00078       *normalp= qh_divzero (*normalp, diagonal, qh MINdenom_1_2, &waszero);
00079       if (waszero) {
00080         zerocol= i;
00081         *(normalp--)= (sign ? -1.0 : 1.0);
00082         for (normal_tail= normalp+2; normal_tail < normal + numcol; normal_tail++)
00083           *normal_tail= 0.0;
00084       }else
00085         normalp--;
00086     }
00087   }
00088   if (zerocol != -1) {
00089     zzinc_(Zback0);
00090     *nearzero= True;
00091     trace4((qh ferr, "qh_backnormal: zero diagonal at column %d.\n", i));
00092     qh_precision ("zero diagonal on back substitution");
00093   }
00094 } /* backnormal */
00095 
00096 /*-<a                             href="qh-c.htm#geom"
00097   >-------------------------------</a><a name="distplane">-</a>
00098   
00099   qh_distplane( point, facet, dist )
00100     return distance from point to facet
00101 
00102   returns:
00103     dist
00104     if qh.RANDOMdist, joggles result
00105   
00106   notes:  
00107     dist > 0 if point is above facet (i.e., outside)
00108     does not error (for sortfacets)
00109     
00110   see:
00111     qh_distnorm in geom2.c
00112 */
00113 void qh_distplane (pointT *point, facetT *facet, realT *dist) {
00114   coordT *normal= facet->normal, *coordp, randr;
00115   int k;
00116   
00117   switch(qh hull_dim){
00118   case 2:
00119     *dist= facet->offset + point[0] * normal[0] + point[1] * normal[1];
00120     break;
00121   case 3:
00122     *dist= facet->offset + point[0] * normal[0] + point[1] * normal[1] + point[2] * normal[2];
00123     break;
00124   case 4:
00125     *dist= facet->offset+point[0]*normal[0]+point[1]*normal[1]+point[2]*normal[2]+point[3]*normal[3];
00126     break;
00127   case 5:
00128     *dist= facet->offset+point[0]*normal[0]+point[1]*normal[1]+point[2]*normal[2]+point[3]*normal[3]+point[4]*normal[4];
00129     break;
00130   case 6:
00131     *dist= facet->offset+point[0]*normal[0]+point[1]*normal[1]+point[2]*normal[2]+point[3]*normal[3]+point[4]*normal[4]+point[5]*normal[5];
00132     break;
00133   case 7:  
00134     *dist= facet->offset+point[0]*normal[0]+point[1]*normal[1]+point[2]*normal[2]+point[3]*normal[3]+point[4]*normal[4]+point[5]*normal[5]+point[6]*normal[6];
00135     break;
00136   case 8:
00137     *dist= facet->offset+point[0]*normal[0]+point[1]*normal[1]+point[2]*normal[2]+point[3]*normal[3]+point[4]*normal[4]+point[5]*normal[5]+point[6]*normal[6]+point[7]*normal[7];
00138     break;
00139   default:
00140     *dist= facet->offset;
00141     coordp= point;
00142     for (k= qh hull_dim; k--; )
00143       *dist += *coordp++ * *normal++;
00144     break;
00145   }
00146   zinc_(Zdistplane);
00147   if (!qh RANDOMdist && qh IStracing < 4)
00148     return;
00149   if (qh RANDOMdist) {
00150     randr= qh_RANDOMint;
00151     *dist += (2.0 * randr / qh_RANDOMmax - 1.0) *
00152       qh RANDOMfactor * qh MAXabs_coord;
00153   }
00154   if (qh IStracing >= 4) {
00155     fprintf (qh ferr, "qh_distplane: ");
00156     fprintf (qh ferr, qh_REAL_1, *dist);
00157     fprintf (qh ferr, "from p%d to f%d\n", qh_pointid(point), facet->id);
00158   }
00159   return;
00160 } /* distplane */
00161 
00162 
00163 /*-<a                             href="qh-c.htm#geom"
00164   >-------------------------------</a><a name="findbest">-</a>
00165   
00166   qh_findbest( point, startfacet, bestoutside, newfacts, dist, isoutside, numpart )
00167     find facet that is furthest below a point 
00168     searches neighbors of coplanar and most flipped facets
00169     for upperDelaunay facets
00170       searches if coplanar (within searchdist)
00171       returns facet only if !noupper and clearly above
00172 
00173   input:
00174     starts search at 'startfacet' (can not be flipped)
00175     if !bestoutside, stops at qh.MINoutside (DISTround if precise)
00176 
00177   returns:
00178     best facet
00179     dist is distance to facet
00180     isoutside is true if point is outside of facet
00181     numpart counts the number of distance tests
00182 
00183   see also:
00184     qh_findbestnew()
00185     
00186   notes:
00187     uses qh.visit_id, qh.searchset
00188     caller traces the results
00189     after qh_distplane, this and qh_partitionpoint are the most expensive in 3-d
00190       avoid calls to distplane, function calls and real number operations.
00191 
00192   when called by qh_partitionvisible():
00193     indicated by newfacets and isoutside defined
00194     qh.newfacet_list is list of simplicial, new facets
00195     qh_findbestnew set if qh_findbestsharp returns True (to use qh_findbestnew)
00196     qh.bestfacet_notsharp set if qh_findbestsharp returns False
00197     searches horizon of best facet unless "exact" and !bestoutside
00198     searchdist is 2 * DISTround
00199 
00200   when called by qh_check_maxout()
00201     indicated by bestoutside and !newfacets and isoutside == NULL
00202     startfacet must be closest to the point
00203     updates facet->maxoutside for good, visited facets
00204     may return NULL
00205 
00206     searchdist is qh.max_outside + 2 * DISTround
00207       + max( MINvisible('Vn'), MAXcoplanar('Un'));
00208     This setting is a guess.  It must be at least max_outside + 2*DISTround 
00209     because a facet may have a geometric neighbor across a vertex
00210 
00211   when called by findfacet() and check_bestdist()
00212     indicated by !newfacets and isoutside defined
00213     searchdist is same as qh_check_maxout()
00214     returns best facet in neighborhood of given facet
00215       this is best facet overall if dist > -   qh.MAXcoplanar 
00216         or hull has at least a "spherical" curvature
00217 
00218   design:
00219     initialize and test for early exit
00220     repeat while there are facets to search (searchset)
00221       for each neighbor of facet
00222         exit if outside facet found
00223         restart searchset if neighbor is much better
00224         append flipped and nearby facets to searchset
00225     if point is inside and partitioning
00226       test for new facets with a "sharp" intersection
00227       if so, future calls go to qh_findbestsharp
00228     if testhorizon
00229       also test facet's horizon facet
00230 */
00231 facetT *qh_findbest (pointT *point, facetT *startfacet, 
00232                      boolT bestoutside, boolT newfacets, boolT noupper,
00233                      realT *dist, boolT *isoutside, int *numpart) {
00234   realT bestdist= -REALmax/2 /* avoid underflow */, searchdist;
00235   realT cutoff, mincutoff;  /* skip facets that are too far from point */
00236   facetT *facet, *neighbor, **neighborp, *bestfacet= NULL;
00237   int oldtrace= qh IStracing;
00238   int searchsize= 0; /* non-zero if searchset defined */
00239   boolT newbest;
00240   boolT ischeckmax= bestoutside && !newfacets && !isoutside;
00241   boolT ispartition= newfacets && isoutside;
00242   boolT isfindfacet= !newfacets && isoutside;
00243   boolT testhorizon = ispartition && (bestoutside || qh APPROXhull || qh MERGING);
00244 
00245   if (!ischeckmax && !ispartition && !isfindfacet) {
00246     fprintf (qh ferr, "qhull internal error (qh_findbest): unknown combination of arguments\n");
00247     qh_errexit (qh_ERRqhull, startfacet, NULL);
00248   }
00249   if (qh TRACElevel && qh TRACEpoint >= 0 && qh TRACEpoint == qh_pointid (point)) {
00250     qh IStracing= qh TRACElevel;
00251     fprintf (qh ferr, "qh_findbest: point p%d starting at f%d bestoutside? %d newfacets %d\n",
00252              qh TRACEpoint, startfacet->id, bestoutside, newfacets);
00253     fprintf(qh ferr, "  ischeckmax %d ispartition %d isfindfacet %d testhorizon %d\n", 
00254               ischeckmax, ispartition, isfindfacet, testhorizon);
00255     fprintf (qh ferr, "  Last point added to hull was p%d.", qh furthest_id);
00256     fprintf(qh ferr, "  Last merge was #%d.\n", zzval_(Ztotmerge));
00257   }
00258   if (isoutside)
00259     *isoutside= True;
00260 
00261   if (!startfacet->flipped) {
00262     *numpart= 1;           
00263     qh_distplane (point, startfacet, dist);  /* this code is duplicated below */
00264     if (!startfacet->upperdelaunay || (!noupper && *dist >= qh MINoutside)) {
00265       bestdist= *dist;
00266       bestfacet= startfacet;
00267       if (!bestoutside && *dist >= qh MINoutside) 
00268         goto LABELreturn_best;
00269     }
00270 #if qh_MAXoutside
00271     if (ischeckmax && (!qh ONLYgood || startfacet->good) && *dist > startfacet->maxoutside)
00272       startfacet->maxoutside= *dist;
00273 #endif
00274   }
00275   if (ispartition)
00276     searchdist= 2 * qh DISTround;
00277   else 
00278     searchdist= qh max_outside + 2 * qh DISTround
00279                 + fmax_( qh MINvisible, qh MAXcoplanar);
00280   cutoff= bestdist - searchdist;
00281   mincutoff= 0;
00282   if (ischeckmax) {
00283     mincutoff= -(qh DISTround - fmax_(qh MINvisible, qh MAXcoplanar));
00284     minimize_(cutoff, mincutoff);
00285   }
00286   startfacet->visitid= ++qh visit_id;
00287   facet= startfacet;
00288   do {  /* search neighbors of coplanar, upperdelaunay, and flipped facets */
00289     if (True) {
00290  LABELrestart:  /* directed search whenever improvement > searchdist */
00291       newbest= False;
00292       trace4((qh ferr, "qh_findbest: neighbors of f%d, bestdist %2.2g cutoff %2.2g searchdist %2.2g\n", 
00293                   facet->id, bestdist, cutoff, searchdist));
00294       FOREACHneighbor_(facet) {
00295         if (ispartition && !neighbor->newfacet)
00296           continue;
00297         if (!neighbor->flipped) {
00298           if (neighbor->visitid == qh visit_id)
00299             continue;
00300           neighbor->visitid= qh visit_id;
00301           (*numpart)++;
00302           qh_distplane (point, neighbor, dist);
00303           if (!bestoutside && *dist >= qh MINoutside 
00304           && (!noupper || !facet->upperdelaunay)) {
00305             bestfacet= neighbor;
00306             goto LABELreturn_best;
00307           }
00308 #if qh_MAXoutside
00309           if (ischeckmax) {
00310             if ((!qh ONLYgood || neighbor->good) 
00311             && *dist > neighbor->maxoutside)
00312               neighbor->maxoutside= *dist;
00313             else if (bestfacet && *dist < cutoff)
00314               continue;
00315           }else 
00316 #endif      /* dangling else! */
00317           if (bestfacet && *dist < cutoff)
00318             continue;
00319           if (*dist > bestdist) {
00320             if (!neighbor->upperdelaunay 
00321             || (bestoutside && !noupper && *dist >= qh MINoutside)) {
00322               if (ischeckmax && qh_MAXoutside) {
00323                 bestdist= *dist;
00324                 bestfacet= neighbor;
00325                 cutoff= bestdist - searchdist;
00326                 minimize_(cutoff, mincutoff);
00327               }else if (*dist > bestdist + searchdist) {
00328                 bestdist= *dist;
00329                 bestfacet= neighbor;
00330                 cutoff= bestdist - searchdist;
00331                 searchsize= 0;
00332                 facet= neighbor;
00333                 if (newbest) /* newbest may be coplanar with facet */
00334                   facet->visitid= ++qh visit_id;
00335                 goto LABELrestart; /* repeat with a new facet */
00336               }else {
00337                 bestdist= *dist;
00338                 bestfacet= neighbor;
00339                 cutoff= bestdist - searchdist;
00340               }
00341               newbest= True;
00342             }
00343           }
00344         }
00345         if (!searchsize++) {
00346           SETfirst_(qh searchset) = neighbor;
00347           qh_settruncate (qh searchset, 1);
00348         }else
00349           qh_setappend (&qh searchset, neighbor);
00350       } /* FOREACHneighbor */
00351     } /* while (restart) */
00352   }while
00353     (searchsize && (facet= (facetT*)qh_setdellast (qh searchset)));
00354   if (!ischeckmax) {
00355     if (!bestfacet) {
00356       fprintf (qh ferr, "qh_findbest: point p%d starting at f%d bestoutside? %d newfacets %d\n",
00357                qh TRACEpoint, startfacet->id, bestoutside, newfacets);
00358       fprintf(qh ferr, "\n\
00359 qh_findbest: all neighbors of facet %d are flipped or upper Delaunay.\n\
00360 Please report this error to qhull_bug@geom.umn.edu with the input and all of the output.\n",
00361          startfacet->id);
00362       qh FORCEoutput= True;
00363       qh_errexit (qh_ERRqhull, startfacet, NULL);
00364     }
00365     if (ispartition && !qh findbest_notsharp && bestdist < - qh DISTround) {
00366       if (qh_findbestsharp ( point, &bestfacet, &bestdist, numpart)) 
00367         qh findbestnew= True;
00368       else
00369         qh findbest_notsharp= True;
00370     }
00371     if (testhorizon) {
00372       facet= SETfirstt_(bestfacet->neighbors, facetT);
00373       trace4((qh ferr, "qh_findbest: horizon facet f%d\n", facet->id));
00374       (*numpart)++;
00375       qh_distplane (point, facet, dist);
00376       if (*dist > bestdist 
00377       && (!facet->upperdelaunay || (!noupper && *dist >= qh MINoutside))) {
00378         bestdist= *dist;
00379         bestfacet= facet;
00380       }
00381     }
00382   }
00383   *dist= bestdist;
00384   if (isoutside && bestdist < qh MINoutside)
00385     *isoutside= False;
00386 LABELreturn_best:
00387   qh IStracing= oldtrace;
00388   return bestfacet;
00389 }  /* findbest */
00390 
00391 
00392 /*-<a                             href="qh-c.htm#geom"
00393   >-------------------------------</a><a name="findbestnew">-</a>
00394   
00395   qh_findbestnew( point, startfacet, dist, isoutside, numpart )
00396     find best newfacet for point
00397     searches new facets starting at startfacet
00398 
00399   returns:
00400     dist is distance to facet
00401     isoutside is true if point is outside of facet
00402     numpart is number of distance tests
00403 
00404   notes:
00405     if qh.BESToutside or !isoutside
00406        stops at furthest facet
00407     if qh.MERGING 
00408        stops when distance > qh_DISToutside (max(4*MINoutside, 2*max_outside))
00409     else 
00410        stops when distance > MINoutside (DISTround in precise case)
00411     searches newfacets then searchs neighbors of best facet.
00412     avoids upperdelaunay facet unless none other or (isoutside and outside)
00413 
00414     uses visit_id and seen flags
00415     caller traces the results
00416   
00417   see also:
00418     qh_partitionall() and qh_findbest()
00419 
00420   design:
00421     for each new facet
00422       test distance from point to facet
00423       select best facet
00424     test each horizon facet of best facet
00425     return best facet 
00426 */
00427 facetT *qh_findbestnew (pointT *point, facetT *startfacet,
00428            realT *dist, boolT *isoutside, int *numpart) {
00429   realT bestdist= -REALmax, bestdist2= -REALmax;
00430   facetT *neighbor, **neighborp, *bestfacet= NULL, *newfacet, *facet;
00431   facetT *bestfacet2= NULL;
00432   int oldtrace= qh IStracing, i;
00433   realT distoutside;
00434 
00435   if (!startfacet) {
00436     if (qh MERGING)
00437       fprintf(qh ferr, "qhull precision error (qh_findbestnew): merging has formed and deleted an independent cycle of facets.  Can not continue.\n");
00438     else
00439       fprintf(qh ferr, "qhull internal error (qh_findbestnew): no new facets for point p%d\n",
00440               qh furthest_id);      
00441     qh_errexit (qh_ERRqhull, NULL, NULL);
00442   }
00443   if (qh BESToutside || !isoutside)
00444     distoutside= REALmax;
00445   else if (qh MERGING)
00446     distoutside= qh_DISToutside; /* defined in user.h */
00447   else
00448     distoutside= qh MINoutside;
00449   if (qh TRACElevel && qh TRACEpoint >= 0 && qh TRACEpoint == qh_pointid (point)) {
00450     qh IStracing= qh TRACElevel;
00451     fprintf(qh ferr, "qh_findbestnew: point p%d facet f%d. Stop if dist > %2.2g\n",
00452              qh TRACEpoint, startfacet->id, distoutside);
00453     fprintf(qh ferr, "  Last point added to hull was p%d.", qh furthest_id);
00454     fprintf(qh ferr, "  Last merge was #%d.\n", zzval_(Ztotmerge));
00455   }
00456   if (isoutside)
00457     *isoutside= True;
00458   *numpart= 0;
00459 
00460   /* visit all new facets starting with startfacet */
00461   for (i= 0, facet= startfacet; i < 2; i++, facet= qh newfacet_list) {
00462     FORALLfacet_(facet) {
00463       if (facet == startfacet && i)
00464         break;
00465       qh_distplane (point, facet, dist);
00466       (*numpart)++;
00467       if (facet->upperdelaunay) {
00468         if (*dist > bestdist2) {
00469           bestdist2= *dist;
00470           bestfacet2= facet;
00471           if (*dist >= distoutside) {
00472             bestfacet= facet;
00473             goto LABELreturn_bestnew;
00474           }
00475         }
00476       }else if (*dist > bestdist) {
00477         bestdist= *dist;
00478         bestfacet= facet;
00479         if (*dist >= distoutside) 
00480           goto LABELreturn_bestnew;
00481       }
00482     }
00483   }
00484   newfacet= bestfacet ? bestfacet : bestfacet2;
00485         /* !bestfacet only occurs if 'd' creates incorrect upper-delaunay facets */
00486   FOREACHneighbor_(newfacet) {
00487     if (!neighbor->newfacet) {
00488       qh_distplane (point, neighbor, dist);
00489       (*numpart)++;
00490       if (neighbor->upperdelaunay) {
00491         if (*dist > bestdist2) {
00492           bestdist2= *dist;
00493           bestfacet2= neighbor;
00494         }
00495       }else if (*dist > bestdist) {
00496         bestdist= *dist;
00497         bestfacet= neighbor;
00498       }
00499     }
00500   }
00501   if (!bestfacet  
00502   || (isoutside && bestdist2 >= qh MINoutside && bestdist2 > bestdist)) {
00503     *dist= bestdist2;
00504     bestfacet= bestfacet2;
00505   }else 
00506     *dist= bestdist;
00507   if (isoutside && *dist < qh MINoutside)
00508     *isoutside= False;
00509 LABELreturn_bestnew:
00510   qh IStracing= oldtrace;
00511   return bestfacet;
00512 }  /* findbestnew */
00513 
00514 
00515 /*-<a                             href="qh-c.htm#geom"
00516   >-------------------------------</a><a name="gausselim">-</a>
00517   
00518   qh_gausselim( rows, numrow, numcol, sign )
00519     Gaussian elimination with partial pivoting
00520 
00521   returns:
00522     rows is upper triangular (includes row exchanges)
00523     flips sign for each row exchange
00524     sets nearzero if pivot[k] < qh.NEARzero[k], else clears it
00525 
00526   notes:
00527     if nearzero, the determinant's sign may be incorrect.
00528     assumes numrow <= numcol
00529 
00530   design:
00531     for each row
00532       determine pivot and exchange rows if necessary
00533       test for near zero
00534       perform gaussian elimination step
00535 */
00536 void qh_gausselim(realT **rows, int numrow, int numcol, boolT *sign, boolT *nearzero) {
00537   realT *ai, *ak, *rowp, *pivotrow;
00538   realT n, pivot, pivot_abs= 0.0, temp;
00539   int i, j, k, pivoti, flip=0;
00540   
00541   *nearzero= False;
00542   for(k= 0; k < numrow; k++) {
00543     pivot_abs= fabs_((rows[k])[k]);
00544     pivoti= k;
00545     for(i= k+1; i < numrow; i++) {
00546       if ((temp= fabs_((rows[i])[k])) > pivot_abs) {
00547         pivot_abs= temp;
00548         pivoti= i;
00549       }
00550     }
00551     if (pivoti != k) {
00552       rowp= rows[pivoti]; 
00553       rows[pivoti]= rows[k]; 
00554       rows[k]= rowp; 
00555       *sign ^= 1;
00556       flip ^= 1;
00557     }
00558     if (pivot_abs <= qh NEARzero[k]) {
00559       *nearzero= True;
00560       if (pivot_abs == 0.0) {   /* remainder of column == 0 */
00561         if (qh IStracing >= 4) {
00562           fprintf (qh ferr, "qh_gausselim: 0 pivot at column %d. (%2.2g < %2.2g)\n", k, pivot_abs, qh DISTround);
00563           qh_printmatrix (qh ferr, "Matrix:", rows, numrow, numcol);
00564         }
00565         zzinc_(Zgauss0);
00566         qh_precision ("zero pivot for Gaussian elimination");
00567         goto LABELnextcol;
00568       }
00569     }
00570     pivotrow= rows[k] + k;
00571     pivot= *pivotrow++;  /* signed value of pivot, and remainder of row */
00572     for(i= k+1; i < numrow; i++) {
00573       ai= rows[i] + k;
00574       ak= pivotrow;
00575       n= (*ai++)/pivot;   /* divzero() not needed since |pivot| >= |*ai| */
00576       for(j= numcol - (k+1); j--; )
00577         *ai++ -= n * *ak++;
00578     }
00579   LABELnextcol:
00580     ;
00581   }
00582   wmin_(Wmindenom, pivot_abs);  /* last pivot element */
00583   if (qh IStracing >= 5)
00584     qh_printmatrix (qh ferr, "qh_gausselem: result", rows, numrow, numcol);
00585 } /* gausselim */
00586 
00587 
00588 /*-<a                             href="qh-c.htm#geom"
00589   >-------------------------------</a><a name="getangle">-</a>
00590   
00591   qh_getangle( vect1, vect2 )
00592     returns the dot product of two, DIM3 vectors
00593     if qh.RANDOMdist, joggles result
00594 
00595   notes:
00596     the angle may be > 1.0 or < -1.0 because of roundoff errors
00597 
00598 */
00599 realT qh_getangle(pointT *vect1, pointT *vect2) {
00600   realT angle= 0, randr;
00601   int k;
00602 
00603   for(k= qh hull_dim; k--; )
00604     angle += *vect1++ * *vect2++;
00605   if (qh RANDOMdist) {
00606     randr= qh_RANDOMint;
00607     angle += (2.0 * randr / qh_RANDOMmax - 1.0) *
00608       qh RANDOMfactor;
00609   }
00610   trace4((qh ferr, "qh_getangle: %2.2g\n", angle));
00611   return(angle);
00612 } /* getangle */
00613 
00614 
00615 /*-<a                             href="qh-c.htm#geom"
00616   >-------------------------------</a><a name="getcenter">-</a>
00617   
00618   qh_getcenter( vertices )
00619     returns arithmetic center of a set of vertices as a new point
00620 
00621   notes:
00622     allocates point array for center
00623 */
00624 pointT *qh_getcenter(setT *vertices) {
00625   int k;
00626   pointT *center, *coord;
00627   vertexT *vertex, **vertexp;
00628   int count= qh_setsize(vertices);
00629 
00630   if (count < 2) {
00631     fprintf (qh ferr, "qhull internal error (qh_getcenter): not defined for %d points\n", count);
00632     qh_errexit (qh_ERRqhull, NULL, NULL);
00633   }
00634   center= (pointT *)qh_memalloc(qh normal_size);
00635   for (k=0; k < qh hull_dim; k++) {
00636     coord= center+k;
00637     *coord= 0.0;
00638     FOREACHvertex_(vertices)
00639       *coord += vertex->point[k];
00640     *coord /= count;
00641   }
00642   return(center);
00643 } /* getcenter */
00644 
00645 
00646 /*-<a                             href="qh-c.htm#geom"
00647   >-------------------------------</a><a name="getcentrum">-</a>
00648   
00649   qh_getcentrum( facet )
00650     returns the centrum for a facet as a new point
00651 
00652   notes:
00653     allocates the centrum
00654 */
00655 pointT *qh_getcentrum(facetT *facet) {
00656   realT dist;
00657   pointT *centrum, *point;
00658 
00659   point= qh_getcenter(facet->vertices);
00660   zzinc_(Zcentrumtests);
00661   qh_distplane (point, facet, &dist);
00662   centrum= qh_projectpoint(point, facet, dist);
00663   qh_memfree(point, qh normal_size);
00664   trace4((qh ferr, "qh_getcentrum: for f%d, %d vertices dist= %2.2g\n",
00665           facet->id, qh_setsize(facet->vertices), dist));
00666   return centrum;
00667 } /* getcentrum */
00668 
00669 
00670 /*-<a                             href="qh-c.htm#geom"
00671   >-------------------------------</a><a name="getdistance">-</a>
00672   
00673   qh_getdistance( facet, neighbor, mindist, maxdist )
00674     returns the maxdist and mindist distance of any vertex from neighbor
00675 
00676   returns:
00677     the max absolute value
00678 
00679   design:
00680     for each vertex of facet that is not in neighbor
00681       test the distance from vertex to neighbor
00682 */
00683 realT qh_getdistance(facetT *facet, facetT *neighbor, realT *mindist, realT *maxdist) {
00684   vertexT *vertex, **vertexp;
00685   realT dist, maxd, mind;
00686   
00687   FOREACHvertex_(facet->vertices)
00688     vertex->seen= False;
00689   FOREACHvertex_(neighbor->vertices)
00690     vertex->seen= True;
00691   mind= 0.0;
00692   maxd= 0.0;
00693   FOREACHvertex_(facet->vertices) {
00694     if (!vertex->seen) {
00695       zzinc_(Zbestdist);
00696       qh_distplane(vertex->point, neighbor, &dist);
00697       if (dist < mind)
00698         mind= dist;
00699       else if (dist > maxd)
00700         maxd= dist;
00701     }
00702   }
00703   *mindist= mind;
00704   *maxdist= maxd;
00705   mind= -mind;
00706   if (maxd > mind)
00707     return maxd;
00708   else
00709     return mind;
00710 } /* getdistance */
00711 
00712 
00713 /*-<a                             href="qh-c.htm#geom"
00714   >-------------------------------</a><a name="normalize">-</a>
00715 
00716   qh_normalize( normal, dim, toporient )
00717     normalize a vector and report if too small
00718     does not use min norm
00719   
00720   see:
00721     qh_normalize2
00722 */
00723 void qh_normalize (coordT *normal, int dim, boolT toporient) {
00724   qh_normalize2( normal, dim, toporient, NULL, NULL);
00725 } /* normalize */
00726 
00727 /*-<a                             href="qh-c.htm#geom"
00728   >-------------------------------</a><a name="normalize2">-</a>
00729   
00730   qh_normalize2( normal, dim, toporient, minnorm, ismin )
00731     normalize a vector and report if too small
00732     qh.MINdenom/MINdenom1 are the upper limits for divide overflow
00733 
00734   returns:
00735     normalized vector
00736     flips sign if !toporient
00737     if minnorm non-NULL, 
00738       sets ismin if normal < minnorm
00739 
00740   notes:
00741     if zero norm
00742        sets all elements to sqrt(1.0/dim)
00743     if divide by zero (divzero ())
00744        sets largest element to   +/-1
00745        bumps Znearlysingular
00746       
00747   design:
00748     computes norm
00749     test for minnorm
00750     if not near zero
00751       normalizes normal
00752     else if zero norm
00753       sets normal to standard value
00754     else
00755       uses qh_divzero to normalize
00756       if nearzero
00757         sets norm to direction of maximum value
00758 */
00759 void qh_normalize2 (coordT *normal, int dim, boolT toporient, 
00760             realT *minnorm, boolT *ismin) {
00761   int k;
00762   realT *colp, *maxp, norm= 0, temp, *norm1, *norm2, *norm3;
00763   boolT zerodiv;
00764 
00765   norm1= normal+1;
00766   norm2= normal+2;
00767   norm3= normal+3;
00768   if (dim == 2)
00769     norm= sqrt((*normal)*(*normal) + (*norm1)*(*norm1));
00770   else if (dim == 3)
00771     norm= sqrt((*normal)*(*normal) + (*norm1)*(*norm1) + (*norm2)*(*norm2));
00772   else if (dim == 4) {
00773     norm= sqrt((*normal)*(*normal) + (*norm1)*(*norm1) + (*norm2)*(*norm2) 
00774                + (*norm3)*(*norm3));
00775   }else if (dim > 4) {
00776     norm= (*normal)*(*normal) + (*norm1)*(*norm1) + (*norm2)*(*norm2) 
00777                + (*norm3)*(*norm3);
00778     for (k= dim-4, colp= normal+4; k--; colp++)
00779       norm += (*colp) * (*colp);
00780     norm= sqrt(norm);
00781   }
00782   if (minnorm) {
00783     if (norm < *minnorm) 
00784       *ismin= True;
00785     else
00786       *ismin= False;
00787   }
00788   wmin_(Wmindenom, norm);
00789   if (norm > qh MINdenom) {
00790     if (!toporient)
00791       norm= -norm;
00792     *normal /= norm;
00793     *norm1 /= norm;
00794     if (dim == 2)
00795       ; /* all done */
00796     else if (dim == 3)
00797       *norm2 /= norm;
00798     else if (dim == 4) {
00799       *norm2 /= norm;
00800       *norm3 /= norm;
00801     }else if (dim >4) {
00802       *norm2 /= norm;
00803       *norm3 /= norm;
00804       for (k= dim-4, colp= normal+4; k--; )
00805         *colp++ /= norm;
00806     }
00807   }else if (norm == 0.0) {
00808     temp= sqrt (1.0/dim);
00809     for (k= dim, colp= normal; k--; )
00810       *colp++ = temp;
00811   }else {
00812     if (!toporient)
00813       norm= -norm;
00814     for (k= dim, colp= normal; k--; colp++) { /* k used below */
00815       temp= qh_divzero (*colp, norm, qh MINdenom_1, &zerodiv);
00816       if (!zerodiv)
00817         *colp= temp;
00818       else {
00819         maxp= qh_maxabsval(normal, dim);
00820         temp= ((*maxp * norm >= 0.0) ? 1.0 : -1.0);
00821         for (k= dim, colp= normal; k--; colp++)
00822           *colp= 0.0;
00823         *maxp= temp;
00824         zzinc_(Znearlysingular);
00825         trace0((qh ferr, "qh_normalize: norm=%2.2g too small during p%d\n", 
00826                norm, qh furthest_id));
00827         return;
00828       }
00829     }
00830   }
00831 } /* normalize */
00832 
00833 
00834 /*-<a                             href="qh-c.htm#geom"
00835   >-------------------------------</a><a name="projectpoint">-</a>
00836   
00837   qh_projectpoint( point, facet, dist )
00838     project point onto a facet by dist
00839 
00840   returns:
00841     returns a new point
00842     
00843   notes:
00844     if dist= distplane(point,facet)
00845       this projects point to hyperplane
00846     assumes qh_memfree_() is valid for normal_size
00847 */
00848 pointT *qh_projectpoint(pointT *point, facetT *facet, realT dist) {
00849   pointT *newpoint, *np, *normal;
00850   int normsize= qh normal_size,k;
00851   void **freelistp; /* used !qh_NOmem */
00852   
00853   qh_memalloc_(normsize, freelistp, newpoint, pointT);
00854   np= newpoint;
00855   normal= facet->normal;
00856   for(k= qh hull_dim; k--; )
00857     *(np++)= *point++ - dist * *normal++;
00858   return(newpoint);
00859 } /* projectpoint */
00860 
00861   
00862 /*-<a                             href="qh-c.htm#geom"
00863   >-------------------------------</a><a name="setfacetplane">-</a>
00864   
00865   qh_setfacetplane( facet )
00866     sets the hyperplane for a facet
00867     if qh.RANDOMdist, joggles hyperplane
00868 
00869   notes:
00870     uses global buffers qh.gm_matrix and qh.gm_row
00871     overwrites facet->normal if already defined
00872     updates Wnewvertex if PRINTstatistics
00873     sets facet->upperdelaunay if upper envelope of Delaunay triangulation
00874 
00875   design:
00876     copy vertex coordinates to qh.gm_matrix/gm_row
00877     compute determinate
00878     if nearzero
00879       recompute determinate with gaussian elimination
00880       if nearzero
00881         force outside orientation by testing interior point
00882 */
00883 void qh_setfacetplane(facetT *facet) {
00884   pointT *point;
00885   vertexT *vertex, **vertexp;
00886   int k,i, normsize= qh normal_size, oldtrace= 0;
00887   realT dist;
00888   void **freelistp; /* used !qh_NOmem */
00889   coordT *coord, *gmcoord= qh gm_matrix;
00890   pointT *point0= SETfirstt_(facet->vertices, vertexT)->point;
00891   boolT nearzero= False;
00892 
00893   zzinc_(Zsetplane);
00894   if (!facet->normal)
00895     qh_memalloc_(normsize, freelistp, facet->normal, coordT);
00896   if (facet == qh tracefacet) {
00897     oldtrace= qh IStracing;
00898     qh IStracing= 5;
00899     fprintf (qh ferr, "qh_setfacetplane: facet f%d created.\n", facet->id);
00900     fprintf (qh ferr, "  Last point added to hull was p%d.", qh furthest_id);
00901     if (zzval_(Ztotmerge))
00902       fprintf(qh ferr, "  Last merge was #%d.", zzval_(Ztotmerge));
00903     fprintf (qh ferr, "\n\nCurrent summary is:\n");
00904       qh_printsummary (qh ferr);
00905   }
00906   if (qh hull_dim <= 4) {
00907     i= 0;
00908     if (qh RANDOMdist) {
00909       FOREACHvertex_(facet->vertices) {
00910         qh gm_row[i++]= gmcoord;
00911         coord= vertex->point;
00912         for (k= qh hull_dim; k--; )
00913           *(gmcoord++)= *coord++ * qh_randomfactor();
00914       }   
00915     }else {
00916       FOREACHvertex_(facet->vertices)
00917        qh gm_row[i++]= vertex->point;
00918     }
00919     qh_sethyperplane_det(qh hull_dim, qh gm_row, point0, facet->toporient,
00920                 facet->normal, &facet->offset, &nearzero);
00921   }
00922   if (qh hull_dim > 4 || nearzero) {
00923     i= 0;
00924     FOREACHvertex_(facet->vertices) {
00925       if (vertex->point != point0) {
00926         qh gm_row[i++]= gmcoord;
00927         coord= vertex->point;
00928         point= point0;
00929         for(k= qh hull_dim; k--; )
00930           *(gmcoord++)= *coord++ - *point++;
00931       }
00932     }
00933     qh gm_row[i]= gmcoord;  /* for areasimplex */
00934     if (qh RANDOMdist) {
00935       gmcoord= qh gm_matrix;
00936       for (i= qh hull_dim-1; i--; ) {
00937         for (k= qh hull_dim; k--; )
00938           *(gmcoord++) *= qh_randomfactor();
00939       }
00940     }
00941     qh_sethyperplane_gauss(qh hull_dim, qh gm_row, point0, facet->toporient,
00942                 facet->normal, &facet->offset, &nearzero);
00943     if (nearzero) { 
00944       if (qh_orientoutside (facet)) {
00945         trace0((qh ferr, "qh_setfacetplane: flipped orientation after testing interior_point during p%d\n", qh furthest_id));
00946       /* this is part of using Gaussian Elimination.  For example in 5-d
00947            1 1 1 1 0
00948            1 1 1 1 1
00949            0 0 0 1 0
00950            0 1 0 0 0
00951            1 0 0 0 0
00952            norm= 0.38 0.38 -0.76 0.38 0
00953          has a determinate of 1, but g.e. after subtracting pt. 0 has
00954          0's in the diagonal, even with full pivoting.  It does work
00955          if you subtract pt. 4 instead. */
00956       }
00957     }
00958   }
00959   if (qh DELAUNAY && facet->normal[qh hull_dim -1] > qh ANGLEround)
00960     facet->upperdelaunay= True;
00961   else 
00962     facet->upperdelaunay= False;
00963   if (qh PRINTstatistics || qh IStracing || qh TRACElevel || qh JOGGLEmax < REALmax) {
00964     qh old_randomdist= qh RANDOMdist;
00965     qh RANDOMdist= False;
00966     FOREACHvertex_(facet->vertices) {
00967       if (vertex->point != point0) {
00968         boolT istrace= False;
00969         zinc_(Zdiststat);
00970         qh_distplane(vertex->point, facet, &dist);
00971         dist= fabs_(dist);
00972         zinc_(Znewvertex);
00973         wadd_(Wnewvertex, dist);
00974         if (dist > wwval_(Wnewvertexmax)) {
00975           wwval_(Wnewvertexmax)= dist;
00976           if (dist > qh max_outside) {
00977             qh max_outside= dist;  /* used by qh_maxouter() */
00978             if (dist > qh TRACEdist) 
00979               istrace= True;
00980           }
00981         }else if (-dist > qh TRACEdist)
00982           istrace= True;
00983         if (istrace) {
00984           fprintf (qh ferr, "qh_setfacetplane: ====== vertex p%d (v%d) increases max_outside to %2.2g for new facet f%d last p%d\n",
00985                 qh_pointid(vertex->point), vertex->id, dist, facet->id, qh furthest_id);
00986           qh_errprint ("DISTANT", facet, NULL, NULL, NULL);
00987         }
00988       }
00989     }
00990     qh RANDOMdist= qh old_randomdist;
00991   }
00992   if (qh IStracing >= 3) {
00993     fprintf (qh ferr, "qh_setfacetplane: f%d offset %2.2g normal: ",
00994              facet->id, facet->offset);
00995     for (k=0; k < qh hull_dim; k++)
00996       fprintf (qh ferr, "%2.2g ", facet->normal[k]);
00997     fprintf (qh ferr, "\n");
00998   }
00999   if (facet == qh tracefacet)
01000     qh IStracing= oldtrace;
01001 } /* setfacetplane */
01002 
01003 
01004 /*-<a                             href="qh-c.htm#geom"
01005   >-------------------------------</a><a name="sethyperplane_det">-</a>
01006   
01007   qh_sethyperplane_det( dim, rows, point0, toporient, normal, offset, nearzero )
01008     given dim X dim array indexed by rows[], one row per point, 
01009         toporient (flips all signs),
01010         and point0 (any row)
01011     set normalized hyperplane equation from oriented simplex
01012 
01013   returns:
01014     normal (normalized)
01015     offset (places point0 on the hyperplane)
01016 
01017   notes:
01018     only defined for dim == 2..4
01019     rows[] is not modified
01020     solves det(P-V_0, V_n-V_0, ..., V_1-V_0)=0, i.e. every point is on hyperplane
01021     see Bower & Woodworth, A programmer's geometry, Butterworths 1983.
01022 
01023 */
01024 void qh_sethyperplane_det (int dim, coordT **rows, coordT *point0, 
01025           boolT toporient, coordT *normal, realT *offset, boolT *nearzero) {
01026 
01027   if (dim == 2) {
01028     normal[0]= dY(1,0);
01029     normal[1]= dX(0,1);
01030     qh_normalize2 (normal, dim, toporient, NULL, NULL);
01031     *offset= -(point0[0]*normal[0]+point0[1]*normal[1]);
01032     *nearzero= False;  /* since nearzero norm => incident points */
01033   }else if (dim == 3) {
01034     normal[0]= det2_(dY(2,0), dZ(2,0),
01035                      dY(1,0), dZ(1,0));
01036     normal[1]= det2_(dX(1,0), dZ(1,0),
01037                      dX(2,0), dZ(2,0));
01038     normal[2]= det2_(dX(2,0), dY(2,0),
01039                      dX(1,0), dY(1,0));
01040     qh_normalize2 (normal, dim, toporient, &qh MINnorm, nearzero);
01041     *offset= -(point0[0]*normal[0] + point0[1]*normal[1]
01042                + point0[2]*normal[2]);
01043   }else if (dim == 4) {
01044     normal[0]= - det3_(dY(2,0), dZ(2,0), dW(2,0),
01045                         dY(1,0), dZ(1,0), dW(1,0),
01046                         dY(3,0), dZ(3,0), dW(3,0));
01047     normal[1]=   det3_(dX(2,0), dZ(2,0), dW(2,0),
01048                         dX(1,0), dZ(1,0), dW(1,0),
01049                         dX(3,0), dZ(3,0), dW(3,0));
01050     normal[2]= - det3_(dX(2,0), dY(2,0), dW(2,0),
01051                         dX(1,0), dY(1,0), dW(1,0),
01052                         dX(3,0), dY(3,0), dW(3,0));
01053     normal[3]=   det3_(dX(2,0), dY(2,0), dZ(2,0),
01054                         dX(1,0), dY(1,0), dZ(1,0),
01055                         dX(3,0), dY(3,0), dZ(3,0));
01056     qh_normalize2 (normal, dim, toporient, &qh MINnorm, nearzero);
01057     *offset= -(point0[0]*normal[0] + point0[1]*normal[1]
01058                + point0[2]*normal[2] + point0[3]*normal[3]);
01059   }
01060   if (*nearzero) {
01061     zzinc_(Zminnorm);
01062     trace0((qh ferr, "qh_sethyperplane_det: degenerate norm during p%d.\n", qh furthest_id));
01063         zzinc_(Znearlysingular);
01064   }
01065 } /* sethyperplane_det */
01066 
01067 
01068 /*-<a                             href="qh-c.htm#geom"
01069   >-------------------------------</a><a name="sethyperplane_gauss">-</a>
01070   
01071   qh_sethyperplane_gauss( dim, rows, point0, toporient, normal, offset, nearzero )
01072     given (dim-1) X dim array of rows[i]= V_{i+1} - V_0 (point0)
01073     set normalized hyperplane equation from oriented simplex
01074 
01075   returns:
01076     normal (normalized)
01077     offset (places point0 on the hyperplane)
01078 
01079   notes:
01080     if nearzero
01081       orientation may be incorrect because of incorrect sign flips in gausselim
01082     solves [V_n-V_0,...,V_1-V_0, 0 .. 0 1] * N == [0 .. 0 1] 
01083         or [V_n-V_0,...,V_1-V_0, 0 .. 0 1] * N == [0] 
01084     i.e., N is normal to the hyperplane, and the unnormalized
01085         distance to [0 .. 1] is either 1 or   0
01086 
01087   design:
01088     perform gaussian elimination
01089     flip sign for negative values
01090     perform back substitution 
01091     normalize result
01092     compute offset
01093 */
01094 void qh_sethyperplane_gauss (int dim, coordT **rows, pointT *point0, 
01095                 boolT toporient, coordT *normal, coordT *offset, boolT *nearzero) {
01096   coordT *pointcoord, *normalcoef;
01097   int k;
01098   boolT sign= toporient, nearzero2= False;
01099   
01100   qh_gausselim(rows, dim-1, dim, &sign, nearzero);
01101   for(k= dim-1; k--; ) {
01102     if ((rows[k])[k] < 0)
01103       sign ^= 1;
01104   }
01105   if (*nearzero) {
01106     zzinc_(Znearlysingular);
01107     trace0((qh ferr, "qh_sethyperplane_gauss: nearly singular or axis parallel hyperplane during p%d.\n", qh furthest_id));
01108     qh_backnormal(rows, dim-1, dim, sign, normal, &nearzero2);
01109   }else {
01110     qh_backnormal(rows, dim-1, dim, sign, normal, &nearzero2);
01111     if (nearzero2) {
01112       zzinc_(Znearlysingular);
01113       trace0((qh ferr, "qh_sethyperplane_gauss: singular or axis parallel hyperplane at normalization during p%d.\n", qh furthest_id));
01114     }
01115   }
01116   if (nearzero2)
01117     *nearzero= True;
01118   qh_normalize2(normal, dim, True, NULL, NULL);
01119   pointcoord= point0;
01120   normalcoef= normal;
01121   *offset= -(*pointcoord++ * *normalcoef++);
01122   for(k= dim-1; k--; )
01123     *offset -= *pointcoord++ * *normalcoef++;
01124 } /* sethyperplane_gauss */
01125 
01126
/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/qhull/geom.c
