inhedron.c

判断点是否在多面体之中

/*
This code is described in "Computational Geometry in C" (Second Edition),
Chapter 7.  It is not written to be comprehensible without the
explanation in that book.

Compile:    gcc -o inhedron inhedron.c -lm
Run (e.g.): inhedron < i.8

Written by Joseph O'Rourke, with contributions by Min Xu.
Last modified: April 1998
Questions to [e-mail].
--------------------------------------------------------------------
This code is Copyright 1998 by Joseph O'Rourke.  It may be freely
redistributed in its entirety provided that this copyright notice is
not removed.
--------------------------------------------------------------------
*/
#include <stdio.h>
#include <math.h>
#define EXIT_FAILURE 1
#define X 0
#define Y 1
#define Z 2
#define MAX_INT   2147483647 
typedef enum { FALSE, TRUE } bool;

#define DIM 3                  /* Dimension of points */
typedef int    tPointi[DIM];   /* Type integer point */
typedef double tPointd[DIM];   /* Type double point */
#define PMAX 10000             /* Max # of pts */
tPointi Vertices[PMAX];        /* All the points */
tPointi Faces[PMAX];           /* Each triangle face is 3 indices */
int check = 0;
tPointi Box[PMAX][2];          /* Box around each face */

/*---------------------------------------------------------------------
Function prototypes.
---------------------------------------------------------------------*/
char 	InPolyhedron( int F, tPointi q, tPointi bmin, tPointi bmax, int radius );
char    SegPlaneInt( tPointi Triangle, tPointi q, tPointi r, tPointd p, int *m )
;
int     PlaneCoeff( tPointi T, tPointd N, double *D );
void    Assigndi( tPointd p, tPointi a );
int     ReadVertices( void );
int     ReadFaces( void );
void    NormalVec( tPointi q, tPointi b, tPointi c, tPointd N );
double  Dot( tPointi q, tPointd d );
void    SubVec( tPointi q, tPointi b, tPointi c );
char    InTri3D( tPointi T, int m, tPointi p );
char    InTri2D( tPointi Tp[3], tPointi pp );
int     AreaSign( tPointi q, tPointi b, tPointi c );
char    SegTriInt( tPointi Triangle, tPointi q, tPointi r, tPointd p );
char    InPlane( tPointi Triangle, int m, tPointi q, tPointi r, tPointd p);
int     VolumeSign( tPointi a, tPointi b, tPointi c, tPointi d );
char    SegTriCross( tPointi Triangle, tPointi q, tPointi r );
int  	ComputeBox( int F, tPointi bmin, tPointi bmax );
void 	RandomRay( tPointi ray, int radius );
void 	AddVec( tPointi q, tPointi ray );
int  	InBox( tPointi q, tPointi bmin, tPointi bmax );
char 	BoxTest ( int n, tPointi a, tPointi b );
void 	PrintPoint( tPointi q );
/*-------------------------------------------------------------------*/

main()
{
  int n, F, i;
  tPointi q, bmin, bmax;
  int radius;

  srandom( (int) time( (long *) 0 ) ); 

  n = ReadVertices();
  F = ReadFaces();

  /* Initialize the bounding box */
  for ( i = 0; i < DIM; i++ )
    bmin[i] = bmax[i] = Vertices[0][i];
  radius = ComputeBox( n, bmin, bmax );
  printf("radius=%d\n", radius);

  while( scanf( "%d %d %d", &q[X], &q[Y], &q[Z] ) != EOF ) {
    printf( "\n----------->q = %d %d %d\n", 
       q[X], q[Y], q[Z] );
    printf( "In = %c\n", 
       InPolyhedron( F, q, bmin, bmax, radius ) );
  }
}

/*
  This function returns a char:
    'V': the query point a coincides with a Vertex of polyhedron P.
    'E': the query point a is in the relative interior of an Edge of polyhedron P.
    'F': the query point a is in the relative interior of a Face of polyhedron P.
    'i': the query point a is strictly interior to polyhedron P.
    'o': the query point a is strictly exterior to( or outside of) polyhedron P.
*/
char InPolyhedron( int F, tPointi q, tPointi bmin, tPointi bmax, int radius )
{
   tPointi r;  /* Ray endpoint. */
   tPointd p;  /* Intersection point; not used. */
   int f, k = 0, crossings = 0;
   char code = '?';
 
   /* If query point is outside bounding box, finished. */
   if ( !InBox( q, bmin, bmax ) )
      return 'o';
  
   LOOP:
   while( k++ < F ) {
      crossings = 0;
  
      RandomRay( r, radius ); 
      AddVec( q, r ); 
      printf("Ray endpoint: (%d,%d,%d)\n", r[0],r[1],r[2] );
  
      for ( f = 0; f < F; f++ ) {  /* Begin check each face */
         if ( BoxTest( f, q, r ) == '0' ) {
              code = '0';
              printf("BoxTest = 0!\n");
         }
         else code = SegTriInt( Faces[f], q, r, p );
         printf( "Face = %d: BoxTest/SegTriInt returns %c\n\n", f, code );

         /* If ray is degenerate, then goto outer while to generate another. */
         if ( code == 'p' || code == 'v' || code == 'e' ) {
            printf("Degenerate ray\n");
            goto LOOP;
         }
   
         /* If ray hits face at interior point, increment crossings. */
         else if ( code == 'f' ) {
            crossings++;
            printf( "crossings = %d\n", crossings );
         }

         /* If query endpoint q sits on a V/E/F, return that code. */
         else if ( code == 'V' || code == 'E' || code == 'F' )
            return( code );

         /* If ray misses triangle, do nothing. */
         else if ( code == '0' )
            ;

         else 
            fprintf( stderr, "Error, exit(EXIT_FAILURE)\n" ), exit(1);

      } /* End check each face */

      /* No degeneracies encountered: ray is generic, so finished. */
      break;

   } /* End while loop */
 
   printf( "Crossings = %d\n", crossings );
   /* q strictly interior to polyhedron iff an odd number of crossings. */
   if( ( crossings % 2 ) == 1 )
      return   'i';
   else return 'o';
}

int ComputeBox( int F, tPointi bmin, tPointi bmax )
{
  int i, j, k;
  double radius;
  
  for( i = 0; i < F; i++ )
    for( j = 0; j < DIM; j++ ) {
      if( Vertices[i][j] < bmin[j] )
	bmin[j] = Vertices[i][j];
      if( Vertices[i][j] > bmax[j] ) 
	bmax[j] = Vertices[i][j];
    }
  
  radius = sqrt( pow( (double)(bmax[X] - bmin[X]), 2.0 ) +
                 pow( (double)(bmax[Y] - bmin[Y]), 2.0 ) +
                 pow( (double)(bmax[Z] - bmin[Z]), 2.0 ) );
  printf("radius = %lf\n", radius);

  return irint( radius +1 ) + 1;
}

/* Return a random ray endpoint */
void RandomRay( tPointi ray, int radius )
{
  double x, y, z, w, t;

  /* Generate a random point on a sphere of radius 1. */
  /* the sphere is sliced at z, and a random point at angle t
     generated on the circle of intersection. */
  z = 2.0 * (double) random() / MAX_INT - 1.0;
  t = 2.0 * M_PI * (double) random() / MAX_INT;
  w = sqrt( 1 - z*z );
  x = w * cos( t );
  y = w * sin( t );
  
  ray[X] = (irint) ( radius * x );
  ray[Y] = (irint) ( radius * y );
  ray[Z] = (irint) ( radius * z );
  
  /*printf( "RandomRay returns %6d %6d %6d\n", ray[X], ray[Y], ray[Z] );*/
}

void AddVec( tPointi q, tPointi ray )
{
  int i;
  
  for( i = 0; i < DIM; i++ )
    ray[i] = q[i] + ray[i];
}

int InBox( tPointi q, tPointi bmin, tPointi bmax )
{
  int i;

  if( ( bmin[X] <= q[X] ) && ( q[X] <= bmax[X] ) &&
      ( bmin[Y] <= q[Y] ) && ( q[Y] <= bmax[Y] ) &&
      ( bmin[Z] <= q[Z] ) && ( q[Z] <= bmax[Z] ) )
    return TRUE;
  return FALSE;
}
    

/*---------------------------------------------------------------------
    'p': The segment lies wholly within the plane.
    'q': The q endpoint is on the plane (but not 'p').
    'r': The r endpoint is on the plane (but not 'p').
    '0': The segment lies strictly to one side or the other of the plane.
    '1': The segement intersects the plane, and 'p' does not hold.
---------------------------------------------------------------------*/
char	SegPlaneInt( tPointi T, tPointi q, tPointi r, tPointd p, int *m)
{
    tPointd N; double D;
    tPointi rq;
    double num, denom, t;
    int i;

    *m = PlaneCoeff( T, N, &D );
    /*printf("m=%d; plane=(%lf,%lf,%lf,%lf)\n", m, N[X],N[Y],N[Z],D);*/
    num = D - Dot( q, N );
    SubVec( r, q, rq );
    denom = Dot( rq, N );
    /*printf("SegPlaneInt: num=%lf, denom=%lf\n", num, denom );*/

    if ( denom == 0.0 ) {  /* Segment is parallel to plane. */
       if ( num == 0.0 )   /* q is on plane. */
           return 'p';
       else
           return '0';
    }
    else
       t = num / denom;
    /*printf("SegPlaneInt: t=%lf \n", t );*/

    for( i = 0; i < DIM; i++ )
       p[i] = q[i] + t * ( r[i] - q[i] );

    if ( (0.0 < t) && (t < 1.0) )
         return '1';
    else if ( num == 0.0 )   /* t == 0 */
         return 'q';
    else if ( num == denom ) /* t == 1 */
         return 'r';
    else return '0';
}
/*---------------------------------------------------------------------
Computes N & D and returns index m of largest component.
---------------------------------------------------------------------*/
int	PlaneCoeff( tPointi T, tPointd N, double *D )
{
    int i;
    double t;              /* Temp storage */
    double biggest = 0.0;  /* Largest component of normal vector. */
    int m = 0;             /* Index of largest component. */

    NormalVec( Vertices[T[0]], Vertices[T[1]], Vertices[T[2]], N );
    /*printf("PlaneCoeff: N=(%lf,%lf,%lf)\n", N[X],N[Y],N[Z]);*/
    *D = Dot( Vertices[T[0]], N );

    /* Find the largest component of N. */
    for ( i = 0; i < DIM; i++ ) {
      t = fabs( N[i] );
      if ( t > biggest ) {
        biggest = t;
        m = i;
      }
    }
    return m;
}
/*---------------------------------------------------------------------
Reads in the number and coordinates of the vertices of a polyhedron
from stdin, and returns n, the number of vertices.
---------------------------------------------------------------------*/
int 	ReadVertices( void )
{
   int   i, n;

   do {
     scanf( "%d", &n );
     if ( n <= PMAX )
       break;
     printf("Error in read_vertex:  too many points; max is %d\n", PMAX);
   }
   while ( 1 );

   printf( "Polyhedron Vertices:\n" );
   printf( "  i:   x   y   z\n");
   for ( i = 0; i < n; i++ ) {
     scanf( "%d %d %d", &Vertices[i][X], &Vertices[i][Y], &Vertices[i][Z] );
     printf( "%3d:%4d%4d%4d\n", i, Vertices[i][X], Vertices[i][Y], Vertices[i][Z