Path: utzoo!attcan!uunet!wuarchive!cs.utexas.edu!news-server.csri.toronto.edu!csri.toronto.edu!corkum
From: corkum@csri.toronto.edu (Brent Thomas Corkum)
Newsgroups: comp.graphics
Subject: point in a volume: PART III - the final chapter
Message-ID: <1990Oct3.121706.11932@jarvis.csri.toronto.edu>
Date: 3 Oct 90 16:17:06 GMT
References: <1990Oct3.121330.11851@jarvis.csri.toronto.edu>
Organization: CSRI, University of Toronto
Lines: 650

In article <1990Oct3.121330.11851@jarvis.csri.toronto.edu> corkum@csri.toronto.edu (Brent Thomas Corkum) writes:
>
>Due to popular demand I am posting my generalised point in a volume (polytope)
>tester. It's written in C, and handles concave,convex polygons making
>up a concave or convex volume. The polygons may have any number of
>vertices.  I don't really like posting it but I don't have, and don't care
>to have, anonymous ftp capability, so if anyone wants to donate some space
>to make this available, mail me.
>
>
>The following two postings contain information and source code for my
>point inside a volume tester. The first file contains the readme file
>that explains how to compile and run the test program. The test program 
>illustrates the use of the point inside a volume tester, and gives an example. 
>
>Feel free to distribute this code, I place it in the public domain with
>the only criteria for using it being proper acknowledgement. You may
>use it in commercial software free of charge with only the above
>restriction. So have fun.
>
>Brent Corkum
>corkum@boulder.civ.toronto.edu (128.100.14.11) or
>corkum@csri.toronto.edu

#include <malloc.h>
#include <math.h>
#include <stdio.h>
#define TRUE 1
#define FALSE 0

/*
   This program will determine if point (x,y,z) is inside
   a closed polytope defined by the list "polygons" which
   contains "numpolys" concave or convex polygons.  The main
   subroutine of the program reads in a closed polytope
   from a .geo file and then prompts the user for a point
   to check.  The Insidepolytope subroutine is then called which
   will return TRUE if the point is inside the polytope.

   J. Andrew Wyllie and Brent Corkum
   Data Visualization Laboratory
   Dept Of Civil Engineering
   University of Toronto
   Canada
   September 1990
*/

/* The following structures are used to keep track of nodes and elements */
/* The node structure holds the x,y and z coordinates of the node */
/* The element structur will hold the number of nodes and a list of
     the nodes included in the structure */

struct Nodes{
  float x;
  float y;
  float z;
}*node;

struct Elements{
  int numnodes;
  int *nodes;
}*element;

int numpolys;

float DotProduct();

/* The function main is used to read in a polytope and prompt for
   a point to be checked  */

void main(void)
{
  register int i,j,k;
  int numnodes,numverts,vert,check;
  float x,y,z,xtemp,ytemp,ztemp;
  char fname[50];
  FILE *in;

   
  printf("enter .geo file name\n");
  gets(fname);
  /* read in a polytope from a .geo file */
  in=fopen(fname,"rt");
  if(!in){
    printf("file cannot be opened\n");
    exit(1);
  }
  printf("file open\n");

  /* read in the number of nodes and the number of elements */
  if(!fscanf(in,"%d%d%*d\n",&numnodes,&numpolys))exit(1);

  /* allocate space for the arrays of node and element structures */
  node = (struct Nodes *)malloc(numnodes*sizeof(struct Nodes));
  element = (struct Elements *)malloc(numpolys*sizeof(struct Elements));
  if(!node || !element){
    printf("memory allocation error <node or element>, abort\n");
    exit(1);
  }
  printf("reading nodes\n");
  for(i=0;i<numnodes;++i){
    if(!fscanf(in,"%f%f%f\n",&xtemp,&ytemp,&ztemp))exit(1);
    node[i].x = xtemp;
    node[i].y = ytemp;
    node[i].z = ztemp;
  }
  printf("reading elements\n");
  for(j=0;j<numpolys;++j){
    fscanf(in,"%d",&numverts);
    element[j].numnodes=numverts;

    /* allocate memory for the list of nodes for each element */
    element[j].nodes = (int *)malloc((numverts+1)*sizeof(int));
    if(!element[j].nodes){
      printf("memory allocation error <element[j].nodes>, abort\n");
      exit(1);
    }
    for(k=0;k<numverts;++k){
      fscanf(in,"%d",&vert);
      element[j].nodes[k]=vert-1;
    }
    element[j].nodes[numverts]=element[j].nodes[0];
  }
  fclose(in);

  /* read in the x,y,z coordinates of the point to be checked */
  do{
    printf("enter x coord\n");
    scanf("%f",&x);
    fflush(stdin);
    printf("enter y coord\n");
    scanf("%f",&y);
    fflush(stdin);
    printf("enter z coord\n");
    scanf("%f",&z);
    fflush(stdin);

    printf("call insidepolytope\n");

    /* call the inside polytope function */
    /* the function returns TRUE if the point is inide the polytope */
    if(Insidepolytope(x,y,z)){
      printf("point inside\n");
    }
    else{
      printf("point outside\n");
    }

    printf("1 to continue, 0 to quit.\n");
    scanf("%1d",&check);
    fflush(stdin);
  }while(check!=0);
}

/* The inside polytope function is used to check if the
   point x,y,z is inside the polygon defined by the arrays of
   node and element structures.
   The subroutine will return TRUE if the point is inside the polytope
*/

Insidepolytope(x,y,z)
float x,y,z;
{
  register int i,j,k,l,m;
  int intersect=0,numverts;
  float xmin,xmax,ymin,ymax,zmin,zmax;
  float *dpl,*pl;
  float xdir,ydir,zdir,dir,nx,ny,nz,d,a1,a2,t;
  float xint,yint,zint,xcheck,ycheck;
  float rot[4][4];

  /* create bounding brick around polytope */
  /* the bounding brick is a 3-d rectangular box which entirely
     encompasses the given polytope.  If a lot of points are to be
     checked, it may be better to generate the bounding brick once in
     another function instead of generating it every time a point is
     to be checked. */

  xmin=node[element[0].nodes[0]].x;
  xmax=node[element[0].nodes[0]].x;
  ymin=node[element[0].nodes[0]].y;
  ymax=node[element[0].nodes[0]].y;
  zmin=node[element[0].nodes[0]].z;
  zmax=node[element[0].nodes[0]].z;

  for(i=0;i<numpolys;++i){
    numverts=element[i].numnodes;
    for(j=0;j<numverts;++j){
      xmin = xmin > node[element[i].nodes[j]].x ? node[element[i].nodes[j]].x : xmin;
      xmax = xmax < node[element[i].nodes[j]].x ? node[element[i].nodes[j]].x : xmax;
      ymin = ymin > node[element[i].nodes[j]].y ? node[element[i].nodes[j]].y : ymin;
      ymax = ymax < node[element[i].nodes[j]].y ? node[element[i].nodes[j]].y : ymax;
      zmin = zmin > node[element[i].nodes[j]].z ? node[element[i].nodes[j]].z : zmin;
      zmax = zmax < node[element[i].nodes[j]].z ? node[element[i].nodes[j]].z : zmax;
    }
  }

  /* check to see if point is inside bounding brick */

  if((x > xmax)||(x < xmin)||
     (y > ymax)||(y < ymin)||
     (z > zmax)||(z < zmin)) return(0); /* point outside bounding brick */

  printf("inside bounding box\n");
  /* If the point is inside the bounding brick */
  /* Generate a random ray from point (x,y,z) */

  xdir = (1.0*rand())/(32767*1.0);
  dir = (1.0*rand())/(32767*1.0);
  if(dir<0.5)xdir= -xdir;
  ydir = (1.0*rand())/(32767*1.0);
  dir = (1.0*rand())/(32767*1.0);
  if(dir<0.5)ydir= -ydir;
  zdir = (1.0*rand())/(32767*1.0);
  dir = (1.0*rand())/(32767*1.0);
  if(dir<0.5)zdir= -zdir;
  if(xdir==0.0 && ydir ==0.0 && zdir==0.0){
    xdir=1.0;ydir=0.0;zdir=0.0;
  }

  Normalize(&xdir,&ydir,&zdir);

  /* Now find out how many polygons the random ray intersects */

  for(k=0;k<numpolys;++k){
    numverts=element[k].numnodes; /* get number of nodes in element */

    /* create an array to hold the coordinates of the element being checked */

    pl=(float *)malloc(3*(numverts)*sizeof(float));
    dpl=(float *)malloc(2*(numverts)*sizeof(float)); 
    if(!pl || !dpl){
      printf("out of memory allocating pl in function Insidepolytope\n");
      exit(1);
    }
    for(m=0;m<numverts;++m){
      pl[m*3]   = node[element[k].nodes[m]].x;
      pl[m*3+1] = node[element[k].nodes[m]].y;
      pl[m*3+2] = node[element[k].nodes[m]].z;
    }

    /* calculate normal to plane */
    Plane_normal(pl,numverts,&nx,&ny,&nz);

    /* now the embedding plane is represented by N.P + d = 0 */
    /* solve for d */

    d= -(DotProduct(pl[0],pl[1],pl[2],nx,ny,nz));

    /* now solve for t, the distance to the plane */
  
    a1=DotProduct(nx,ny,nz,xdir,ydir,zdir);
    if(a1!=0){
      a2=DotProduct(nx,ny,nz,x,y,z);
      t= -(d+a2)/a1;
      if(t>=0){
        /* ray intersects embedding plane */
        /* now find intersection point */

        xint=x+xdir*t;
        yint=y+ydir*t;
        zint=z+zdir*t;

        /* calculate rotation transformation matrix to x-y plane */

        Vector_rot(nx,ny,nz,0.0,0.0,1.0,rot);

        /* now transform polygon to x-y plane */

        Transformpolygon(pl,numverts,rot);

        /* now transform intersection point to the x-y plane */

        Transformpoint(&xint,&yint,&zint,rot);

        /* map the 3-d to the 2-d array */

        for(l=0;l<numverts;++l){
          dpl[l*2] = pl[l*3];
          dpl[l*2+1] = pl[l*3+1];
        }

        /* calculate whether the intersection point is inside the polygon */

        if(Inside_2D_Polygon(xint,yint,numverts,dpl)){
          if(t==0.0)return(0); /* point is on polytope surface */
          ++intersect;
        }
      }
    }
  }
  if(intersect%2 != 0)return(TRUE);
  return(FALSE);
}

/* the following routine checks to see if a point is inside
   a polygon which can be concave or convex
   x and y are the coordinates of the point to check
   numpoints is the number of verticies on the polygon
   data is the array containing the verticies of the polygon

   data[0]=x0,data[1]=y0,data[2]=x1,data[3]=y1,...,
   data[2*(numpoints-1)]=xn,data[2*(numpoints-1)+1]=yn

   note: the last vertex is not the same as the first vertex.
*/

Inside_2D_Polygon(x,y,numpoints,data)
int numpoints;
float x,y,*data;
{
  int intersect,i,j,k,flag;
  float x1,y1,x2,y2,x3,y4,xlast,ylast;

  intersect=0;
  for(i=0;i<numpoints;++i){
    x1=data[i*2];
    y1=data[i*2+1];
    if(i==(numpoints-1)){
      x2=data[0];
      y2=data[1];
    }
    else{
      x2=data[i*2+2];
      y2=data[i*2+3];
    }
    if(y!=y2){
      if(y2>y1){
        xlast=x1;x1=x2;x2=xlast;
        ylast=y1;y1=y2;y2=ylast;
      }
      if(y>y2&&y<y1){
        if(y1-y2==0){
          x3=x1;
        }
        else{
          x3=x1-(x1-x2)*(y1-y)/(y1-y2);
        }
        /* x3 is the x intercept, if x3 is to the left of x */
        /* then the counter intersect is incremented        */
        if(x3<x) ++intersect;
      }
    }
    else{
      if(x2<x){
        if(y1==y2){
          if(((x1>x)&&(x2<x))||((x1<x)&&(x2>x))){
            ++intersect;
          }
        }
        else{
          flag=TRUE;
          j=i;
          while(flag){
            k=j==numpoints-1?0:j+1;
            y4=data[k*2+3];
            if(y4!=y){
              if(((y1<y)&&(y4>y))||((y1>y)&&(y>y4)))++intersect;
              flag=FALSE;
            }
            else ++j;
          }
        }
      }
    }
  }
  if(intersect%2!=0){
    return(1);
  }
  else return(0);
}


float DotProduct(u1,u2,u3,v1,v2,v3)
float u1,u2,u3,v1,v2,v3;
{
  float dp;

  dp = u1*v1 + u2*v2 + u3*v3;

  return(dp);
}
		       
Parallel_Normal_Vectors(u1,u2,u3,v1,v2,v3)
float u1,u2,u3,v1,v2,v3;
{
  float eps,a,cp1,cp2,cp3;

  eps = 1e-4;

  cp1 = (u2*v3 - u3*v2);
  cp2 = (u3*v1 - u1*v3);
  cp3 = (u1*v2 - u2*v1);

  a = cp1*cp1 + cp2*cp2 + cp3*cp3;

  if(a<eps)return(1);
  return(0);
}

Setident(rotation)
float rotation[4][4];
{

  rotation[0][0] = 1.0; 
  rotation[0][1] = 0.0;
  rotation[0][2] = 0.0;
  rotation[0][3] = 0.0;
  rotation[1][0] = 0.0;
  rotation[1][1] = 1.0;
  rotation[1][2] = 0.0;
  rotation[1][3] = 0.0;
  rotation[2][0] = 0.0;
  rotation[2][1] = 0.0;
  rotation[2][2] = 1.0;
  rotation[2][3] = 0.0;
  rotation[3][0] = 0.0;
  rotation[3][1] = 0.0;
  rotation[3][2] = 0.0;
  rotation[3][3] = 1.0;
  return(1);
}

Plane_normal(pl,numvert,nx,ny,nz)
int numvert;
float *pl,*nx,*ny,*nz;
{
  int i,j;
  float len;

  *nx = 0.0; *ny = 0.0; *nz = 0.0;

/* compute normal vector */

  for(i=0;i<numvert;++i){
    j = (i+1)%numvert;
    *nx += (pl[3*i+1]-pl[3*j+1])*(pl[3*i+2]+pl[3*j+2]);
    *ny += (pl[3*i+2]-pl[3*j+2])*(pl[3*i]+pl[3*j]);
    *nz += (pl[3*i]-pl[3*j])*(pl[3*i+1]+pl[3*j+1]);
  }

/* normalize vector */

  len = sqrt((*nx)*(*nx) + (*ny)*(*ny) + (*nz)*(*nz));
  
  if(len != 0.0){
    *nx /= len;
    *ny /= len;
    *nz /= len;
  }
  return(1);
}

Normalize(xdir,ydir,zdir)
float *xdir,*ydir,*zdir;
{

  float len;
/* normalize vector */

  len = sqrt((*xdir)*(*xdir) + (*ydir)*(*ydir) + (*zdir)*(*zdir));
  
  if(len != 0.0){
    *xdir /= len;
    *ydir /= len;
    *zdir /= len;
  }
  return(1);
}

/* Takes a vector whose direction defines an axis of rotation and whose
   length is the angle of rotation in radians and fills in the corresponding
   (4x4) rotation matrix.  Obtained from a paper by Michael Pique */


Vector_rot(u1,u2,u3,v1,v2,v3,rotation)
float u1,u2,u3,v1,v2,v3,rotation[4][4];
{
  float   s, c, t, a, drot[3], lenu, lenv, eps, dp, pi, dx, dy, dz;
  float tri[9];
  int colinear;

/* normalize u and v */

  pi = 4.0*atan(1.0);

  lenu = sqrt(u1*u1 + u2*u2 + u3*u3);
  lenv = sqrt(v1*v1 + v2*v2 + v3*v3);

  if(lenu>lenv)eps = 1e-5*lenu;
  else eps = 1e-5*lenv;

  dx = u1 - v1; dx = dx<0.0 ? -dx : dx;
  dy = u2 - v2; dy = dy<0.0 ? -dy : dy;
  dz = u3 - v3; dz = dz<0.0 ? -dz : dz;
  if(dx<eps && dy<eps && dz<eps){
    Setident(rotation);
    return(0);
  }

  u1 /= lenu;
  u2 /= lenu;
  u3 /= lenu;

  v1 /= lenv;
  v2 /= lenv;
  v3 /= lenv;

/* calculate cross product of u X v  */
/* remember that length of u X v = sin (angle between u and v) */

  drot[0] = (u2*v3 - u3*v2);
  drot[1] = (u3*v1 - u1*v3);
  drot[2] = (u1*v2 - u2*v1);

/* calculate length of u X v */

  a = sqrt(drot[0] * drot[0] + drot[1] * drot[1] + drot[2] * drot[2]);

  if(a > eps){ /* if this is not true a = sin(180) */

    colinear = 0; /* vectors are not colinear */

/* scale u X v so that its length = angle between u and v */

    drot[0] *= asin(a)/a;
    drot[1] *= asin(a)/a;
    drot[2] *= asin(a)/a;


/* normalize this vector */

    a = sqrt(drot[0] * drot[0] + drot[1] * drot[1] + drot[2] * drot[2]);

    drot[0] /= a;
    drot[1] /= a;
    drot[2] /= a;

  }
  else {
    colinear = 1;
    a = 0.0;
  }

/* if dot product of u and v < 0 then subtract a from 180 degrees (pi) */

  dp = u1*v1 + u2*v2 + u3*v3;
  if(dp<0.0) a = pi - a;

/* if u and v are colinear, must set the arbitrary rotation vector drot */

  if(colinear){

    if(!Parallel_Normal_Vectors(u1,u2,u3,0.0,1.0,0.0)){
      tri[0]=u1;tri[1]=u2;tri[2]=u3;
      tri[3]=v1;tri[4]=v2;tri[5]=v3;
      tri[6]=0.0;tri[7]=1.0;tri[8]=0.0;
      Plane_normal(tri,3,&drot[0],&drot[1],&drot[2]);
    }
    else{
      tri[0]=u1;tri[1]=u2;tri[2]=u3;
      tri[3]=v1;tri[4]=v2;tri[5]=v3;
      tri[6]=1.0;tri[7]=0.0;tri[8]=0.0;
      Plane_normal(tri,3,&drot[0],&drot[1],&drot[2]);
    }

  }

/* calculate transformation matrix */

  s = sin(a);
  c = cos(a);
  t = 1.0 - c;

  rotation[0][0] = t * drot[0] * drot[0] + c;
  rotation[0][1] = t * drot[0] * drot[1] + s * drot[2];
  rotation[0][2] = t * drot[0] * drot[2] - s * drot[1];
  rotation[0][3] = 0.0;
  rotation[1][0] = t * drot[0] * drot[1] - s * drot[2];
  rotation[1][1] = t * drot[1] * drot[1] + c;
  rotation[1][2] = t * drot[1] * drot[2] + s * drot[0];
  rotation[1][3] = 0.0;
  rotation[2][0] = t * drot[0] * drot[2] + s * drot[1];
  rotation[2][1] = t * drot[1] * drot[2] - s * drot[0];
  rotation[2][2] = t * drot[2] * drot[2] + c;
  rotation[2][3] = 0.0;
  rotation[3][0] = 0.0;
  rotation[3][1] = 0.0;
  rotation[3][2] = 0.0;
  rotation[3][3] = 1.0;
  return(1);
}

Transformpolygon(poly,count,mm)
int count;
float *poly,mm[4][4];
{
  int i;
  float xtemp,ytemp,ztemp;

  for(i=0;i<count;++i){
    xtemp=poly[i*3]*mm[0][0]+poly[i*3+1]*mm[1][0]+poly[i*3+2]*mm[2][0]+mm[3][0];
    ytemp=poly[i*3]*mm[0][1]+poly[i*3+1]*mm[1][1]+poly[i*3+2]*mm[2][1]+mm[3][1];
    ztemp=poly[i*3]*mm[0][2]+poly[i*3+1]*mm[1][2]+poly[i*3+2]*mm[2][2]+mm[3][2];
    poly[i*3] = xtemp; poly[i*3+1] = ytemp; poly[i*3+2] = ztemp;
  }
  return(1);
}


Transformpoint(x,y,z,mm)
float *x,*y,*z,mm[4][4];
{
  float xtemp,ytemp,ztemp;

  xtemp=(*x)*mm[0][0]+(*y)*mm[1][0]+(*z)*mm[2][0]+mm[3][0];
  ytemp=(*x)*mm[0][1]+(*y)*mm[1][1]+(*z)*mm[2][1]+mm[3][1];
  ztemp=(*x)*mm[0][2]+(*y)*mm[1][2]+(*z)*mm[2][2]+mm[3][2];
  *x = xtemp; *y = ytemp; *z = ztemp;
  return(1);
}




Matrixmult44(b,c)
float b[4][4],c[4][4];
{
  register i,j,k;
  float temp[4][4];

/*   ZERO array temp  */

  for(i=0;i<=3;++i)
    for(j=0;j<=3;++j) temp[i][j] = 0.0;

/*  Do matrix multiplication  */

  for(i=0;i<4;++i){
    for(j=0;j<4;++j){
      for(k=0;k<4;++k)temp[i][j] += b[i][k]*c[k][j];
    }
  }

/* assign b to temp matrix */

  for(i=0;i<=3;++i)
    for(j=0;j<=3;++j) b[i][j]=temp[i][j];
  return(1);
}