geom2.c

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

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

   copyright (c) 1993-1995 The Geometry Center        

   frequently used code goes into geom.c
*/
   
#include "qhull_a.h"
   


/*-------------------------------------------------
-crossproduct- of 2 dim vectors, C= A x B
    from Glasner, Graphics Gems I, p. 639
    NOTE: only defined for dim==3
*/
void qh_crossproduct (int dim, realT vecA[3], realT vecB[3], realT vecC[3]){

  if (dim == 3) {
    vecC[0]=   det2_(vecA[1], vecA[2],
		     vecB[1], vecB[2]);
    vecC[1]= - det2_(vecA[0], vecA[2],
		     vecB[0], vecB[2]);
    vecC[2]=   det2_(vecA[0], vecA[1],
		     vecB[0], vecB[1]);
  }
} /* vcross */

/*-------------------------------------------------
-determinant- compute signed determinant of a square matrix
  rows= row vectors
  uses qh NEARzero to test for degenerate matrices
    this does look right, probably no easy way of doing it
returns:
  determinant
  overwrites rows and the matrix
  nearzero set if degenerate, else cleared
   if dim == 2 or 3
     nearzero iff determinant < qh NEARzero[dim-1]  (not quite correct)
   if dim >= 4
     nearzero iff diagonal[k] < qh NEARzero[k]
*/
realT qh_determinant (realT **rows, int dim, boolT *nearzero) {
  realT det=0;
  int i;
  boolT sign= False;

  *nearzero= False;
  if (dim < 2) {
    fprintf (qh ferr, "qhull internal error (qh_determinate): only implemented for dimension >= 2\n");
    qh_errexit (qh_ERRqhull, NULL, NULL);
  }else if (dim == 2) {
    det= det2_(rows[0][0], rows[0][1],
		 rows[1][0], rows[1][1]);
    if (fabs_(det) < qh NEARzero[1])  /* not really correct, what should this be? */
      *nearzero= True;
  }else if (dim == 3) {
    det= det3_(rows[0][0], rows[0][1], rows[0][2],
		 rows[1][0], rows[1][1], rows[1][2],
		 rows[2][0], rows[2][1], rows[2][2]);
    if (fabs_(det) < qh NEARzero[2])  /* not really correct, what should this be? */
      *nearzero= True;
  }else {	
    qh_gausselim(rows, dim, dim, &sign, nearzero);  /* if nearzero, diagonal still ok*/
    det= 1.0;
    for (i= dim; i--; )
      det *= (rows[i])[i];
    if (sign)
      det= -det;
  }
  return det;
} /* determinant */

/*-------------------------------------------------
-detsimplex- compute determinant of a simplex with point apex and base points
   uses qh gm_matrix/qh gm_row (assumes they're big enough)
   uses dim coordinates of point and vertex->point
returns:
   signed determinant and nearzero from qh_determinant
*/
realT qh_detsimplex(pointT *apex, setT *points, int dim, boolT *nearzero) {
  pointT *coorda, *coordp, *gmcoord, *point, **pointp;
  coordT **rows;
  int k,  i=0;
  realT det;

  zinc_(Zdetsimplex);
  gmcoord= qh gm_matrix;
  rows= qh gm_row;
  FOREACHpoint_(points) {
    if (i == dim)
      break;
    rows[i++]= gmcoord;
    coordp= point;
    coorda= apex;
    for (k= dim; k--; )
      *(gmcoord++)= *coordp++ - *coorda++;
  }
  if (i < dim) {
    fprintf (qh ferr, "qhull internal error (qh_detsimplex): #points %d < dimension %d\n", 
               i, dim);
    qh_errexit (qh_ERRqhull, NULL, NULL);
  }
  det= qh_determinant (rows, dim, nearzero);
  trace2((qh ferr, "qh_detsimplex: det=%2.2g for point p%d, dim %d, nearzero? %d\n",
	  det, qh_pointid(apex), dim, *nearzero)); 
  return det;
} /* detsimplex */

/*--------------------------------------------------
-divzero -- divide by a number that's nearly zero
  mindenom1= minimum denominator for dividing into 1.0
returns:
  zerodiv and 0.0 if it would overflow
*/
realT qh_divzero (realT numer, realT denom, realT mindenom1, boolT *zerodiv) {
  realT temp, numerx, denomx;
  

  if (numer < mindenom1 && numer > -mindenom1) {
    numerx= fabs_(numer);
    denomx= fabs_(denom);
    if (numerx < denomx) {
      *zerodiv= False;
      return numer/denom;
    }else {
      *zerodiv= True;
      return 0.0;
    }
  }
  temp= denom/numer;
  if (temp > mindenom1 || temp < -mindenom1) {
    *zerodiv= False;
    return numer/denom;
  }else {
    *zerodiv= True;
    return 0.0;
  }
} /* divzero */
  

/*-------------------------------------------------
-facetarea- return area for a facet
  if non-simplicial, uses centrum to triangulate facet.  Sums the projected areas.
  assumes facet->normal exists
  if (qh DELAUNAY),
     computes projected area instead for last coordinate
*/
realT qh_facetarea (facetT *facet) {
  vertexT *apex;
  pointT *centrum;
  realT area= 0.0;
  ridgeT *ridge, **ridgep;

  if (facet->simplicial) {
    apex= SETfirst_(facet->vertices);
    area= qh_facetarea_simplex (qh hull_dim, apex->point, facet->vertices, 
                    apex, facet->toporient, facet->normal, &facet->offset);
  }else {
    if (qh CENTERtype == qh_AScentrum)
      centrum= facet->center;
    else
      centrum= qh_getcentrum (facet);
    FOREACHridge_(facet->ridges) 
      area += qh_facetarea_simplex (qh hull_dim, centrum, ridge->vertices, 
                 NULL, (ridge->top == facet),  facet->normal, &facet->offset);
    if (qh CENTERtype != qh_AScentrum)
      qh_memfree (centrum, qh normal_size);
  }
  trace4((qh ferr, "qh_facetarea: f%d area %2.2g\n", facet->id, area)); 
  return area;
} /* facetarea */

/*-------------------------------------------------
-facetarea_simplex- return area for a simplex defined by an apex,
        a base of vertices, an orientation, and a unit normal
  if simplicial facet, notvertex is defined and it is skipped in vertices
  if (qh DELAUNAY),
     computes projected area instead for last coordinate
notes:
  computes area of simplex projected to plane [normal,offset]
  returns 0 if vertex too far below plane (qh WIDEfacet)
  uses qh gm_matrix/gm_row and qh hull_dim
  helper function for qh_facetarea
*/
realT qh_facetarea_simplex (int dim, coordT *apex, setT *vertices, 
        vertexT *notvertex,  boolT toporient, coordT *normal, realT *offset) {
  pointT *coorda, *coordp, *gmcoord;
  coordT **rows, *normalp;
  int k,  i=0;
  realT area, dist;
  vertexT *vertex, **vertexp;
  boolT nearzero;

  gmcoord= qh gm_matrix;
  rows= qh gm_row;
  FOREACHvertex_(vertices) {
    if (vertex == notvertex)
      continue;
    rows[i++]= gmcoord;
    coorda= apex;
    coordp= vertex->point;
    normalp= normal;
    if (notvertex) {
      for (k= dim; k--; )
	*(gmcoord++)= *coordp++ - *coorda++;
    }else {
      dist= *offset;
      for (k= dim; k--; )
	dist += *coordp++ * *normalp++;
      if (dist < -qh WIDEfacet) {
	zinc_(Znoarea);
	return 0.0;
      }
      coordp= vertex->point;
      normalp= normal;
      for (k= dim; k--; )
	*(gmcoord++)= (*coordp++ - dist * *normalp++) - *coorda++;
    }
  }
  if (i != dim-1) {
    fprintf (qh ferr, "qhull internal error (qh_facetarea_simplex): #points %d != dim %d -1\n", 
               i, dim);
    qh_errexit (qh_ERRqhull, NULL, NULL);
  }
  rows[i]= gmcoord;
  if (qh DELAUNAY) {
    for (i= 0; i < dim-1; i++)
      rows[i][dim-1]= 0.0;
    for (k= dim; k--; )
      *(gmcoord++)= 0.0;
    rows[dim-1][dim-1]= 1.0;
  }else {
    normalp= normal;
    for (k= dim; k--; )
      *(gmcoord++)= *normalp++;
  }
  zinc_(Zdetsimplex);
  area= qh_determinant (rows, dim, &nearzero);
  if (toporient)
    area= -area;
  area *= qh AREAfactor;
  trace4((qh ferr, "qh_facetarea_simplex: area=%2.2g for point p%d, toporient %d, nearzero? %d\n",
	  area, qh_pointid(apex), toporient, nearzero)); 
  return area;
} /* facetarea_simplex */

/*-------------------------------------------------
-facetcenter- return Voronoi center for a facet's vertices
*/
pointT *qh_facetcenter (setT *vertices) {
  setT *points= qh_settemp (qh_setsize (vertices));
  vertexT *vertex, **vertexp;
  pointT *center;
  
  FOREACHvertex_(vertices) 
    qh_setappend (&points, vertex->point);
  center= qh_voronoi_center (qh hull_dim-1, points);
  qh_settempfree (&points);
  return center;
} /* facetcenter */

/*-------------------------------------------------
-findbestsharp- find best facet on newfacet_list
  skips visited facets (qh visit_id) on qh newfacet_list
  skips upperdelaunay facets unless point is outside
returns:
  true if could be an acute angle (facets in different quadrants)
  returns bestfacet and distance to facet
  increments numpart by number of distance tests
  for qh_findbest
*/
boolT qh_findbestsharp (pointT *point, facetT **bestfacet, 
           realT *bestdist, int *numpart) {
  facetT *facet;
  realT dist;
  boolT issharp = False;
  int *quadrant, k;
  
  quadrant= (int*)qh_memalloc (qh hull_dim * sizeof(int));
  FORALLfacet_(qh newfacet_list) {
    if (facet == qh newfacet_list) {
      for (k= qh hull_dim; k--; )
      	quadrant[ k]= (facet->normal[ k] > 0);
    }else if (!issharp) {
      for (k= qh hull_dim; k--; ) {
        if (quadrant[ k] != (facet->normal[ k] > 0)) {
          issharp= True;
          break;
        }
      }
    }
    if (facet->visitid != qh visit_id) {
      qh_distplane (point, facet, &dist);
      (*numpart)++;
      if (dist > *bestdist) {
      	if (!facet->upperdelaunay || dist > qh MINoutside) {
 	  *bestdist= dist;
	  *bestfacet= facet;
	}
      }
    }
  }
  qh_memfree( quadrant, qh hull_dim * sizeof(int));
  return issharp;
} /* findbestsharp */
  
/*-------------------------------------------------
-findgooddist- find best good facet visible for point from facetA
  assumes facetA is visible from point
  uses qh visit_id
returns:
  furthest distance to good facet, if any
  list of good, visible facets (and some other visible facets)
     at end of qh facet_list
*/
facetT *qh_findgooddist (pointT *point, facetT *facetA, realT *distp, 
               facetT **facetlist) {
  realT bestdist= -REALmax, dist;
  facetT *neighbor, **neighborp, *bestfacet=NULL, *facet;
  boolT goodseen= False;  

  if (facetA->good) {
    zinc_(Zcheckpart);  /* calls from check_bestdist occur after print stats */
    qh_distplane (point, facetA, &bestdist);
    bestfacet= facetA;
    goodseen= True;
  }
  qh_removefacet (facetA);
  qh_appendfacet (facetA);
  *facetlist= facetA;
  facetA->visitid= ++qh visit_id;
  FORALLfacet_(*facetlist) {
    FOREACHneighbor_(facet) {
      if (neighbor->visitid == qh visit_id)
        continue;
      neighbor->visitid= qh visit_id;
      if (goodseen && !neighbor->good)
        continue;
      zinc_(Zcheckpart); 
      qh_distplane (point, neighbor, &dist);
      if (dist > 0) {
        qh_removefacet (neighbor);
        qh_appendfacet (neighbor);
        if (neighbor->good) {
          goodseen= True;
          if (dist > bestdist) {
            bestdist= dist;
            bestfacet= neighbor;
          }
        }
      }
    }
  }
  if (bestfacet) {
    *distp= bestdist;
    trace2((qh ferr, "qh_findgooddist: p%d is %2.2g above good facet f%d\n",
      qh_pointid(point), bestdist, bestfacet->id));
    return bestfacet;
  }
  trace4((qh ferr, "qh_findgooddist: no good facet for p%d above f%d\n", 
      qh_pointid(point), facetA->id));
  return NULL;
}  /* findgooddist */
    
/*------------------------------------------
-getarea- get area of all facets in facetlist, collect statistics

notes:
  could compute outer volume by expanding facet area by rays from interior
#if qh_MAXoutside  // a perpendicular projection underestimates badly
      qh totoutvol += (-dist + facet->maxoutside + qh DISTround) 
                            * area/ qh hull_dim;
#endif
*/
void qh_getarea (facetT *facetlist) {
  realT area;
  realT dist;
  facetT *facet;

  if (qh REPORTfreq)
    fprintf (qh ferr, "computing area of each facet and volume of the convex hull\n");
  else 
    trace1((qh ferr, "qh_getarea: computing volume and area for each facet\n"));
  qh totarea= qh totvol= 0.0;
  FORALLfacet_(facetlist) {
    if (!facet->normal)
      continue;
    facet->f.area= area= qh_facetarea (facet);
    facet->isarea= True;
    if (!qh DELAUNAY) {
      qh_distplane (qh interior_point, facet, &dist);
      qh totvol += -dist * area/ qh hull_dim;
    }
    qh totarea += area;
    if (qh PRINTstatistics) {
      wadd_(Wareatot, area);
      wmax_(Wareamax, area);
      wmin_(Wareamin, area);
    }
  }
} /* getarea */

/*-------------------------------------------------
-gram_schmidt- implements Gram-Schmidt orthogonalization by rows
   overwrites rows[dim][dim]
returns:
   false if gets a zero norm
notes:
   see Golub & van Loan Algorithm 6.2-2
   overflow due to small divisors not handled
*/
boolT qh_gram_schmidt(int dim, realT **row) {
  realT *rowi, *rowj, norm;
  int i, j, k;
  
  for(i=0; i < dim; i++) {
    rowi= row[i];
    for (norm= 0.0, k= dim; k--; rowi++)
      norm += *rowi * *rowi;
    norm= sqrt(norm);
    wmin_(Wmindenom, norm);
    if (norm == 0.0)  /* either 0 or overflow due to sqrt */
      return False;
    for(k= dim; k--; )
      *(--rowi) /= norm;  
    for(j= i+1; j < dim; j++) {
      rowj= row[j];
      for(norm= 0.0, k=dim; k--; )
	norm += *rowi++ * *rowj++;
      for(k=dim; k--; )
	*(--rowj) -= *(--rowi) * norm;
    }
  }
  return True;
} /* gram_schmidt */



	
/*--------------------------------------------------
-inthresholds- return True if normal within qh lower_/upper_threshold
returns:
  angle cos to a threshold border (may be NULL, invalid if qh SPLITthresholds)
*/
boolT qh_inthresholds (coordT *normal, realT *angle) {
  boolT within= True;
  int k;

  if (angle)
    *angle= 0.0;
  for(k= 0; k < qh hull_dim; k++) {
    if (qh lower_threshold[k] > -REALmax/2) {
      if (normal[k] < qh lower_threshold[k])
        within= False;