geom.c

这是一个新的用于图像处理的工具箱

/* geom.c -- geometric routines of qhull
   
   see README and geom.h

   copyright (c) 1993-1995 The Geometry Center        

   infrequent code goes into geom2.c
*/
   
#include "qhull_a.h"
   
/*-------------------------------------------------
-backnormal- solve for normal x using back substitution over rows U
     solves Ux=b where Ax=b and PA=LU
     b= [0,...,0,sign or 0]  (-1 if sign, else +1)
     last row of A= [0,...,0,1]
     assumes numrow == numcol-1
returns:
     normal= x 
     if can't divzero() for later normalization (qh MINdenom_2 and qh MINdenom_1_2),
         sets tail of normal to [...,sign,0,...], i.e., solves for b= [0...]
	 sets nearzero, unless last row (i.e., hyperplane intersects [0,..,1])
notes:
     1) Ly=Pb == y=b since P only permutes the 0's of b
     see Golub & van Loan 4.4-9 for back substitution
*/
void qh_backnormal (realT **rows, int numrow, int numcol, boolT sign,
  	coordT *normal, boolT *nearzero) {
  int i, j;
  coordT *normalp, *normal_tail, *ai, *ak;
  realT diagonal;
  boolT waszero;
  int zerocol=-1;
  
  normalp= normal + numcol - 1;
  *normalp--= (sign ? -1.0 : 1.0);
  for(i= numrow; i--; ) {
    *normalp= 0.0;
    ai= rows[i] + i + 1;
    ak= normalp+1;
    for(j= i+1; j < numcol; j++)
      *normalp -= *ai++ * *ak++;
    diagonal= (rows[i])[i];
    if (fabs_(diagonal) > qh MINdenom_2)
      *(normalp--) /= diagonal;
    else {
      waszero= False;
      *normalp= qh_divzero (*normalp, diagonal, qh MINdenom_1_2, &waszero);
      if (waszero) {
        zerocol= i;
	*(normalp--)= (sign ? -1.0 : 1.0);
	for (normal_tail= normalp+2; normal_tail < normal + numcol; normal_tail++)
	  *normal_tail= 0.0;
      }else
	normalp--;
    }
  }
  if (zerocol != -1) {
    zzinc_(Zback0);
    *nearzero= True;
    trace4((qh ferr, "qh_backnormal: zero diagonal at column %d.\n", i));
  }
} /* backnormal */

/*-------------------------------------------
-distplane- get distance from point to facet
returns:
    positive if point is above facet (i.e., outside)
    can not errexit (for sortfacets)
*/
void qh_distplane (pointT *point, facetT *facet, realT *dist) {
  coordT *normal= facet->normal, *coordp, randr;
  int k;
  
  switch(qh hull_dim){
  case 2:
    *dist= facet->offset + point[0] * normal[0] + point[1] * normal[1];
    break;
  case 3:
    *dist= facet->offset + point[0] * normal[0] + point[1] * normal[1] + point[2] * normal[2];
    break;
  case 4:
    *dist= facet->offset+point[0]*normal[0]+point[1]*normal[1]+point[2]*normal[2]+point[3]*normal[3];
    break;
  case 5:
    *dist= facet->offset+point[0]*normal[0]+point[1]*normal[1]+point[2]*normal[2]+point[3]*normal[3]+point[4]*normal[4];
    break;
  case 6:
    *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];
    break;
  case 7:  
    *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];
    break;
  case 8:
    *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];
    break;
  default:
    *dist= facet->offset;
    coordp= point;
    for (k= qh hull_dim; k--; )
      *dist += *coordp++ * *normal++;
    break;
  }
  zinc_(Zdistplane);
  if (!qh RANDOMdist && qh IStracing < 4)
    return;
  if (qh RANDOMdist) {
    randr= qh_RANDOMint;
    *dist += (2.0 * randr / qh_RANDOMmax - 1.0) *
      qh RANDOMfactor * qh maxmaxcoord;
  }
  if (qh IStracing >= 4) {
    fprintf (qh ferr, "qh_distplane: ");
    fprintf (qh ferr, qh_REAL_1, *dist);
    fprintf (qh ferr, "from p%d to f%d\n", qh_pointid(point), facet->id);
  }
  return;
} /* distplane */


/*-------------------------------------------------
-findbest- find facet that is furthest below a point 
  starts search at 'facet' (can not be flipped)
  if !bestoutside, stops at qh MINoutside (DISTround if precise)
  searches neighbors of coplanar and most flipped facets
  does not search upper envelope of Delaunay triangulations if clearly below
  does not return upperdelaunay facets if not outside or if findfacet()
returns:
  best facet
  distance to facet
  isoutside true if point is outside of facet
  numpart counts the number of distance tests
notes:
  uses qh visit_id, qh searchset
  caller traces the results
  see also qh_findbestnew()
  after qh_distplane, this and qh_partitionpoint are the most expensive in 3-d
    avoid calls to distplane, function calls and real number operations.

for partitionvisible():
  indicated by newfacets and isoutside defined
  qh newfacet_list is list of simplicial, new facets
  qh_findbestnew set if qh_findbestsharp returns True (to use qh_findbestnew)
  qh bestfacet_notsharp set if qh_findbestsharp returns False
  searches horizon of best facet unless "exact" and !bestoutside
  searchdist is 2 * DISTround

for check_maxout()
  indicated by bestoutside and !newfacets and isoutside == NULL
  facet must be closest to the point
  searchdist is qh max_outside + 2 * DISTround
    + max( MINvisible('Vn'), MAXcoplanar('Un'));
    This setting is a guess.  It must be at least max_outside + 2*DISTround 
    because a facet may have a geometric neighbor across a vertex
returns:
  updates facet->maxoutside for good, visited facets

for findfacet() and check_bestdist()
  indicated by !newfacets and isoutside defined
  searchdist is same as check_maxout()
returns:
  best facet in neighborhood of given facet
  this is best facet overall if dist > - qh MAXcoplanar 
    or hull has at least a "spherical" curvature
*/
facetT *qh_findbest (pointT *point, facetT *facet, boolT bestoutside,
	   boolT newfacets, realT *dist, boolT *isoutside, int *numpart) {
  realT bestdist, searchdist;
  facetT *neighbor, **neighborp, *bestfacet;
  int oldtrace= qh IStracing;
  int searchsize= 0; /* non-zero if searchset defined */
  boolT ischeckmax= bestoutside && !newfacets && !isoutside;
  boolT ispartition= newfacets && isoutside;
  boolT isfindfacet= !newfacets && isoutside;
  boolT testhorizon = ispartition && (bestoutside || qh APPROXhull || qh MERGING);

  if (!ischeckmax && !ispartition && !isfindfacet) {
    fprintf (qh ferr, "qhull internal error (qh_findbest): unknown combination of arguments\n");
    qh_errexit (qh_ERRqhull, facet, NULL);
  }
  if (qh TRACEpoint >= 0 && qh TRACEpoint == qh_pointid (point)) {
    qh IStracing= qh TRACElevel;
    fprintf (qh ferr, "qh_findbest: point p%d starting at f%d bestoutside? %d newfacets %d\n",
	     qh TRACEpoint, facet->id, bestoutside, newfacets);
    fprintf (qh ferr, "  Last point added to hull was p%d.", qh furthest_id);
    fprintf(qh ferr, "  Last merge was #%d.\n", zzval_(Ztotmerge));
  }
  if (isoutside)
    *isoutside= True;

  *numpart= 1;           
  qh_distplane (point, facet, dist);  /* this code is duplicated below */
  bestdist= *dist;
  bestfacet= facet;
  if (!bestoutside &&  *dist >= qh MINoutside) 
    goto LABELreturn_best;
#if qh_MAXoutside
  if (ischeckmax && (!qh ONLYgood || facet->good) && *dist > facet->maxoutside)
    facet->maxoutside= *dist;
#endif

  if (ispartition)
    searchdist= 2 * qh DISTround;
  else 
    searchdist= qh max_outside + 2 * qh DISTround
                + fmax_( qh MINvisible, qh MAXcoplanar);
  facet->visitid= ++qh visit_id;
  do {  /* search all neighbors of coplanar and flipped facets */
    if (True) {
   LABELrestart:  /* directed search as long as improvement > searchdist */
      trace4((qh ferr, "qh_findbest: neighbors of f%d\n", facet->id));
      FOREACHneighbor_(facet) {
	if (neighbor->visitid == qh visit_id)
          continue;
        if (ispartition && !neighbor->newfacet)
          continue;
        neighbor->visitid= qh visit_id;
        if (neighbor->flipped) { /* not tested if LABELrestart */
          if (!searchsize++) {
            SETfirst_(qh searchset) = neighbor;
            qh_settruncate (qh searchset, 1);
          }else
            qh_setappend (&qh searchset, neighbor);
          continue;
        }
        if (isfindfacet && neighbor->upperdelaunay)
          continue;
        (*numpart)++;
        qh_distplane (point, neighbor, dist);
        if (!bestoutside && *dist >= qh MINoutside) {
 	  bestfacet= neighbor;
	  goto LABELreturn_best;
        }
#if qh_MAXoutside
        if (ischeckmax) {
          if ((!qh ONLYgood || neighbor->good) 
                  && *dist > neighbor->maxoutside)
            neighbor->maxoutside= *dist;
        }
#endif
        if (neighbor->upperdelaunay && *dist <= - qh DISTround)
          continue;
        if (*dist > bestdist + searchdist) {
          if (!neighbor->upperdelaunay || *dist >= qh MINoutside) {
	    bestdist= *dist;
	    bestfacet= neighbor;
	  }
	  searchsize= 0;
	  facet= neighbor;
	  goto LABELrestart; /* repeat with a new facet */
        }
        if (*dist > bestdist - searchdist)  {
          if (*dist > bestdist) {
            if (!neighbor->upperdelaunay || *dist >= qh MINoutside) {
              bestdist= *dist;
              bestfacet= neighbor;
            }
          }
          if (!searchsize++) {
            SETfirst_(qh searchset) = neighbor;
            qh_settruncate (qh searchset, 1);
          }else
            qh_setappend (&qh searchset, neighbor);
        }
      } /* FOREACHneighbor */
    }
  }while
    (searchsize && (facet= (facetT*)qh_setdellast (qh searchset)));

  if (ispartition && !qh findbest_notsharp && bestdist < - qh DISTround) {
    if (qh_findbestsharp ( point, &bestfacet, &bestdist, numpart)) 
      qh findbestnew= True;
    else
      qh findbest_notsharp= True;
  }
  if (testhorizon) {
    facet= SETfirst_(bestfacet->neighbors);
    trace4((qh ferr, "qh_findbest: horizon facet f%d\n", facet->id));
    (*numpart)++;
    qh_distplane (point, facet, dist);
    if (*dist > bestdist && !facet->upperdelaunay) {
      bestdist= *dist;
      bestfacet= facet;
    }
  }
  *dist= bestdist;
  if (isoutside && bestdist < qh MINoutside)
    *isoutside= False;
LABELreturn_best:
  qh IStracing= oldtrace;
  return bestfacet;
}  /* findbest */


/*-------------------------------------------------
-findbestnew- find best newfacet for point
  searches new facets from facet
  if qh BESToutside or !isoutside
       stops at furthest facet
  if qh MERGING 
       stops when distance > qh_DISToutside (max(4*MINoutside, 2*max_outside))
  else 
       stops when distance > MINoutside (DISTround in precise case)
  searches newfacets then searchs neighbors of best facet.
  does not return upperdelaunay facets if (!isoutside or if not outside)
returns:
  distance to facet
  isoutside true if point is outside of facet
  numpart is number of distance tests
notes:
  uses visit_id and seen flags
  caller traces the results
  see also qh_partitionall() and qh_findbest()
*/
facetT *qh_findbestnew (pointT *point, facetT *startfacet,
	   realT *dist, boolT *isoutside, int *numpart) {
  realT bestdist= -REALmax, bestdist2= -REALmax;
  facetT *neighbor, **neighborp, *bestfacet= NULL, *newfacet, *facet;
  facetT *bestfacet2= NULL;
  int oldtrace= qh IStracing, i;
  realT distoutside;

  if (qh BESToutside || !isoutside)
    distoutside= REALmax;
  else if (qh MERGING)
    distoutside= qh_DISToutside; /* defined in user.h */
  else
    distoutside= qh MINoutside;
  if (qh TRACEpoint >= 0 && qh TRACEpoint == qh_pointid (point)) {
    qh IStracing= qh TRACElevel;
    fprintf(qh ferr, "qh_findbestnew: point p%d facet f%d. Stop if dist > %2.2g\n",
	     qh TRACEpoint, startfacet->id, distoutside);
    fprintf(qh ferr, "  Last point added to hull was p%d.", qh furthest_id);
    fprintf(qh ferr, "  Last merge was #%d.\n", zzval_(Ztotmerge));
  }
  if (isoutside)
    *isoutside= True;
  *numpart= 0;

  /* visit all new facets starting with startfacet */
  for (i= 0, facet= startfacet; i<2; i++, facet= qh newfacet_list) {
    FORALLfacet_(facet) {
      if (facet == startfacet && i)
	break;
      qh_distplane (point, facet, dist);
      (*numpart)++;
      if (facet->upperdelaunay) {
	if (*dist > bestdist2) {
	  bestdist2= *dist;
	  bestfacet2= facet;
	  if (*dist >= distoutside) {
	    bestfacet= facet;
	    goto LABELreturn_bestnew;
	  }
	}
      }else if (*dist > bestdist) {
	bestdist= *dist;
	bestfacet= facet;
	if (*dist >= distoutside) 
	  goto LABELreturn_bestnew;
      }
    }
  }
  newfacet= bestfacet;
  if (!newfacet) {
    fprintf(qh ferr, "qhull internal error (qh_findbestnew): merging had formed an independent cycle of facets.  New facet list is empty\n");
    qh_errexit (qh_ERRqhull, NULL, NULL);
  }
  FOREACHneighbor_(newfacet) {
    if (!neighbor->newfacet) {
      qh_distplane (point, neighbor, dist);
      (*numpart)++;
      if (neighbor->upperdelaunay) {
	if (*dist > bestdist2) {
	  bestdist2= *dist;
	  bestfacet2= neighbor;
	}
      }else if (*dist > bestdist) {
	bestdist= *dist;
	bestfacet= neighbor;
      }
    }
  }
  if (isoutside && bestdist2 >= qh MINoutside && bestdist2 > bestdist) {
    *dist= bestdist2;
    bestfacet= bestfacet2;
  }else {
    *dist= bestdist;
    if (isoutside && bestdist < qh MINoutside)
      *isoutside= False;
  }
LABELreturn_bestnew:
  qh IStracing= oldtrace;
  return bestfacet;
}  /* findbestnew */


/*-------------------------------------------------
-gausselim- Gaussian elimination with partial pivoting
  coordT data in rows
  assumes numrow <= numcol
returns:
  rows is upper triangular (includes row exchanges)
  flips sign for each row exchange
  sets nearzero if pivot[k] < qh NEARzero[k], else False.
    if nearzero, the determinant's sign may be incorrect.
*/
void qh_gausselim(realT **rows, int numrow, int numcol, boolT *sign, boolT *nearzero) {
  realT *ai, *ak, *rowp, *pivotrow;
  realT n, pivot, pivot_abs= 0.0, temp;
  int i, j, k, pivoti, flip=0;
  
  *nearzero= False;
  for(k= 0; k < numrow; k++) {
    pivot_abs= fabs_((rows[k])[k]);
    pivoti= k;
    for(i= k+1; i < numrow; i++) {
      if ((temp= fabs_((rows[i])[k])) > pivot_abs) {
	pivot_abs= temp;
	pivoti= i;
      }
    }
    if (pivoti != k) {
      rowp= rows[pivoti]; 
      rows[pivoti]= rows[k]; 
      rows[k]= rowp; 
      *sign ^= 1;
      flip ^= 1;
    }
    if (pivot_abs <= qh NEARzero[k]) {
      *nearzero= True;
      if (pivot_abs == 0.0) {   /* remainder of column == 0 */
	if (qh IStracing >= 4) {
	  fprintf (qh ferr, "qh_gausselim: 0 pivot at column %d. (%2.2g < %2.2g)\n", k, pivot_abs, qh DISTround);
	  qh_printmatrix (qh ferr, "Matrix:", rows, numrow, numcol);
	}
	zzinc_(Zgauss0);
	goto LABELnextcol;
      }
    }
    pivotrow= rows[k] + k;
    pivot= *pivotrow++;  /* signed value of pivot, and remainder of row */
    for(i= k+1; i < numrow; i++) {
      ai= rows[i] + k;
      ak= pivotrow;
      n= (*ai++)/pivot;   /* divzero() not needed since |pivot| >= |*ai| */
      for(j= numcol - (k+1); j--; )
	*ai++ -= n * *ak++;
    }
  LABELnextcol:
    ;
  }