ขั้นตอนวิธีแบบกรวย
รหัสในภาษาซี > ฟังก์ชันต่างๆ

void readConf(){
  cin >> num;
  char s[1024];
  cin.getline( s, 1024 );       // comment line
  cin.getline( s, 1024 );
  d =new int[num];
  list = new int[num];
  for( int i=0; i<num; i++ ){
    d[i] = 0;
    list[i] = -1;
  }
  q = new Vector[num];
  for( int i=0; i<num; i++ ) cin >> s >> q[i];
}
void initCell(){
  // Create cell array
  Vector min( q[0].x, q[0].y, q[0].z );
  Vector max( min );
  for( int i=1; i<num; i++ ){
    if( q[i].x < min.x ) min.x = q[i].x;
    if( q[i].x > max.x ) max.x = q[i].x;
    if( q[i].y < min.y ) min.y = q[i].y;
    if( q[i].y > max.y ) max.y = q[i].y;
    if( q[i].z < min.z ) min.z = q[i].z;
    if( q[i].z > max.z ) max.z = q[i].z;
  }
  nx = (int)ceil( (max.x-min.x)/bin );
  ny = (int)ceil( (max.y-min.y)/bin );
  nz = (int)ceil( (max.z-min.z)/bin );
  cell = new int **[nx];
  curr = new int **[nx];
  for( int i=0; i<nx; i++ ){
    cell[i] = new int *[ny];
    curr[i] = new int *[ny];
    for( int j=0; j<ny; j++ ){
      cell[i][j] = new int[nz];
      curr[i][j] = new int[nz];
    }
  }
  // Locate particles into cells
  for( int i=0; i<nx; i++ )
    for( int j=0; j<ny; j++ )
      for( int k=0; k<nz; k++ ){
         cell[i][j][k] = -1;
         curr[i][j][k] = -1;
        }
  for( int i=0; i<num; i++ ){
    Vector vi = (q[i] - min) / bin;
    int x = (int)vi.x;   int y = (int)vi.y;   int z = (int)vi.z;
    if( x>=nx || y>=ny || z>=nz ||x<0 || y<0 || z<0 ){
      cerr << "Out of boundary: " << endl;
      cerr << "ix=" << x << " iy=" << y << " iz=" << z << endl;
      cerr << "bin=" << bin << endl;
      cerr << "x[i]=" << q[i].x << " y[i]=" << q[i].y
	   << " z[i]=" << q[i].z << endl;
      cerr << "xmin=" << min.x << " ymin=" << min.y
	   << " zmin=" << min.z << endl;
      exit( 1 );
     }
    if( cell[x][y][z] == -1 ) {  // add the head of the cell chain
      cell[x][y][z] = curr[x][y][z] = i;
    }
    else {  // add an element at the tail of the chain and move the pointer
      list[ curr[x][y][z] ] = i;
      curr[x][y][z] = i;
    }
  }
}
void end(){
  delete []q; delete []d;   delete []list;
  for( int i=0; i<nx; i++ ){
    for( int j=0; j<ny; j++ ) {
      delete [](cell[i][j]);
      delete [](curr[i][j]);
    }
    delete [](cell[i]);
    delete [](curr[i]);
   }
  delete []cell;
  delete []curr;
 }
void writeXYZ(){
  cout << num << endl << endl;
  for( int i=0; i<num; i++ ){
    if( d[i]== -1 ) cout << "Au ";
    else if( d[i]==1 ) cout << "Cd ";
    else cout << "Ag ";
    cout << q[i] << endl;
  }
}
void markCell(){
  for( int i=2; i<nx-2; i++ )
    for( int j=2; j<ny-2; j++ )
      for( int k=2; k<nz-2; k++ ){
	if( cell[i][j][k] != -1 &&
	    cell[i-2][j][k] != -1 && cell[i+2][j][k] != -1 &&
	    cell[i][j-2][k] != -1 && cell[i][j+2][k] != -1 &&
	    cell[i][j][k-2] != -1 && cell[i][j][k+2] != -1 &&
	    cell[i-1][j-1][k-1] != -1 && cell[i-1][j-1][k+1] != -1 &&
	    cell[i-1][j+1][k-1] != -1 && cell[i-1][j+1][k+1] != -1 &&
	    cell[i+1][j-1][k-1] != -1 && cell[i+1][j-1][k+1] != -1 &&
	    cell[i+1][j+1][k-1] != -1 && cell[i+1][j+1][k+1] != -1 ){
	  // mark the particles in this cell to be internal
	  int p = cell[i][j][k];
	  while( p != -1 ){
	    d[ p ] = -1;
	    p = list[p];
	  }
	  // mark this cell to be checked to avoid further investigation
	  curr[i][j][k] = -1;
	}
      }
}
void markSurface(){
  // First round quick check for surface particles by center-of-mass normal
  // vector criterion.
  for( int i=0; i<nx; i++ )
    for( int j=0; j<ny; j++ )
      for( int k=0; k<nz; k++ ) {
	if( curr[i][j][k] != -1 ){ // -1 means either no particles or checked
	                          // to be all internal
	  int p = cell[i][j][k];
	  while( p != -1 ) {
	    d[p] = cmCheck( p, i, j, k );
	    p = list[ p ];
	  }
	}
      }
  // Complete searching for a satisfied cone region to vanish all the
  // undetermined particles.
  for( int i=0; i<nx; i++ )
    for( int j=0; j<ny; j++ )
      for( int k=0; k<nz; k++ ){
	if( curr[i][j][k] != -1 ){ // -1 means either no particles or checked
	                          // to be all internal
	  int p = cell[i][j][k];
	  while( p != -1 ){
	    if( d[p] == 0 )  {  // undermined particles
	      d[p] = realCheck( p, i, j, k );
	    }
	    p = list[ p ];
	  }
	  curr[i][j][k] = -1;    // this cell finished
	}
      }
}
int cmCheck( const int p, const int i, const int j, const int k ){
// Loop over all particles in this cell and the neighboring cells
  for( int ni=i-1; ni<=i+1; ni++ ){
    if( ni<0 || ni>=nx ) continue;
    for( int nj=j-1; nj<=j+1; nj++ ){
      if( nj<0 || nj>=ny ) continue;
      for( int nk=k-1; nk<=k+1; nk++ ){
	if( nk<0 || nk>=nz ) continue;
	// internal particles are definitely out of the cone region
	if( curr[ni][nj][nk] == -1 ) continue;
	int np = cell[ni][nj][nk];
	while( np != -1 ){
	  if( cmCone( p, np ) == 1 ) return 0;
	  np = list[ np ];
	}
      }
    }
  }
  return 1;        // surface particle
}
int cmCone( const int p, const int np ){
  return cone( q[p].normalize(), p, np );
}
//-------------------------- function cone---------------------------
// Check if a given particle np is in the cone region centered at particle p
// with center vector of (nx,ny,nz) and cone angle theta
// with cos(theta)=cosine. Cone center vector must be normalized.
// Return 0 if the particle is not inside the region(s), 1 if in the cone
// region, -1 if in the inverse cone region.
int cone( const Vector &v, const int p, const int np ){
  Vector d = (q[np] - q[p]).normalize();
  double s = v*d;
  if( s > cosine ) return 1;    // less than the angle
  else if( s<-cosine ) return -1;
  else return 0;
}
int realCheck( const int p, const int i, const int j, const int k ){
  int *nbr = new int[1000];   // list of neighboring particles
  int nnbr=0;    // number of neighbor particles
  int *snbr = new int[1000];  // neighboring surface particles
  int snnbr = 0;
  int *unbr = new int[1000];  // neighboring undetermined particles
  int unnbr = 0;
  // Loop over all particles in this cell and the neighboring cells to make
  // a list of neighboring particles.
  for( int ni=i-1; ni<=i+1; ni++ ) {
    if( ni<0 || ni>=nx ) continue;
    for( int nj=j-1; nj<=j+1; nj++ ){
      if( nj<0 || nj>=ny ) continue;
      for( int nk=k-1; nk<=k+1; nk++ ){
	if( nk<0 || nk>=nz ) continue;
	int np = cell[ni][nj][nk];
	while( np != -1 ){
	  if( np != p )  {
	    // check if np is inside the cutoff region
	    Vector v = q[np] - q[p];
	    if( v.r2() < cutoff2 ) {
	      nnbr++;
	      nbr[ nnbr-1 ] = np;
	      if( d[np] == 1 ) { // surface particle
		snnbr++;
		snbr[ snnbr-1 ] = np;
	      }
	      else if( d[np] == 0 ) // undertermined particle
	      {
		unnbr++;
		unbr[ unnbr-1 ] = np;
	      }
	    }
	  }
	  np = list[ np ];
	}
      }
    }
  }
  if( nnbr <= 3 ) return 1;   // surface particle
  // Check the combinations of 3 neighboring particles to see if they span
  // a hollow cone region.
  // Internal particles are excluded from the loop of the combinations.
  // Combinations made by surface particles are checked first to improve the
  // chance of hitting a target earlier.
  // 3 surface particles
  for( int i=0; i<snnbr-2; i++ )
    for( int j=i+1; j<snnbr-1; j++ )
      for( int k=j+1; k<snnbr; k++ )
	if( realCone( p, snbr[i], snbr[j], snbr[k], nbr, nnbr ) ){ // hollow
	  delete []nbr; delete []snbr; delete []unbr;
	  return 1;   // surface particle
	}
  // 2 surface particles, 1 undetermined particle
  for( int i=0; i<snnbr-1; i++ )
    for( int j=i+1; j<snnbr; j++ )
      for( int k=0; k<unnbr; k++ ){
	if( realCone( p, snbr[i], snbr[j], unbr[k], nbr, nnbr ) ){
	  delete []nbr; delete []snbr; delete []unbr;
	  return 1;
	}
      }
  // 1 surface particle, 2 undetermined particles
  for( int i=0; i<snnbr; i++ )
    for( int j=0; j<unnbr-1; j++ )
      for( int k=j+1; k<unnbr; k++ ){
	if( realCone( p, snbr[i], unbr[j], unbr[k], nbr, nnbr ) ){
	  delete []nbr; delete []snbr; delete []unbr;
	  return 1;
	}
      }
  // 3 undetermined particles
  for( int i=0; i<unnbr-2; i++ )
    for( int j=i+1; j<unnbr-1; j++ )
      for( int k=j+1; k<unnbr; k++ )
	if( realCone( p, unbr[i], unbr[j], unbr[k], nbr, nnbr ) ) {// hollow
	  delete []nbr; delete []snbr; delete []unbr;
	  return 1;   // surface particle
	}
  delete []nbr; delete []snbr; delete []unbr;
  return -1;   // internal particle
}
bool realCone( const int p, const int np1, const int np2,
	       const int np3, const int *nbr, const int nnbr ){
  Vector v;
  // Find the center vector of the cone spanned by ( np1, np2, np3 )
  // centered at p.
  if( !coneVector( p, np1, np2, np3, v ) ) return false;
  // If any particles inside the cone region and the inverse cone region.
  bool has = false;
  bool hasInv = false;
  for( int m=0; m<nnbr; m++ ){
    int nn = nbr[m];
    if( nn==np1 || nn==np2 || nn==np3 ) continue;
    int h = cone( v, p, nn );
    if( h == 1 ) {
      has = true;
      if( hasInv ) break;
    }
    else if( h==-1 ) {
      hasInv = true;
      if( has ) break;
    }
  }
  if( !has || !hasInv ) return true;   // there is a hollow cone
  else return false;
}
bool coneVector( const int p, const int np1, const int np2, const int np3,
		 Vector &v ){
  const Vector v1 = (q[np1] - q[p]).normalize();
  const Vector v2 = (q[np2] - q[p]).normalize();
  const Vector v3 = (q[np3] - q[p]).normalize();
  // it's the normal vector of plane spanned by v1, v2 and v3
  v = (v1-v2)^(v2-v3);
  // check if three points are on one line
  double r = sqrt( v.r2() );
  if( r < TINY )    return false;
  v = v/r;
  if( fabs( v*v1 ) > cosine ) // smaller angle
    return false;
  return true;
}