geom.c

DT三角化实现

  realT bestdist= -REALmax/2;
  facetT *bestfacet= NULL, *facet;
  int oldtrace= qh IStracing, i;
  unsigned int visitid= ++qh visit_id;
  realT distoutside= 0.0;
  boolT isdistoutside; /* True if distoutside is defined */
  boolT testhorizon = True; /* needed if precise, e.g., rbox c D6 | qhull Q0 Tv */

  if (!startfacet) {
    if (qh MERGING)
      fprintf(qh ferr, "qhull precision error (qh_findbestnew): merging has formed and deleted a cone of new facets.  Can not continue.\n");
    else
      fprintf(qh ferr, "qhull internal error (qh_findbestnew): no new facets for point p%d\n",
      	      qh furthest_id);      
    qh_errexit (qh_ERRqhull, NULL, NULL);
  }
  zinc_(Zfindnew);
  if (qh BESToutside || bestoutside)
    isdistoutside= False;
  else {
    isdistoutside= True;
    distoutside= qh_DISToutside; /* multiple of qh.MINoutside & qh.max_outside, see user.h */
  }
  if (isoutside)
    *isoutside= True;
  *numpart= 0;
  if (qh IStracing >= 3 || (qh TRACElevel && qh TRACEpoint >= 0 && qh TRACEpoint == qh_pointid (point))) {
    if (qh TRACElevel > qh IStracing)
      qh IStracing= qh TRACElevel;
    fprintf(qh ferr, "qh_findbestnew: point p%d facet f%d. Stop? %d if dist > %2.2g\n",
	     qh_pointid(point), startfacet->id, isdistoutside, distoutside);
    fprintf(qh ferr, "  Last point added p%d visitid %d.",  qh furthest_id, visitid);
    fprintf(qh ferr, "  Last merge was #%d.\n", zzval_(Ztotmerge));
  }
  /* visit all new facets starting with startfacet, maybe qh facet_list */
  for (i= 0, facet= startfacet; i < 2; i++, facet= qh newfacet_list) {
    FORALLfacet_(facet) {
      if (facet == startfacet && i)
	break;
      facet->visitid= visitid;
      if (!facet->flipped) {
	qh_distplane (point, facet, dist);
	(*numpart)++;
	if (*dist > bestdist) {
	  if (!facet->upperdelaunay || *dist >= qh MINoutside) {
	    bestfacet= facet;
	    if (isdistoutside && *dist >= distoutside)
	      goto LABELreturn_bestnew;
	    bestdist= *dist;
  	  }
	}
      } /* end of !flipped */
    } /* FORALLfacet from startfacet or qh newfacet_list */
  }
  if (testhorizon || !bestfacet)
    bestfacet= qh_findbesthorizon (!qh_IScheckmax, point, bestfacet ? bestfacet : startfacet, 
	                                !qh_NOupper, &bestdist, numpart);  
  *dist= bestdist;
  if (isoutside && *dist < qh MINoutside)
    *isoutside= False;
LABELreturn_bestnew:
  zadd_(Zfindnewtot, *numpart);
  zmax_(Zfindnewmax, *numpart);
  trace4((qh ferr, "qh_findbestnew: bestfacet f%d bestdist %2.2g\n", getid_(bestfacet), *dist));
  qh IStracing= oldtrace;
  return bestfacet;
}  /* findbestnew */

/* ============ hyperplane functions -- keep code together [?] ============ */

/*-<a                             href="qh-geom.htm#TOC"
  >-------------------------------</a><a name="backnormal">-</a>
  
  qh_backnormal( rows, numrow, numcol, sign, normal, nearzero )
    given an upper-triangular rows array and a sign,
    solve for normal equation x using back substitution over rows U

  returns:
     normal= x
      
     if will not be able to divzero() when normalized (qh.MINdenom_2 and qh.MINdenom_1_2),
       if fails on last row
         this means that the hyperplane intersects [0,..,1]
         sets last coordinate of normal to sign
       otherwise
         sets tail of normal to [...,sign,0,...], i.e., solves for b= [0...0]
         sets nearzero

  notes:
     assumes numrow == numcol-1

     see Golub & van Loan 4.4-9 for back substitution

     solves Ux=b where Ax=b and PA=LU
     b= [0,...,0,sign or 0]  (sign is either -1 or +1)
     last row of A= [0,...,0,1]

     1) Ly=Pb == y=b since P only permutes the 0's of   b
     
  design:
    for each row from end
      perform back substitution
      if near zero
        use qh_divzero for division
        if zero divide and not last row
          set tail of normal to 0
*/
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));
    qh_precision ("zero diagonal on back substitution");
  }
} /* backnormal */

/*-<a                             href="qh-geom.htm#TOC"
  >-------------------------------</a><a name="gausselim">-</a>
  
  qh_gausselim( rows, numrow, numcol, sign )
    Gaussian elimination with partial pivoting

  returns:
    rows is upper triangular (includes row exchanges)
    flips sign for each row exchange
    sets nearzero if pivot[k] < qh.NEARzero[k], else clears it

  notes:
    if nearzero, the determinant's sign may be incorrect.
    assumes numrow <= numcol

  design:
    for each row
      determine pivot and exchange rows if necessary
      test for near zero
      perform gaussian elimination step
*/
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);
        qh_precision ("zero pivot for Gaussian elimination");
	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:
    ;
  }
  wmin_(Wmindenom, pivot_abs);  /* last pivot element */
  if (qh IStracing >= 5)
    qh_printmatrix (qh ferr, "qh_gausselem: result", rows, numrow, numcol);
} /* gausselim */


/*-<a                             href="qh-geom.htm#TOC"
  >-------------------------------</a><a name="getangle">-</a>
  
  qh_getangle( vect1, vect2 )
    returns the dot product of two vectors
    if qh.RANDOMdist, joggles result

  notes:
    the angle may be > 1.0 or < -1.0 because of roundoff errors

*/
realT qh_getangle(pointT *vect1, pointT *vect2) {
  realT angle= 0, randr;
  int k;

  for(k= qh hull_dim; k--; )
    angle += *vect1++ * *vect2++;
  if (qh RANDOMdist) {
    randr= qh_RANDOMint;
    angle += (2.0 * randr / qh_RANDOMmax - 1.0) *
      qh RANDOMfactor;
  }
  trace4((qh ferr, "qh_getangle: %2.2g\n", angle));
  return(angle);
} /* getangle */


/*-<a                             href="qh-geom.htm#TOC"
  >-------------------------------</a><a name="getcenter">-</a>
  
  qh_getcenter( vertices )
    returns arithmetic center of a set of vertices as a new point

  notes:
    allocates point array for center
*/
pointT *qh_getcenter(setT *vertices) {
  int k;
  pointT *center, *coord;
  vertexT *vertex, **vertexp;
  int count= qh_setsize(vertices);

  if (count < 2) {
    fprintf (qh ferr, "qhull internal error (qh_getcenter): not defined for %d points\n", count);
    qh_errexit (qh_ERRqhull, NULL, NULL);
  }
  center= (pointT *)qh_memalloc(qh normal_size);
  for (k=0; k < qh hull_dim; k++) {
    coord= center+k;
    *coord= 0.0;
    FOREACHvertex_(vertices)
      *coord += vertex->point[k];
    *coord /= count;
  }
  return(center);
} /* getcenter */


/*-<a                             href="qh-geom.htm#TOC"
  >-------------------------------</a><a name="getcentrum">-</a>
  
  qh_getcentrum( facet )
    returns the centrum for a facet as a new point

  notes:
    allocates the centrum
*/
pointT *qh_getcentrum(facetT *facet) {
  realT dist;
  pointT *centrum, *point;

  point= qh_getcenter(facet->vertices);
  zzinc_(Zcentrumtests);
  qh_distplane (point, facet, &dist);
  centrum= qh_projectpoint(point, facet, dist);
  qh_memfree(point, qh normal_size);
  trace4((qh ferr, "qh_getcentrum: for f%d, %d vertices dist= %2.2g\n",
	  facet->id, qh_setsize(facet->vertices), dist));
  return centrum;
} /* getcentrum */


/*-<a                             href="qh-geom.htm#TOC"
  >-------------------------------</a><a name="getdistance">-</a>
  
  qh_getdistance( facet, neighbor, mindist, maxdist )
    returns the maxdist and mindist distance of any vertex from neighbor

  returns:
    the max absolute value

  design:
    for each vertex of facet that is not in neighbor
      test the distance from vertex to neighbor
*/
realT qh_getdistance(facetT *facet, facetT *neighbor, realT *mindist, realT *maxdist) {
  vertexT *vertex, **vertexp;
  realT dist, maxd, mind;
  
  FOREACHvertex_(facet->vertices)
    vertex->seen= False;
  FOREACHvertex_(neighbor->vertices)
    vertex->seen= True;
  mind= 0.0;
  maxd= 0.0;
  FOREACHvertex_(facet->vertices) {
    if (!vertex->seen) {
      zzinc_(Zbestdist);
      qh_distplane(vertex->point, neighbor, &dist);
      if (dist < mind)
	mind= dist;
      else if (dist > maxd)
	maxd= dist;
    }
  }
  *mindist= mind;
  *maxdist= maxd;
  mind= -mind;
  if (maxd > mind)
    return maxd;
  else
    return mind;
} /* getdistance */


/*-<a                             href="qh-geom.htm#TOC"
  >-------------------------------</a><a name="normalize">-</a>

  qh_normalize( normal, dim, toporient )
    normalize a vector and report if too small
    does not use min norm
  
  see:
    qh_normalize2
*/
void qh_normalize (coordT *normal, int dim, boolT toporient) {
  qh_normalize2( normal, dim, toporient, NULL, NULL);
} /* normalize */

/*-<a                             href="qh-geom.htm#TOC"
  >-------------------------------</a><a name="normalize2">-</a>
  
  qh_normalize2( normal, dim, toporient, minnorm, ismin )
    normalize a vector and report if too small
    qh.MINdenom/MINdenom1 are the upper limits for divide overflow

  returns:
    normalized vector
    flips sign if !toporient
    if minnorm non-NULL, 
      sets ismin if normal < minnorm

  notes:
    if zero norm
       sets all elements to sqrt(1.0/dim)
    if divide by zero (divzero ())
       sets largest element to   +/-1
       bumps Znearlysingular
      
  design:
    computes norm
    test for minnorm
    if not near zero
      normalizes normal
    else if zero norm
      sets normal to standard value
    else
      uses qh_divzero to normalize
      if nearzero
        sets norm to direction of maximum value
*/
void qh_normalize2 (coordT *normal, int dim, boolT toporient, 
            realT *minnorm, boolT *ismin) {
  int k;
  realT *colp, *maxp, norm= 0, temp, *norm1, *norm2, *norm3;
  boolT zerodiv;

  norm1= normal+1;
  norm2= normal+2;
  norm3= normal+3;
  if (dim == 2)
    norm= sqrt((*normal)*(*normal) + (*norm1)*(*norm1));
  else if (dim == 3)
    norm= sqrt((*normal)*(*normal) + (*norm1)*(*norm1) + (*norm2)*(*norm2));
  else if (dim == 4) {
    norm= sqrt((*normal)*(*normal) + (*norm1)*(*norm1) + (*norm2)*(*norm2) 
               + (*norm3)*(*norm3));
  }else if (dim > 4) {
    norm= (*normal)*(*normal) + (*norm1)*(*norm1) + (*norm2)*(*norm2) 
               + (*norm3)*(*norm3);
    for (k= dim-4, colp= normal+4; k--; colp++)