www.pudn.com > TRICUT.zip > TRICUT.CXX, change:2001-08-29,size:72095b


// CH. LINDENBECK, H. D. EBERT, H. ULMER, L. PALLOZZI LAVORANTE, and R. PFLUG 
// TRICUT: A PROGRAM TO CLIP TRIANGLE MESHES USING  
// THE RAPID AND TRIANGLE LIBRARIES AND THE VISUALIZATION TOOLKIT 
 
#include "vtk.h"
#include <stdlib.h>
#include <math.h>
#include <float.h>
#include "./RAPID.H"


#define EPS 0.00001 	   // Tolerance for Line-Triangle intersection
#define PTOL 0.001	   // Tolerance for pcoords computations
#define RTOD 57.2957795131 // Conversion Factor from radians to degrees

// These declarations are necessary for the triangle library (constrained Delaunay triangulation)
#define REAL double
#include "triangle.h"


void Normalise (double *p);  // Function to normalise a vector
int line_tri(double *p1, double *p2, double *pa, double *pb, double *pc, double *p);
void feed_point (double *out, float *in); // converts a point from float to double

int cross_polyline (float *p1, float *p2, float *pa, float *pb);

void shellsort (int *x, int n, int *incrmnts, int numinc); // Sorting functions
void fshellsort (float *x, int n, int *incrmnts, int numinc);

int bin_search (int key, int *x, int n); // Binary search functions
int fbin_search (float key, float *x, int n);

typedef short int SHORT;

// The next two structures are used to link each intersecting triangles from surf2
// with all the intersection points which lie on it. Each triangle has a <cell_type> struct
// associated with it. There are some nodes accessible from the pointers <first> and <last>.
// Each node contains the identification number (id) of the corresponding intersection point.
// This way, we are able to detrmine a set of points for each triangle (including its vertices)
// for further triangulation.

struct nodetype {
        SHORT id; // Use unsigned int type if your short ints are one byte sized
        struct nodetype *next;
  };
typedef struct nodetype *nodeptr;

struct celltype {
        nodeptr last;
        nodeptr first;
  };
typedef struct celltype *cellptr;

// The following is a structure used to chain the intersection points together
// See ahead for more informations
struct inter_node_type {
      char visited;
      SHORT next1, next2;
      char num_tris; // Number of surf2's triangles which contain the point
};
typedef struct inter_node_type *inode_ptr;

inode_ptr inode; // pointer to the structure above




int add_node (cellptr cell, int ptid);  // Add a node to a <celltype> struct
void show (cellptr cell);		// Shows all the nodes cointained in a <celltype> struct

void delete_marked_cells (vtkPolyData *poly); // Remove cells marked as deleted from a polydata

// The next function accepts a polydata as input and returns in <out_poly> the same set of triangles
// but it remove first the unused points.


void points_info (char *s1, char *s2, vtkIdList *plist, vtkIdList *polylist);
void remove_intersection_point (int pos);
void show_help (void);
void sort_lists (vtkIdList *idlist, vtkFloatScalars *clist, int n, int *incrmnts, int numinc);

float get_angle (float *x1, float *x2, float *x3);

int diag_factor = 10000000;
short int node_number=0, node_fault=0;
char debug = 0;
char bad_flag = 1; // 1 if bad new triangles must be removed
char reduction = 1; // 1 if point reduction is allowed

int inter_num;     // Number of intersection points
int *order;        // Dinamic array to store intersection points's ids (for binary search)

int *red_order;    // The same as before, but invalid intersection points are removed
int *red_pos;      // For each red_order point stores its position in allList
int *red_points;   // Temporarily stores intersection points for each intersecting cell

vtkPolyData *surf1; // First polygonal surface. No topological modification will be made on it.
vtkPolyData *surf2; // Second surface. Parts of it will be removed if users wants

float z_translation = 0.0; // Initial translation in z direction for surf1

// vtkIdList *point_list;     // List containing the ids of all intersection points
   vtkIdList *allList;



char *helptxt[] = {

"----------------------------------------------------------------------------",
"                                                                            ",
"  NAME                                                                      ",
"    tricut - computes intersection points betweeen two dis files            ",  
"                                                                            ",
"  USAGE                                                                     ",
"    tricut <surf1> <surf2> -d diag_factor -t z_translation -debug -a ANGLE  ",
"                                                                            ",
"    <surf1> : topography file (shown in gray)                               ",
"    <surf2> : fault file (shown in purple)                                  ",
"    -d diag_factor : specifies diagonal factor for bad intersection points  ",
"                     removal (distance < bound_diagonal/diag_factor, where  ", 
"                     bound_diagonal is the length for surf1 volume's diag). ",
"                     The default diag_factor is 1000.                       ",
"    -t z_translation :  translate the topography z_translation units in the ",
"                        positive z direction. Default translation is 0.0    ",
"    -a ANGLE         :  defines minimum allowed internal angle for new      ",
"                        triangles created after triangulation process       ", 
"    -debug  : show informations while processing                            ",
"                                                                            ",
"  The program creates two temporary files, surf2.dis and tricut.tmp, which  ",
"  are used by the <cutter> program.                                         ",
"----------------------------------------------------------------------------",
"  AUTHOR                                                                    ",
"    Luca Pallozzi Lavorante                                                 ",
"    lucapl@rc.unesp.br                                                      ",
"                                                                            ",
"----------------------------------------------------------------------------",
"!"
};

// ************************************************
// Begin of main() function
// ************************************************


main (int argc, char *argv[])
{
  FILE *f;
  char aux[20]; 
  int aux1, aux2, err;  
  char *answ[] = {"no", "yes"};
  char choice;
  double pa[3], pb[3], pc[3], p1[3], p2[3], p[3];
  
  int ncells1=0, ncells2=0;
  float x1[3][3], x2[3][3];
  float xa[3], xb[3], xc[3];   
  int i,j,k; 
  int pos;
  int inter[5] = {0, 0, 0, 0, 0};       // Controls the n. of intersections for each pair  
					          // of triangles (tr1, tr2)
  float x[3]={0, 0, 0}; 		    // intersection point between line and triangle

  double R1[3][3] = {{1.0, 0.0, 0.0},   // Rotation Matrix used by RAPID models 
		     {0.0, 1.0, 0.0},
		     {0.0, 0.0, 1.0}}; 
  double T1[3] = {0.0, 0.0, 0.0};       // Translation Vector used by RAPID models
  double R2[3][3] = {{1.0, 0.0, 0.0},   // Rotation Matrix used by RAPID models
                     {0.0, 1.0, 0.0},
                     {0.0, 0.0, 1.0}};
  double T2[3] = {0.0, 0.0, 0.0};       // Translation Vector used by RAPID models
 
  double X[3][3]; 			    // Three double precision points for RAPID library

  vtkCell *cell1, *cell2;
  int cell1Id, cell2Id;
  char a;

  int inter_pairs = 0; 	 // Number of intersecting pairs of triangles
  float ip_pts[4][3];  	 // Array of x-y-z intersection points for a triangle pair (tr1, tr2)
  int int2;            	 // Number of intersecting triangles from surf2
  int *cell_ids;       	 // array containing these triangles' ids in ascending order 	
  
  // The following is a struct containing the Ids of intersection points in the
  // vtkFloatPoints object used by the locator. 
  // There is such a struct for each pair of triangles.  
  struct ip_ptids_type  { 
	short int id[3];       
  };			
  typedef struct ip_ptids_type *ip_ptids_ptr;

  ip_ptids_ptr ip_ptids; // pointer to the structure above

  int ct;   		 // counter of the number of intersection points for each pair
  int id1, id2; 	       // id's in the intersection points' locator for an edge  

  int head = -1;  
  int current, ant, next;

  int lines_num;  	 // number of determined open polygonal lines
  int loops_num;  	 // idem, for closed polygonal lines
  int pl_num;      	 // number of all lines
  int visited_ct; 	 // counter for visited intersection points
  char is_loop;   	 // 1 if the actual polyline is closed, 0 otherwise
  int npts;       	 // number of points in a polyline or triangle
 

  int numinc = 3; 	 // number of increments for shellsort function
  int incrmnts[3] = {5, 3, 1};

  int cell2_pos; 

  cellptr cell_list; // Pointers used for <cell_list> traversal 
  nodeptr pnode, qnode;

  int new_inter_num;

  // Parametric Coordinates variables
  float closestPoint[3] = {0.0, 0.0, 0.0};
  float pcoords[2] = {0.0, 0.0};
  float sum;
  float weights[3] = {0.0, 0.0, 0.0};
  float dist2 = 0.0;
  int subId = 0;

  float bg_color[3] = {0.0, 0.0, 0.0};
  float s1_color[3] = {0.9, 0.9, 0.9};
  float s2_color[3] = {0.9, 0.6, 0.9};

  // Declarations for the triangulation function

  int nsegs;		// Number of constrained non-boundary segments for each intersecting triangle
  int nbo_segs;		// Number of constrained boundary segments
  int num_tris;		// Number of new triangles for each triangulated intersecting triangle
  struct triangulateio tr_in, tr_out, *vorout; // structures used to exchange data with the
				      		// triangulation function 
  char s_npts, t_npts, st_npts;
  float faux;
  float *p_order;
  int *p_ids;
  float angle1, angle2;
  float min_angle = 0.05;
  int bad = 0;
  int ct_aux;


  if (argc < 3) { // If less than two arguments in command line
		  // Show help
	show_help();
	return (0);
  }


  ct = 3;
  while (ct<argc)  { 

	if (!strcmp (argv[ct], "-debug"))  // show informations while processing
		debug = 1;

	if (!strcmp (argv[ct], "-t") && argv[ct+1]) { // define surf1's translation in z direction
		z_translation = atof (argv[ct+1]);
		ct++;
	}

	if (!strcmp (argv[ct], "-d") && argv[ct+1]) { // define diagonal factor for inter. points reduction
		diag_factor = atoi (argv[ct+1]);
		ct++;
	}

	if (!strcmp (argv[ct], "-noreduct")) { // overrides point reduction
                reduction = 0;

        }

	if (!strcmp (argv[ct], "-nobad")) { // overrides bad triangles' checking
                bad_flag = 0;
                
        }

	if (!strcmp (argv[ct], "-a") && argv[ct+1]) { // define minimum allowed angle in new surf2's triangles
                min_angle = atof (argv[ct+1]);
                ct++;
        }

  	ct++;
  }
  printf ("\nCurrent surf1's z-translation : %f\n", z_translation);
  printf ("Point reduction: %s\n", answ[reduction]);
  if (reduction)
  	printf ("\tCurrent diag_factor: %i\n", diag_factor);
  printf ("Minimum allowed angle in new triangles: %f\n", min_angle);
  printf ("Bad triagles removal: %s\n", answ[bad_flag]);
  printf ("\nDebug : %s\n", answ[debug]);

  vtkBYUReader *reader1 = vtkBYUReader::New();
  	reader1->SetGeometryFileName (argv[1]);
        reader1->Update();
  vtkBYUReader *reader2 = vtkBYUReader::New();
  	reader2->SetGeometryFileName (argv[2]);
        reader2->Update();


  vtkTransform *transform = vtkTransform::New(); // Set surf1's Traslation trasformation
	transform->Translate (0.0, 0.0, z_translation);

  vtkTransformPolyDataFilter *translator = vtkTransformPolyDataFilter::New();
	translator->SetInput(reader1->GetOutput());
	translator->SetTransform(transform);
		transform->Delete();
	translator->Update();
  surf1 = vtkPolyData::New(); 
  	surf1->CopyStructure(translator->GetOutput());
  reader1->Delete();

  surf2 = vtkPolyData::New();
  surf2->CopyStructure(reader2->GetOutput());
  surf2->Update();

  reader2->Delete();
  translator->Delete();

  ncells1=surf1->GetNumberOfCells();
  ncells2=surf2->GetNumberOfCells();

  printf ("surf1: %s\n", argv[1]); 
  printf ("	Number of cells: %d\n", ncells1); 
  printf ("	Number of points: %d\n", surf1->GetNumberOfPoints());
  printf ("surf2: %s\n", argv[2]);
 
  printf ("     Number of cells: %d\n", ncells2);
  printf ("     Number of points: %d\n", surf2->GetNumberOfPoints());





  // ***************************************************************************************
  //
  // D E T E R M I N I N G   I N T E R S E C T I N G   T R I A N G L E S '   P A I R S
  //
  // ***************************************************************************************


 

  // Feeding RAPID library with first surface
  RAPID_model *model1 = new RAPID_model;
  model1->BeginModel();

  for (i=0; i<ncells1; i++) {   	      // retrieves vertices of ith surf1's triangle
	cell1 = surf1->GetCell(i);
        cell1->GetPoints()->GetPoint(0, x1[0]);    
        cell1->GetPoints()->GetPoint(1, x1[1]);
        cell1->GetPoints()->GetPoint(2, x1[2]);
	
        for (j=0; j<3; j++)     	      // Make casting from float to double
            for (k=0; k<3; k++) 
	        X[j][k] = (double) x1[j][k];		 
            
        model1->AddTri( X[0], X[1], X[2], i); // Adding current triangle to <model1>      
  }
  model1->EndModel();

  // Feeding RAPID library with second surface
  RAPID_model *model2 = new RAPID_model;
  model2->BeginModel();

  for (i=0; i<ncells2; i++) {   	      // retrieves vertices of ith surf2's triangle
        cell2 = surf2->GetCell(i);
        cell2->GetPoints()->GetPoint(0, x1[0]);
        cell2->GetPoints()->GetPoint(1, x1[1]);
        cell2->GetPoints()->GetPoint(2, x1[2]);
       
        for (j=0; j<3; j++) 		      // Make casting from float to double
            for (k=0; k<3; k++)         
                X[j][k] = (double) x1[j][k];            

        model2->AddTri( X[0], X[1], X[2], i); // Adding current triangle to <model2>
  }
  model2->EndModel();


  // Determining the intersection triangle pairs (tr1, tr2) between model1 and model2

  if (RAPID_Collide(R1, T1, model1, R2, T2, model2, RAPID_ALL_CONTACTS))
	return 1; // exits the program if the function returns error (not zero)

  printf ("\nNumber of overlapping (old) triangles: %d\n", RAPID_num_contacts);

  inter_pairs = RAPID_num_contacts; // Number of intersecting pairs of triangles 

  if (inter_pairs == 0) {
	printf ("No intersection!\n");
	return (1);
  }

  if (debug) {
 	  for (i=0; i<inter_pairs; i++)
		printf ("Contact %3d : %3d  and  %3d\n", i, RAPID_contact[i].id1, RAPID_contact[i].id2); 
  }

  // *********************************************************************************************
  // Creating a special structure to link each cell of the second surface with
  // all the intersection points it contains. This is for further triangulation of the cells.
  // The structure is called <cell_list> and is an array of linked lists. There is a linked list
  // for each intersection triangle in surf2. The number of these triangles is <int2>.
  // Each linked list is addressed using a <cell_type> structure.
  // Each structure contains two pointers:
  //
  // <first> : points to the first inserted intersection point id - used for traversal purpose
  //
  // <last>  : points to last inserted node - used to efficiently add new nodes (ids)
  //
  // Each structure will address the ids of the intersection points which lie on the
  // corresponding triangle. This will be used to know which points must be used to triangulate
  // the triangle.
  // An array of surf2's intersecting triangles ids (<cell_ids>) is created and therefore sorted to
  // allow binary search for a triangle id and, using the position of this id in the array, locate 
  // (in the <cell_list> structures array) the correct <first> and <last> pointers
  // *********************************************************************************************

  // Create temporary list to store surf2's intersecting triangles (cells)
  vtkIdList *celltemp = vtkIdList::New();
  for (i=0; i< inter_pairs; i++) {
	id2 = RAPID_contact[i].id2;
	if (!celltemp->IsId(id2))
		celltemp->InsertNextId(id2);
  }
  

  int2 = celltemp->GetNumberOfIds(); // gets the number of surf2's intersecting cells
  printf ("Number of intersecting cells in surf2: %d\n", int2);

  cell_ids = (int *) malloc (int2 * sizeof(int)); // create surf2's intersecting cells ids array
  // cell_ids = new int[int2];

  if (cell_ids == NULL) {
	printf ("cell_ids : unable to allocate memory!\n");
	return (1);
  }  


  for (i=0; i<int2; i++) 	// Insert cell's ids into <cell_ids>
	cell_ids[i] = celltemp->GetId(i);
  
  celltemp->Reset();
  celltemp->Squeeze();
  celltemp->Delete();

  shellsort (cell_ids, int2, incrmnts, numinc); // Sort <cell_ids> in ascending order


  // Create <cell_list> array of (first, last) pointers' pairs
  // <int2> is the number of pairs in the array

  cell_list = (cellptr) malloc (int2 * sizeof (struct celltype));
  // cell_list = new struct celltype[int2];

  for (i=0; i<int2; i++)
        cell_list[i].first = NULL;



  // *************************************************************************************
  //
  // D E T E R M I N I N G   A N D   S T O R I N G   I N T E R S E C T I O N   P O I N T S 
  //
  // *************************************************************************************

	// The following list stores ids of intersection points which were determined considering
	// intersection tests between surf1's triangles and surf2's triangle edges. This is to know
	// which intersection point lie on surf2's edges, which is important to remove bad points
	// before performing the neighbor test for part selection
	vtkIdList *inter_aux = vtkIdList::New();
	

  // Creating a new array for surf2's points coordinates

  vtkFloatPoints *newpoints2 = vtkFloatPoints::New();
	


  // Create a vtkMergePoints object (locator) to store points' coordinates avoiding duplications
  // The locator must be initialized with surf1's Bounding Box and a vtkFloatPoints object
  // which will fisically store the points' coordinates


  vtkMergePoints *locator = vtkMergePoints::New();
        locator->SetNumberOfPointsPerBucket(25);
       	locator->InitPointInsertion(newpoints2, surf2->GetPoints()->GetBounds());
	locator->SetTolerance (0.0); // Eliminate exactly coincident points; 


  // Making a copy of surf2's points array into <newpoints2>


  npts = surf2->GetPoints()->GetNumberOfPoints();

  for (i=0; i<npts; i++) {
	surf2->GetPoints()->GetPoint(i, x);
	locator->InsertNextPoint(x);
  }

  // Deleting old surf2's points coordinates array

  ((vtkFloatPoints *) surf2->GetPoints())->Reset();
  surf2->GetPoints()->Squeeze();

  surf2->SetPoints(newpoints2);
	newpoints2->Delete();
  surf2->Update();

  // At this point surf2 has the same points as before, but now we are ready
  // to add the intersection points using the locator


  // Create and initialize the ip_ptids array to contain the Ids of intersection
  // points for each pair of triangles. These Ids refer to the <point_list> object

  ip_ptids = (ip_ptids_ptr) malloc (inter_pairs * sizeof (struct ip_ptids_type));
  // ip_ptids = new struct ip_ptids_type[inter_pairs]; 


  if (ip_ptids == NULL) {
	printf ("*** inter_pair_pt_ids: memory allocation error!\n");
	return (1);
  }
  for (i=0; i<inter_pairs; i++)  
	for (j=0; j<3; j++)
		ip_ptids[i].id[j] = -1; // null ids

  // Creating a list to store the intersection points' ids

  vtkIdList *point_list = vtkIdList::New();

  model1->BeginModel();
  model2->BeginModel();
  delete model1; // We don't need the RAPID objects any more
  delete model2;



  // Now we start traversing each pair  (tr1, tr2) of intersecting triangles 
 
  for (i=0; i<inter_pairs; i++) {

	cell1Id = RAPID_contact[i].id1;  // Get tr1's id
	cell2Id = RAPID_contact[i].id2;  // Get tr2's id

	cell1 = surf1->GetCell(cell1Id); // vtkCell pointer to tr1
        cell2 = surf2->GetCell(cell2Id); // vtkCell pointer to tr2
	        
	// Part1: cell1 treated as a triangle 
        //        cell2 treated as a set of three segments

 	ct =0; // the counter of intersection points for this pair


        // Retrieves tr1's points in x1[3][3]

	cell1->GetPoints()->GetPoint(0, x1[0]);
       	cell1->GetPoints()->GetPoint(1, x1[1]);
       	cell1->GetPoints()->GetPoint(2, x1[2]);

	// Retrieves tr2's points in x2[3][3]

	cell2->GetPoints()->GetPoint(0, x2[0]);
        cell2->GetPoints()->GetPoint(1, x2[1]);
        cell2->GetPoints()->GetPoint(2, x2[2]);

	if (debug) {
		printf ("Contact : %d\n", i);
		printf ("Cell1: %5d -- Segmented Cell2 : %5d\n", cell1Id, cell2Id);
        }
 
	// Makes Intersection Test between cell1 and cell2's edges 

	feed_point (pa, x1[0]); // Makes the casting from float to double
	feed_point (pb, x1[1]); // for the line_tri function
	feed_point (pc, x1[2]); // pa, pb and pc are cell1's vertices

	feed_point (p1, x2[0]); // Idem, p1 and p2 are points of  1st cell2's edge 
	feed_point (p2, x2[1]);

	if (line_tri(p1, p2, pa, pb,pc, p)) {
 		 	
		 for (k=0; k<3; k++) 	// Stores (ct+1)th intersection point
   	       	        ip_pts[ct][k] = (float) p[k]; 
	         ct++;	
        }

	feed_point (p1, x2[1]); // Second test: second edge of cell2 with cell1
        feed_point (p2, x2[2]);

	if (line_tri(p1, p2, pa, pb,pc, p)) {
                        
                 for (k=0; k<3; k++)     
                        ip_pts[ct][k] = (float) p[k];
                 ct++;   
        }
	

	feed_point (p1, x2[0]); // Third test: third edge of cell2 with cell1 
        feed_point (p2, x2[2]);

	if (line_tri(p1, p2, pa, pb,pc, p)) {

                 for (k=0; k<3; k++)
                        ip_pts[ct][k] = (float) p[k];
                 ct++;
        }
	
        
	// Makes Intersection test between cell1's edges and cell2

	ct_aux = -1;

	if (debug)
		printf ("Segmented Cell1: %5d -- Cell2 : %5d\n", cell1Id, cell2Id);

	feed_point (pa, x2[0]); // Get cell2's vertices in pa, pb and pc 
        feed_point (pb, x2[1]);
        feed_point (pc, x2[2]);

        feed_point (p1, x1[0]); // Fourth test: first edge of cell1 with cell2
        feed_point (p2, x1[1]);

	if (line_tri(p1, p2, pa, pb,pc, p)) {

                 for (k=0; k<3; k++)
                        ip_pts[ct][k] = (float) p[k];
		 if (ct_aux == -1) 
			ct_aux = ct;
                 ct++;
                }

        feed_point (p1, x1[1]); // Fifth test: second edge of cell1 with cell2
        feed_point (p2, x1[2]);

	if (line_tri(p1, p2, pa, pb,pc, p)) {

                 for (k=0; k<3; k++)
                        ip_pts[ct][k] = (float) p[k];
		 if (ct_aux == -1)
                        ct_aux = ct;

                 ct++;
        }

        feed_point (p1, x1[0]); // Sixth test: third edge of cell1 with cell2        
        feed_point (p2, x1[2]);

	if (line_tri(p1, p2, pa, pb,pc, p)) {

          	 for (k=0; k<3; k++)
                	ip_pts[ct][k] = (float) p[k];
		 if (ct_aux == -1)
                        ct_aux = ct;

                 ct++;
        }

	if (debug)
		printf ("********* Intersections : %d\n", ct);
        inter[ct]++;

	// At this point, ct contains the number of detected intersections for the pair
	// and ip_pts these points' coordinates
	if (debug)
		for (j=0; j<ct; j++)
			printf ("%f   %f   %f\n", ip_pts[j][0], ip_pts[j][1], ip_pts[j][2]);

	// Retrieve position of actual cell2 in <cell_ids> array. This position has to be 
	// determined because we need to update the corresponding linked list in <cell_list>
	// with the ids of the intersection points which lie on cell2

	cell2_pos = bin_search(cell2Id, cell_ids, int2);

	err = 0; // Error variable: controls node_type memory allocation

	if (ct == 3) { // tree intersections detected

		// check if first and second intersections are different
		// if yes, insert them into locator and update ip_ptids with their id's

		if (!(ip_pts[0][0] == ip_pts[1][0] && 
		      ip_pts[0][1] == ip_pts[1][1] &&
		      ip_pts[0][2] == ip_pts[1][2])) {
			if ((id1 = locator->IsInsertedPoint(ip_pts[0])) < 0)
				id1 = locator->InsertNextPoint(ip_pts[0]);
			
			if ((id2 = locator->IsInsertedPoint(ip_pts[1])) < 0)
                                id2 = locator->InsertNextPoint(ip_pts[1]);

			ip_ptids[i].id[0] = id1; // Stores id1 and id2 for ith surf2's triangle
			ip_ptids[i].id[1] = id2; // This is to build the polygonal intersection  
						 // line

		 	err+= add_node(&cell_list[cell2_pos], id1);  // Updates with id1 and id2 the
			err+= add_node(&cell_list[cell2_pos], id2);  // linked list associated with
							       // cell2. Used for triangulation

			if (!point_list->IsId(id1))	       // Update <point_list> too
				point_list->InsertNextId(id1);

			if (!point_list->IsId(id2))
				point_list->InsertNextId(id2);

		}
		else { // first and third point are different (as two points are the same)

			if ((id1 = locator->IsInsertedPoint(ip_pts[0])) < 0)
                                id1 = locator->InsertNextPoint(ip_pts[0]);

                        if ((id2 = locator->IsInsertedPoint(ip_pts[2])) < 0)
                                id2 = locator->InsertNextPoint(ip_pts[2]);
                        ip_ptids[i].id[0] = id1;
                        ip_ptids[i].id[1] = id2;


			err+= add_node(&cell_list[cell2_pos], id1);   
                        err+= add_node(&cell_list[cell2_pos], id2); 

			if (!point_list->IsId(id1))
                                point_list->InsertNextId(id1);

                        if (!point_list->IsId(id2))
                                point_list->InsertNextId(id2);

         	}
	}
	// case of two DIFFERENT intersection points detected

	else if (ct == 2 && !(ip_pts[0][0] == ip_pts[1][0] &&
                      	    ip_pts[0][1] == ip_pts[1][1] &&
                      	    ip_pts[0][2] == ip_pts[1][2])) {
			if ((id1 = locator->IsInsertedPoint(ip_pts[0])) < 0)
                                id1 = locator->InsertNextPoint(ip_pts[0]);

                        if ((id2 = locator->IsInsertedPoint(ip_pts[1])) < 0)
                                id2 = locator->InsertNextPoint(ip_pts[1]);
                        ip_ptids[i].id[0] = id1;
                        ip_ptids[i].id[1] = id2;


			err+= add_node(&cell_list[cell2_pos], id1);   
                        err+= add_node(&cell_list[cell2_pos], id2); 

			if (!point_list->IsId(id1))
                                point_list->InsertNextId(id1);

                        if (!point_list->IsId(id2))
                                point_list->InsertNextId(id2);


	}
	// else ... case of one intersection detected. Do nothing, by now...

/*
	if (err != 2) {
		printf ("Intersections: Unable to allocate node_type memory!\n");
		printf ("err: %d\n", err);
		return (1);
	}	
*/
	if (debug) {
	printf ("********************** Id's in locator: %d  %d\n\n", ip_ptids[i].id[0], ip_ptids[i].id[1]);
	printf ("i: %d\n", i);
	}

	   
                for (k=0; k<ct_aux; k++) {
                        id1 = locator->IsInsertedPoint(ip_pts[k]);
                        inter_aux->InsertNextId(id1);
                }




  } // End of first main for   


  // delete model1; // We don't need the RAPID objects any more 
  // delete model2;  

  delete[] RAPID_contact;

  surf2->Update();

  locator->FreeSearchStructure(); // Frees memory used by the locator
  locator->Delete();


  // inter_num is the number of determined intersection points 

  inter_num = point_list->GetNumberOfIds();
  printf ("Number of detected intersection points: %d\n", inter_num); 





  // Create <order> array to store the intersection points' ids

  order = (int *) malloc (inter_num * sizeof(int));
  // order = new int[inter_num];


  if (order == NULL) {
	printf ("<order> dinamic array: Unable to allocate memory!\n");
	return (1);
  }

  // Feed the array with the points ids in point_list

  for (i=0; i<inter_num; i++)
	order[i] = point_list->GetId(i);

  // Sort the array in ascending order to allow binary search

  shellsort (order, inter_num, incrmnts, numinc);

  point_list->Reset();
  point_list->Squeeze();
  point_list->Delete();

  // **************************************************************************************
  //
  // 		C H A I N I N G   I N T E R S E C T I O N   P O I N T S
  //
  // **************************************************************************************




  /* Now we can chain the intersection points together. For this we'll use the information
     contained in the ip_ptids array.
     We need to allocate memory for the points linking structure. We'll have a dynamic array
     of structs. Every struct will contain the following information:
   
     visited: flag used to determine if the point has already been used for the actual polyline
  
     next1 : (newpoints2) id of the neighbor point in one direction
  
     next2 : (newpoints2) id of neighbor point in the opposite direction

     For open polylines, a point with only ONE neighbor is the head (or tail) of
     the line. 
     In a loop (closed polyline) all the points will have TWO neighbors.
  */

  inode = (inode_ptr) malloc (inter_num * sizeof(struct inter_node_type));
  // inode = new struct inter_node_type[inter_num];


  if (inode == NULL) {
	printf ("*** inode: memory allocation error!\n");
        return (1);    
  }
  // Initialize the array assuming that each point has NO neighbor

  for (i=0; i<inter_num; i++) {
	inode[i].visited = 0;
	inode[i].num_tris = 0;
	inode[i].next1 = -1; 
	inode[i].next2 = -1;
  }


        for (k=0; k<inter_aux->GetNumberOfIds(); k++) {
                id1 = inter_aux->GetId(k);
                pos = bin_search (id1, order, inter_num);
		inode[pos].num_tris++;
	}




  // Now we put into this array the relevant informations

  for (i=0; i<inter_pairs; i++) { // Traverse again each pair of intersecting triangles

        // We are assuming that the number of intersection points (for each pair) is always TWO
        // This is a simplification and other special cases should be studied.
	// id1 and id2 are no longer points ids in surf2's structure. (absolute points' addresses).
	// They are now addresses of these points in the <order> array (relative addresses)
        // This point addresses  are the same used in the inode[] array, to store and retrieve
        // neighbor points' informations.


	id1 = bin_search(ip_ptids[i].id[0], order, inter_num); // Uses binary search to
        id2 = bin_search(ip_ptids[i].id[1], order, inter_num); // retrieve points id's addresses 
							       // from <order> array

	if (inode[id2].next1 < 0) // Referencing point id1 from point id2
		inode[id2].next1 = id1;
	else
		inode[id2].next2 = id1;

	if (inode[id1].next1 < 0) // Referencing point id2 from point id1
                inode[id1].next1 = id2;
        else
                inode[id1].next2 = id2;

  }
  free (ip_ptids); 		  // We don't need the triangles pair structs any more
  // delete[] ip_ptids;
  
  

  for (i=0; debug && i<inter_num; i++) {
	printf ("Point %d    Visited   %d\n", i, inode[i].visited); 
	printf ("\tNumber of referencing surf2's triangles: %d\n", inode[i].num_tris);
	printf ("\tNeighbours: %4d and %4d\n\n", inode[i].next1, inode[i].next2);
  }

  // The points have been chained each other in the inode array. 
  // Now we have to detect the number of intersection polylines for actors' memory allocation
  // We also have to create a different array of points' Ids for each polygonal line

  // The following vtkIdList object will contain three ids for each polygonal line:
  //
  // 1st : the id of the polyline head (absolute address) in another special list (allList)
  //
  // 2nd : the polyline type: 0 = open, 1 = closed
  //
  // 3rd : the number of points in the polyline. This number is decreased by one if line is closed

  vtkIdList *polyList = vtkIdList::New();

  // The following list will contain the ordered points ids for each polyline. Therefore it is a
  // sequence of ids groups. Traversing this list starting from each group head point's Id will
  // let the program build the correct points ids sequence for that line. This is made to find
  // the number of polygonal lines.
 
  allList = vtkIdList::New();

  lines_num = 0; 	// Set to zero the number of found open and closed polygonal lines
  loops_num = 0;
  visited_ct = 0; 	// this counts how many intersection points have been traversed

  do {  // main loop, repeated until all intersection points have been visited

	// First search for open polylines. <head> will be the id of the first point of a polyline
  
  	head = -1; // Head point Id for the current open polyline
        is_loop = 0;
  	for (i=0; head < 0 && i<inter_num; i++) // searches for open polyline (next2 = -1) 
		if (!inode[i].visited && inode[i].next2 == -1) {
			head = i;
 			lines_num++;
                }              

        // If head is NOW -1 (no open polyline) the next <for> searches for closed ones (loops)
	// <head> will be the first unvisited point

	for (i=0; head<0 && i<inter_num; i++)
		if (!inode[i].visited) {
			head = i;
			is_loop = 1; // This is to know that the current polyline is closed
			loops_num++;
		}

  
	k = allList->InsertNextId(order[head]); // k is the position in allList of the current
						// polyline's absolute head-point address
        inode[head].visited = 1;

	polyList->InsertNextId(k);  // polyList is updated with the position of the head id in
	polyList->InsertNextId(is_loop); // allList. The type of this polyline is is_loop

	ct = 1; // counter of the n. of points (of actual polyline)
		// whose ids were put into allList

  	current = inode[head].next1; 		   // Current visited point for polyline
						   // construction

  	ant = head;		     		   // Previous visited point	

	do { // Inner loop : building the current polyline

            if (current == head) // One loop has been completed, leave the cicle
		break;
      	    allList->InsertNextId(order[current]); // Stores current visited point's
						   // absolute address

      	    inode[current].visited = 1;		   // Marks the point as visited
	    ct++;				   // Increments polyline points counter

            if (inode[current].next2 == ant) {	   // Updates <current> with the next
		ant = current;			   // polyline relative point's Id
		current = inode[current].next1;
      	    }
            else {
		ant = current;
		current = inode[current].next2;
            }

        } while (current != -1);		   // Inner loop is repeated until the tail
						   // of the current polyline is found

	polyList->InsertNextId(ct);		   // <ct> now contains the number of points
						   // in the actual polyline. In a loop
				    		   // (closed polyline) the last point
 						   // (which is the first) is not included

	visited_ct+=ct;				   // Updates total number of visited points

   } while (visited_ct<inter_num);		   // End of main loop: all intersection
						   // points have been visited

  if (debug) {
  	printf ("polyList: %d   allList: %d\n", polyList->GetNumberOfIds(), allList->GetNumberOfIds());
  	for (i=0; i<polyList->GetNumberOfIds(); i++)
		printf ("poly %d\n", polyList->GetId(i));
  }

  // The following function removes from allList all intersection points which are too close each other.

  points_info (argv[1], argv[2], allList, polyList);
  printf ("New number of intersection points after distance control: %d\n", allList->GetNumberOfIds());


// ******************************************************************************************
//
//                  S O R T I N G   C E L L _ L I S T ' S   L I N K E D   L I S T S
//
//                    I N    P O L Y L I N E ' S   T R A V E R S A L   O R D E R
//
// ******************************************************************************************
/* The following block sorts each linked list in cell_ids containing the intersection points's ids
   which lie on the current intersecting triangle. The sorting is performed in the order given by
   the traversal of the intersection polyline containing the referenced points. To get this order, the position
   of each point in the allList list is stored in the <red_pos> array (whose size is the same as
   allList). This array is accessed by executing a binary search in the <red_order> array for the
   desired point's id. The position of the point in <red_order> is the same as its allList's position
   stored in the <red_pos> array.
   For each intersecting triangle, a dynamic array called <red_points> is created to store all its 
   intersection points' positions in allList. The array is then sorted in ascending order to
   get the correct polyline's traversal order. The linked list corresponding to the current
   triangle is then cleaned and rebuilt, traversing the <red_points> array and retrieving each intersection
   point's id by using its position in allList.

   The sorting executed in this block is necessary to determine each intersecting triangle's internal
   edges in order to perform a constrained Delaunay triangulation on the triangle. By traversing the
   corresponding linked list in cell_list, the edges will be traversed. Note that a triangle could be
   traversed by more than a polygonal intersection line. In this case the sequence of points's positions
   in allList is not unique. That is, it will contain more than a subsequence.

   Example:

	one polyline  : 34 35 36 37 38 39 (only one sequence)

	two polylines : 34 35 36 37 	  (two sequences)
			50 51 52 53 54 55 

   The numbers above are positions in allList retrieving by using the arrays <red_order> and <red_pos>
   for each intersection point's id.
*/



  vtkIdList *tr_ids = vtkIdList::New();


  npts  = allList->GetNumberOfIds(); // new number of intersection points after points reduction
  new_inter_num = npts;

  red_order = (int *) malloc (npts * sizeof (int)); // New point array to allow binary search
  if (!red_order) {
	printf ("red_order array: unable to allocate memory!\n");
	exit (1);
  }

  for (i=0; i<npts; i++)			    // Copy the contents of allList into red_order
	red_order[i] = allList->GetId(i);

  shellsort (red_order, npts, incrmnts, numinc);    // Sort red_order in ascending order

  red_pos = (int *) malloc (npts * sizeof (int)); // The array stores a point position in allList

  if (!red_pos) {
        printf ("red_pos array: unable to allocate memory!\n");
        exit (1);
  }

  // builds red_pos
  for (i=0; i<npts; i++) {
	id1 = allList->GetId(i);
	pos = bin_search (id1, red_order, npts);
	red_pos[pos] = i;
  }

  // Traverse each intersecting cell's linked list and sort it 
  // in polyline traversal order. This way, for each intersecting cell, it will be
  // possible to define its intersection edges as input for a constrained triangulation

  if (debug)
	printf ("\nList of intersection points for each triangle (the numbers are positions in the allList list):\n\n"); 

  for (i=0; i<int2; i++) { // Main for: one loop for each intersecting triangle

	tr_ids->Reset();

	pnode = cell_list[i].first;

        while (pnode != NULL) { // Traverse ith linked list
		
		// Store the point if it is valid (allList contains it after reduction) and if
		// it hasn't been inserted yet
		if (!tr_ids->IsId(pnode->id) && allList->IsId(pnode->id))
			tr_ids->InsertNextId(pnode->id);
		pnode = pnode->next;
	}
	ct = tr_ids->GetNumberOfIds(); // Number of non-duplicated and valid points in current triangle
	if (debug)
		printf ("triangle %d - %d points: ", cell_ids[i], ct);

	red_points = (int *) malloc (ct * sizeof(int));
	if (red_points == NULL) {
		printf ("red_points : unable to allocate memory for %dth cell_list's triangle!!!\n", i);
		exit (1);
	}

	// build red_points array as a list of allList positions
	for (j=0; j<ct; j++) { // Get intersection points's positions in allList
		pos = bin_search (tr_ids->GetId(j), red_order, npts);	
		red_points[j] = red_pos[pos];
	}
	shellsort (red_points, ct, incrmnts, numinc); // sort red_points
	if (debug) {
		for (j=0; j<ct; j++) printf ("  %d", red_points[j]);
		printf ("\n");
	}
	
	
	pnode = cell_list[i].first;
	while (pnode != NULL) {
		qnode = pnode;
		pnode = pnode->next;
		free (qnode);
	}
	cell_list[i].first = NULL;
         
	// Rebuilding ith linked list in cell_list 
	for (j=0; j<ct; j++) {
                err = add_node (&cell_list[i], red_points[j]); // Feed cell_list with allList positions!	
							       // instead of points' ids	
		// err = add_node (&cell_list[i], allList->GetId(red_points[j]));
		if (!err) {
			printf ("Missing memory while rebuilding cell_list's %dth list!!!", i);
			exit (1);
		}
	}

	free (red_points);

  } // End of Main for 

  // free (red_order);
  // free (red_pos);

// ******************************************************************************************
//
// 				T R I A N G U L A T I O N                          
//
// 	   O F   S U R F 2 ' S   I N T E R S E C T I N G   T R I A N G L E S
//
// ******************************************************************************************

 printf ("\nTriangulation...\n");

 vtkFloatPoints *points2 = (vtkFloatPoints *) surf2->GetPoints();


 tr_ids->Reset() ; 				// List containing all the ids of the points
				       		// used for current cell's triangulation

 vtkIdList *new_ids = vtkIdList::New(); 	// Absolute points' adresses for each new tr.
 vtkIdList *boundpoints = vtkIdList::New();
 vtkIdList *noboundpoints = vtkIdList::New();

 vtkIdList *side_s = vtkIdList::New();
 vtkIdList *side_t = vtkIdList::New();
 vtkIdList *side_st = vtkIdList::New();

 vtkFloatScalars *side_sc = vtkFloatScalars::New();
 vtkFloatScalars *side_tc = vtkFloatScalars::New();
 vtkFloatScalars *side_stc = vtkFloatScalars::New();



 printf ("\nsurf2:\n"); 
 printf ("\tN. of cells before triangulation : %d\n", surf2->GetNumberOfCells()); 

 for (i=0; i<int2; i++) { 			// main <for>. The cicle is repeated
			  			// for each intersecting cell in surf2

	cell2Id = cell_ids[i]; 			// Gets the number of the cell	

	if (debug)
		printf ("\nCell %d :\n", cell2Id);

	cell2 = surf2->GetCell(cell2Id); 	// Retrieves a pointer to the cell

	tr_ids->Reset();			// Clean cell's points list

	surf2->GetCellPoints(cell2Id, tr_ids);  // Get the cell vertices ids in tr_ids


	// Use cell_list[i] to Retrieve all the intersection points which lie on the triangle	

	pnode = cell_list[i].first;		// First node in ith Linked List

	while (pnode != NULL) {			// Traverse Linked List to get stored intersection points
	    tr_ids->InsertNextId(pnode->id);
	    pnode = pnode->next;
	};

	npts = tr_ids->GetNumberOfIds(); 	// Total number of triangulation points

	if (debug)
		printf ("\tN. of points in triangle : %d\n", npts);


  	tr_in.numberofpoints = npts;
	tr_in.numberofpointattributes = 0;
	tr_in.numberofcorners = 3;
	tr_in.numberofholes = 0;
	tr_in.numberofregions = 0;
 
	tr_in.pointlist = (REAL *) malloc (npts * 2 * sizeof (REAL));

	tr_out.trianglelist = (int *) NULL;

        ct =0;

	side_s->Reset(); side_sc->Reset();
	side_t->Reset(); side_tc->Reset();
	side_st->Reset(); side_stc->Reset();

	// Insert points' relative ids and parametric coordinates in the above lists
	// The firts three points are the intersecting triangle's vertices and their pcoords are known


	
	// Second <for> : get the parametric coordinates (pcoords) for
	//		  each intersection point belonging to the triangle;

	for (j=0; j<npts; j++) {

		// Gets jth point's coordinates into x[] and use them to retrieve
		// the point's parametric coordinates (<pcoords>) in triangle <cell2>
		// The first three ids in tr_ids are real surf2 ids, while the others
		// are point's positions in the allList list

		if (j<3)
			id1 = tr_ids->GetId(j);
		else
			id1 = allList->GetId(tr_ids->GetId(j));
		
	 	points2->GetPoint (id1, x);
		cell2->EvaluatePosition(x, closestPoint, subId, pcoords, dist2, weights);	


		sum = pcoords[0] + pcoords[1];
		
		if (pcoords[0] < 0.0) 	// Prevent the appearence of negative pcoords.
			pcoords[0] = 0.0; // As the point lie on the triangle, pcoords
		
		if (pcoords[1] < 0.0) 	// must range between 0 and 1.
			pcoords[1] = 0.0;
			

		// pcoords[0] -> s ; pcoords[1] -> t

		if (fabs ((double) pcoords[0]) < PTOL) {// point lies on t axis
			side_t->InsertNextId(j);
			side_tc->InsertNextScalar(pcoords[1]);
		}

		if (fabs ((double) pcoords[1]) < PTOL) {// point lies on s axis        
                        side_s->InsertNextId(j); 
                        side_sc->InsertNextScalar(pcoords[0]);
                }

		if ((fabs ((double) sum - 1.0)) < PTOL) { // point lies on s+t side
			side_st->InsertNextId(j);  
                        side_stc->InsertNextScalar(pcoords[1]); 
                } 
 
		if (debug) 
			printf ("\t   id %d :   Pcoords %12.10f   %12.10f  Sum: %12.10f\n",j ,pcoords[0], pcoords[1], sum);

		tr_in.pointlist[ct++] = (REAL) pcoords[0];
		tr_in.pointlist[ct++] = (REAL) pcoords[1];
	}

	s_npts = side_s->GetNumberOfIds();
        t_npts = side_t->GetNumberOfIds();
        st_npts = side_st->GetNumberOfIds();

 	if (debug)	
		printf ("s: %d    t: %d    s+t: %d\n", s_npts, t_npts, st_npts);


	nbo_segs = s_npts + t_npts + st_npts - 3; // Number of boundary segments

	// Allocate memory for internal and boundary constrain segments
	tr_in.segmentlist = (int *) malloc ((npts + nbo_segs - 3) * 2 * sizeof(int));


	// *************************************************
	// Defining constrained segments : Internal segments
	// *************************************************
	
	nsegs = 0; // Initializes number of non-boundary (internal) segments
	ct =0; k = npts-1;

	if (npts>4) { // at least two points different from the first three
           current = tr_ids->GetId(3);
           for (j=3; j<k; j++) {
                next = tr_ids->GetId(j+1);

                if (next == current+1) { // points are adjacent on the same polyline
                        nsegs++;
                        tr_in.segmentlist[ct++] = j;
                        tr_in.segmentlist[ct++] = j+1;
                }
                current = next;
           }
        } 

	// Replace points' positions in tr_ids with their ids in surf2
	for (j=3; j<npts; j++) {
		id1 = allList->GetId(tr_ids->GetId(j));
		tr_ids->SetId(j, id1);
	}



	// *************************************************
	// Defining constrained segments : boundary segments
	// *************************************************
		
	// points on the s axis: sort them in ascending order by using their side_s pcoords
	
	if (s_npts == 3) { // Three points on  the axis: swap second and third
		aux1 = side_s->GetId(2);
		side_s->SetId(1, aux1);
		side_s->SetId(2, 1);

		faux = side_sc->GetScalar(2);
		side_sc->SetScalar (1, faux);
		side_sc->SetScalar (2, 1.0);
	}
	else if (s_npts > 3) // More than three points. Use sorting algorithms
		sort_lists (side_s, side_sc, s_npts, incrmnts, numinc);

	// points on the t axis: sort them in ascending order by using their side_t pcoords
	if (t_npts == 3) { // Three points on  the axis: swap second and third
		aux1 = side_t->GetId(2);
                side_t->SetId(1, aux1);
                side_t->SetId(2, 2);

		faux = side_tc->GetScalar(2); 
                side_tc->SetScalar (1, faux);
                side_tc->SetScalar (2, 1.0);
        }
	else if (t_npts > 3) // More than three points. Use sorting algorithms
                sort_lists (side_t, side_tc, t_npts, incrmnts, numinc);

	// points on the s+t axis: sort them in ascending order by using their side_st pcoords
	if (st_npts == 3) { // Three points on  the axis: swap second and third
		aux1 = side_st->GetId(2);
                side_st->SetId(1, aux1);
                side_st->SetId(2, 2);
 
		faux = side_stc->GetScalar(2);
                side_stc->SetScalar (1, faux);
                side_stc->SetScalar (2, 1.0);
        }
	else if (st_npts > 3) // More than three points. Use sorting algorithms
                sort_lists (side_st, side_stc, st_npts, incrmnts, numinc);

	// Add constrains to triangulation struct

	k = s_npts - 1; // s axis
        for (j=0; j<k; j++) {
                tr_in.segmentlist[ct++] = side_s->GetId(j);
                tr_in.segmentlist[ct++] = side_s->GetId(j+1);
           }	


	k = st_npts - 1; // s+t triangle edge
        for (j=0; j<k; j++) {
                tr_in.segmentlist[ct++] = side_st->GetId(j);;
                tr_in.segmentlist[ct++] = side_st->GetId(j+1);;
           }



	k = t_npts - 1; // t axis  
        for (j=k; j>0; j--) {   

                tr_in.segmentlist[ct++] = side_t->GetId(j);
                tr_in.segmentlist[ct++] = side_t->GetId(j-1);
           }

	tr_in.numberofsegments = nsegs + nbo_segs;

	// Creating markers for boundary segments (0 = non-boundary, 1 = boundary)
	
	tr_in.segmentmarkerlist = (int *) malloc ((nsegs+nbo_segs)* sizeof (int));

	ct = 0;

	for (j=0; j<nsegs; j++) // non-boundary segments
		tr_in.segmentmarkerlist[ct++] = 0; 

	for (j=0; j<nbo_segs; j++) // boundary segments
		tr_in.segmentmarkerlist[ct++] = 1;
	
	if (debug) {
		for (k=0; k<s_npts; k++)
                	printf ("%f  %d\n", side_sc->GetScalar(k), side_s->GetId(k));
        	printf ("\n");

        	for (k=0; k<t_npts; k++)
                	printf ("%f  %d\n", side_tc->GetScalar(k), side_t->GetId(k));
        	printf ("\n");

        	for (k=0; k<st_npts; k++)
                	printf ("%f  %d\n", side_stc->GetScalar(k), side_st->GetId(k));
        	printf ("\n");	
	} 
	vorout = (struct triangulateio *) NULL;


	// ****************************************************
	// *************  T R I A N G U L A T I O N  **********
	// ****************************************************

	triangulate ("pzQPNB", &tr_in, &tr_out, vorout);


	num_tris = tr_out.numberoftriangles; // Number of new triangles replacing cell2Id
	if (num_tris<2) {
		printf ("Warning!!!: Triangle library has returned only %d new triangle for cell %d!!!\n", num_tris, cell2Id);
		num_tris = 0; // No triangle replacing the old surf2's one. There will be an hole in the surface
		// printf ("Program aborted.\n");
		// exit (1);
	}

	ct = 0;
	for (j=0; j<num_tris; j++) { // Add new triangles to surf2's topology
		new_ids->Reset();

		aux1 = 0; // Count the number of intersection points defining the new triangle

		// Get new triangle's point ids	in allList
		for (k=0; k<3; k++) {
			id1 = tr_out.trianglelist[ct++];
			if (id1>2) { // The point is an intersection one
				aux1++;
				pos = k;
			} 
			id2 = tr_ids->GetId(id1);

			new_ids->InsertNextId(id2);

		}	
	
		if (bad_flag && aux1==1) { // one intersection point. The triangle could traverse a polyline	

			id1=new_ids->GetId(pos); // Get allList id for intersection point 

			if (pos == 0) {
				aux1=1;
				aux2=2;
			}
			else if (pos == 1) {
				aux1=0;
				aux2=2;
		 	     }
		             else { // pos is 2
				aux1=0;
				aux2=1;
			     }	

			// Get non-intersection points' coordinates	
			points2->GetPoint(new_ids->GetId(aux1), xb);
                        points2->GetPoint(new_ids->GetId(aux2), xc);

			// Search for an intersection point adjacent to id1 on some polyline
			pos = bin_search (id1, red_order, new_inter_num);
			if (pos<new_inter_num-1) { // point is not the last one in allList
				
        			aux1= red_pos[pos]; // Get intersection point position in allList
				id2 = allList->GetId(aux1+1); // Get id1's adjacent in id2;
			}
			else { // point was the last in allList. Get the previous one
				aux1= red_pos[pos]; // Get intersection point position in allList
				id2 = allList->GetId(aux1-1); // Get id1's adjacent in id2;
			}

			// Get points coordinates
			points2->GetPoint(id1, xa); // xa is the intersection point
			points2->GetPoint(id2, x); // x is its neighbour on some polyline

			// Check if triangle segment (xb,xc) crosses polyline segment (xa,x). If yes the
		        // triangle cannot be added to surf2, as it will disturb the selection criterium.
			// If no crossing is detected the triangle can be added.
			// This check is made only for thin triangles, that is, triangle whose internal
			// angles are smaller than min_angle 

			angle1 = get_angle(xa, xb, xc);
			angle2 = get_angle(xb, xa, xc);

			/*if (!((angle<min_angle || angle>=max_angle) &&*/
			// if triangle is not bad
			if (!(angle1< min_angle || angle2<min_angle || (180-angle1-angle2) < min_angle &&
			   cross_polyline (xa, x, xb, xc)))
				surf2->GetPolys()->InsertNextCell(new_ids); // Add new triangle	
			else bad++; // triangle is bad, update bad counter

		}// end of (ct ==1 ) condition
		else surf2->GetPolys()->InsertNextCell(new_ids);

		/*
                // area = triclass->TriangleArea(x1, x2, x3);
                angle = get_angle(xa, xb, xc);
		// Don't add very thin triangles
		if (!(angle<min_angle || angle>=max_angle))
			surf2->GetPolys()->InsertNextCell(new_ids);
		else printf ("angle: %f\n", angle);
		*/
	} // End of j for	


	if (debug)
		printf ("The triangle was divided into %d tr.\n", num_tris);

	free (tr_in.pointlist);
	free (tr_in.segmentlist);
	free (tr_in.segmentmarkerlist);
	free (tr_out.trianglelist);

 } // end of main <for>
 surf2->Update();

 printf ("Removed bad triangles: %d\n", bad);
 printf ("\t\nN. of cells after triangulation : %d\n", surf2->GetNumberOfCells());

 new_ids->Delete();
 tr_ids->Delete();
	
 side_s->Delete(); side_sc->Delete();
 side_t->Delete(); side_tc->Delete();
 side_st->Delete(); side_stc->Delete();

 // Deletes <cell_list> array of Linked Lists

 for (i=0; i<int2; i++) { 
	pnode = cell_list[i].first;
        while (pnode != NULL) {
                qnode = pnode;
                pnode = pnode->next;
                free (qnode);
        }
        cell_list[i].first = NULL;
 }
 free (cell_list);
 // delete[] cell_list;

 // *****************************************************************************************
 //
 // 	R E M O V I N G   S U R F 2 ' S   I N T E R S E C T I N G   T R I A N G L E S
 //
 // *****************************************************************************************


 surf2->BuildCells();
 for (i=0; i<int2; i++)
	surf2->DeleteCell(cell_ids[i]); // Marks all surf2's intersecting cells for deletion

 surf2->Update(); 

 delete_marked_cells (surf2);  	// Fisically removes marked cells from surf2

 surf2->BuildCells();
 surf2->BuildLinks(); 			// Necessary to perform cell edge's neighbors query

 printf ("\tN. of cells after old triangles's removal : %d\n", surf2->GetNumberOfCells());






 // sss Saving data for cutter  

 vtkBYUWriter *writer = vtkBYUWriter::New();
 	writer->SetInput(surf2);
 	writer->SetGeometryFileName ("./surf2.dis");
 	writer->Update();
 	writer->Delete();

 f= fopen ("tricut.tmp", "w");
 if (!f) {
	printf ("Unable to open tricut.tmp file!\n");
	exit (1);
 }

 // save polyList and allList

 k = polyList->GetNumberOfIds(); // Save polyList data
 fprintf (f, "%d\n", k);
 for (i=0; i<k; i++)
	fprintf (f, "%d\n", polyList->GetId(i));

 k = allList->GetNumberOfIds();  // Save allList data
 fprintf (f, "%d\n", k);
 for (i=0; i<k; i++)  
        fprintf (f, "%d\n", allList->GetId(i));

 fprintf (f, "%f\n", z_translation); // saving z_translation 
 
 fclose (f);



 // *****************************************************************************************
 //
 //  		R E M O V I N G   N O   L O N G E R   U S E D   S T R U C T U R E S 
 //
 // *****************************************************************************************



 // allList->Delete();
 polyList->Delete();
 free (cell_ids);
 // delete[] cell_ids;

 printf ("\nDone.\nRun 'cutter %s' to perform cutting on %s\n", argv[1], argv[2]);
} // ******************** End of main function **********************


// This function normalizes a vector

void Normalise (double *p)
{
 double norm;

 norm = sqrt (p[0]*p[0] + p[1]*p[1] + p[2]*p[2]);
 p[0] /= norm;
 p[1] /= norm;
 p[2] /= norm;
}


/*
   Determine whether or not the line segment p1,p2
   Intersects the triangle bounded by pa,pb,pc
   Return true/false and the intersection point p

   The equation of the line is p = p1 + mu (p2 - p1)
   The equation of the plane is a x + b y + c z + d = 0
                                n.x x + n.y y + n.z z + d = 0

   This function has been written by Paul Bourke (http://www.mhri.edu.au/~pdb/)
*/

int line_tri(double *p1, double *p2, double *pa, double *pb, double *pc, double *p)

{
   double d;
   double a1,a2,a3;
   double total,denom,mu;
   double n[3] , pa1[3], pa2[3], pa3[3];

   /* Calculate the parameters for the plane */
   n[0] = (pb[1] - pa[1])*(pc[2] - pa[2]) - (pb[2] - pa[2])*(pc[1] - pa[1]);
   n[1] = (pb[2] - pa[2])*(pc[0] - pa[0]) - (pb[0] - pa[0])*(pc[2] - pa[2]);
   n[2] = (pb[0] - pa[0])*(pc[1] - pa[1]) - (pb[1] - pa[1])*(pc[0] - pa[0]);
   Normalise(n);
   d = - n[0] * pa[0] - n[1] * pa[1] - n[2] * pa[2];

   /* Calculate the position on the line that intersects the plane */
   denom = n[0] * (p2[0] - p1[0]) + n[1] * (p2[1] - p1[1]) + n[2] * (p2[2] - p1[2]);
   if (fabs(denom) < EPS)         /* Line and plane don't intersect */
      return(0);
   mu = - (d + n[0] * p1[0] + n[1] * p1[1] + n[2] * p1[2]) / denom;
   p[0] = p1[0] + mu * (p2[0] - p1[0]);
   p[1] = p1[1] + mu * (p2[1] - p1[1]);
   p[2] = p1[2] + mu * (p2[2] - p1[2]);
   if (mu < 0 || mu > 1)   /* Intersection not along line segment */
      return(0);

   /* Determine whether or not the intersection point is bounded by pa,pb,pc */
   pa1[0] = pa[0] - p[0];
   pa1[1] = pa[1] - p[1];
   pa1[2] = pa[2] - p[2];
   Normalise(pa1);
   pa2[0] = pb[0] - p[0];
   pa2[1] = pb[1] - p[1];
   pa2[2] = pb[2] - p[2];
   Normalise(pa2);
   pa3[0] = pc[0] - p[0];
   pa3[1] = pc[1] - p[1];
   pa3[2] = pc[2] - p[2];
   Normalise(pa3);
   a1 = pa1[0]*pa2[0] + pa1[1]*pa2[1] + pa1[2]*pa2[2];
   a2 = pa2[0]*pa3[0] + pa2[1]*pa3[1] + pa2[2]*pa3[2];
   a3 = pa3[0]*pa1[0] + pa3[1]*pa1[1] + pa3[2]*pa1[2];
   total = (acos(a1) + acos(a2) + acos(a3)) * RTOD;
   if (fabs(total - 360) > EPS)
      return(0);

   return(1);
}


// Converts a point's coordinates from float to double 

void feed_point (double *out, float *in)
{
 out[0] = (double) in[0];
 out[1] = (double) in[1];
 out[2] = (double) in[2];
}


// Shellsort function: This sorts the x[] array in ascending order

void shellsort (int *x, int n, int *incrmnts, int numinc)
{
 int incr, j, k, span, y;

 for (incr = 0; incr < numinc; incr++) {
	span = incrmnts[incr];
	for (j=span; j<n; j++) {
		y = x[j];
		for (k = j-span; k>= 0 && y<x[k]; k-=span)
			x[k+span] = x[k];
		x[k+span] = y;
	}
 }
}

// Shellsort function: floating point version

void fshellsort (float *x, int n, int *incrmnts, int numinc)
{
 int incr, j, k, span;
 float y;

 for (incr = 0; incr < numinc; incr++) {
        span = incrmnts[incr];
        for (j=span; j<n; j++) {
                y = x[j];
                for (k = j-span; k>= 0 && y<x[k]; k-=span)
                        x[k+span] = x[k];
                x[k+span] = y;
        }
 }
}



// Binary search function:
// If it finds <key> returns its position in the x[] array
// Else returns -1
// The size of the array is <n>

int bin_search (int key, int *x, int n)
{ 
 int low = 0;
 int hi, mid;

 hi = n-1;

 while (low <= hi) {
	mid = (low + hi)/2;
	if (key == x[mid])
		return (mid);
	if (key < x[mid])
		hi = mid-1;
	else
		low = mid+1;
 }
 return (-1);
}

// Binary search function for floating point arrays

int fbin_search (float key, float *x, int n)
{
 int low = 0;
 int hi, mid;

 hi = n-1;

 while (low <= hi) {
        mid = (low + hi)/2;
        if (key == x[mid])
                return (mid);
        if (key < x[mid])
                hi = mid-1;
        else
                low = mid+1;
 }
 return (-1);
}




// Add a node to a cell of the <cell_list> structure

int add_node (cellptr cell, int ptid)
{
 nodeptr p;

 p = (nodeptr) malloc (sizeof(struct nodetype)); // p points to created node
 // p = new struct nodetype[1];

 if (p == NULL) {
	node_fault++;
	return (0);
 }
 node_number++;

 p->id = (SHORT) ptid;
 p->next = NULL;

 if (cell->first == NULL) // if the list is empty
        cell->first = p; // update <first> pointer
 else
        (cell->last)->next = p; // else update last node's <next> pointer

 cell->last = p; // anyway, <last> must point to the last new node

 return (1);
}

// Show the ids associated with a cell from the cell_list structure 
void show (cellptr cell)
{
 nodeptr paux;

 paux = cell->first;
 if (!paux) return;

 do {
        printf ("%d\t", paux->id);
        paux = paux->next;
 } while (paux != NULL);
}

void delete_marked_cells (vtkPolyData *poly)
{
 int i;

 vtkIdList *temp_ids = vtkIdList::New();
 vtkCellArray *newcells = vtkCellArray::New();

 for (i=0; i<poly->GetNumberOfCells(); i++)
       if (poly->GetCellType(i) != 0) {
               temp_ids->Reset();
               temp_ids->Squeeze();
               poly->GetCellPoints(i, temp_ids);
               newcells->InsertNextCell(temp_ids);
       }
 vtkCellArray *temp_cells_array = poly->GetPolys();

 poly->SetPolys(newcells);

 temp_cells_array->Reset();

 poly->Update();
}


void points_info (char *s1, char *s2, vtkIdList *plist, vtkIdList *polyList)
{
 int i,j, poly_num;
 int pt1, pt2, pos, npts, step, aux;
 char type;
 FILE *f, *f1;
 float x1[3], x2[3];
 float bounds[6]; 
 float distance, temp, diag, diag_min;
 float average;
 float dmin, dmax;

 int new_npts, ant, current, next, current_pos, next_pos, aux1;


 poly_num = polyList->GetNumberOfIds()/3;


 surf2->GetPoints()->GetBounds (bounds);


 f = fopen ("./infos", "w");


 fprintf (f, "***************************************************\n");
 fprintf (f, "Informations on Intersection Points ***************\n");
 fprintf (f, "File1: %s\n", s1);
 fprintf (f, "File2: %s\n", s2);
 fprintf (f, "Number of polylines : %d\n\n\n", poly_num);

 temp = (bounds[0]-bounds[1]) *  (bounds[0]-bounds[1]) +
        (bounds[2]-bounds[3]) * (bounds[2]-bounds[3]) +
        (bounds[4]-bounds[5]) * (bounds[4]-bounds[5]);
 
 diag = (double) sqrt ((double) temp);

 // The following variable is the minimum allowed distance between intersection points. It is set
 // to the diag_factor'th part of surf2's (bounding box) diagonal
 // **********************************************************************************
 
 diag_min = diag/diag_factor;

 // **********************************************************************************
 // **********************************************************************************


 pos  = 0; step = 0;




 for (i=0; i<poly_num; i++) {
	 

     average = 0;
     npts = polyList->GetId(pos+2);

     dmin = 1000; dmax = 0;


     fprintf (f, "PolyLine n. %d  --  Number of points: %d\n", i, npts);


     fprintf (f, " Point   Id          X               Y               Z          dist. to next     ");
     fprintf (f, "Refs.    Bad dist.\n\n");

     for (j=0; j<npts-1; j++)  {

	pt1 = plist->GetId(step + j);
	pt2 = plist->GetId(step+1 + j);

	surf2->GetPoints()->GetPoint (pt1, x1);
	surf2->GetPoints()->GetPoint (pt2, x2);


	fprintf (f, "%4d   %4d  %14.9f %14.9f %14.9f", j, pt1, x1[0], x1[1], x1[2]);
	
 
		temp = (x1[0]-x2[0]) * (x1[0]-x2[0]) +
		       (x1[1]-x2[1]) * (x1[1]-x2[1]) + 
		       (x1[2]-x2[2]) * (x1[2]-x2[2]);
		distance = (float) sqrt ((double) temp);
		if (distance < dmin)
			dmin = distance;
		if (distance > dmax)
			dmax = distance;

		average += distance;

		aux = bin_search (pt1, order, inter_num);
		fprintf (f, "      %14.9lf      %d", distance, inode[aux].num_tris);

		if (distance < diag_min) 
			fprintf (f, "            *");	
		fprintf (f, "\n");

     }
	
 	average /= npts;

 	fprintf (f, "\nMinimum Distance: %14.9f\n", dmin);
 	fprintf (f, "Average Distance: %14.9f\n", average);
 	fprintf (f, "Maximum Distance: %14.9f\n\n", dmax);

 	fprintf (f, "\nBounding Box:\n\n");
 	fprintf (f, "\n    X : %14.9f -> %14.9f\n", bounds[0], bounds[1]);
 	fprintf (f, "    y : %14.9f -> %14.9f\n", bounds[2], bounds[3]);
 	fprintf (f, "    z : %14.9f -> %14.9f\n", bounds[4], bounds[5]);


 	fprintf (f, "\nModel Size (Bounding Box diagonal): %14.9f\n", diag);
 	fprintf (f, "\ndmin to Model Size: %14.9f\n", dmin/diag);

	pos += 3; step+=npts;

	fflush (f);

  }

  fprintf (f, "\n\n\n node_number: %d \n node_fault: %d\n", node_number, node_fault);


  if (!reduction) {
	fclose(f);
	return;
  }

  pos = 0; step=0;
  vtkIdList *new_allList = vtkIdList::New();
  vtkIdList *new_polyList = vtkIdList::New();
 
  for (i=0; i<poly_num; i++) {

	npts = polyList->GetId(pos+2);
        type = polyList->GetId(pos+1);

	j = 2; ant = -1; aux1 = -1; new_npts=0;
	current = plist->GetId(step); 
	next = plist->GetId(step + 1);


	while (next != -1) { 

		if (current != ant) { // Get information for a new current point 
			current_pos = bin_search (current, order, inter_num); // Retrieve current's pos in inode[]
			surf2->GetPoints()->GetPoint (current, x1); // Retrieve current's coords in x1;
			ant = current;
		}
		
		// Get information for the new next point which is different on each iteration

		next_pos = bin_search (next, order, inter_num); // Retrieve next's pos in inode[]
		surf2->GetPoints()->GetPoint (next, x2); // Retrieve next's coords in x2 

		// Compute distance between x1 and x2

		temp = (x1[0]-x2[0]) * (x1[0]-x2[0]) +
                       (x1[1]-x2[1]) * (x1[1]-x2[1]) + 
                       (x1[2]-x2[2]) * (x1[2]-x2[2]);
                distance = (float) sqrt ((double) temp);


		if (distance <= diag_min) { // The distance is invalid
			if (inode[next_pos].num_tris) {// next lies on a surf2's edge
				if (inode[current_pos].num_tris) { // current lies on a surf2's edge too
								   // consider distance as valid

					aux = new_allList->InsertNextId(current);
       			                new_npts++; // Update new number of points in polyline
                       		 	if (aux1 == -1) { // This is the first inserted point
                                		aux1 = aux;
                                		new_polyList->InsertNextId(aux); // stores new polyline head information
                                		new_polyList->InsertNextId(type); // stores polyline type
                        		}
					current = next;
				}
				else { // current doesn't lie on surf2's edge	
					remove_intersection_point (current_pos); // Remove current from inode[] chain
					current = next;
				}
			}
			else remove_intersection_point (next_pos);
		}
		else { // The distance is valid, consider the next point	
			aux = new_allList->InsertNextId(current);
			new_npts++; // Update new number of points in polyline
			if (aux1 == -1) { // This is the first inserted point
				aux1 = aux;
				new_polyList->InsertNextId(aux); // stores new polyline head information
				new_polyList->InsertNextId(type); // stores polyline type
			}
			current = next;
		}
	 		
		if (j<npts) 
			next = plist->GetId(step + j++);
		else next = -1;
	} 

	if (!new_allList->IsId(current)) { // Save last point
		new_allList->InsertNextId(current);
		new_npts++;
	}	
	
	step+= npts; pos+=3;
	new_polyList->InsertNextId(new_npts);

  } // End of main for
  plist->Reset(); plist->DeepCopy(new_allList);
  polyList->Reset(); polyList->DeepCopy(new_polyList);

  new_allList->Delete();
  new_polyList->Delete();

  aux = plist->GetNumberOfIds();
  fprintf (f, "***** allList:\n");
  for (i=0; i<aux; i++)
	fprintf (f, "%d -- %d\n", i, plist->GetId(i));
  fprintf (f, "\n");

  aux = polyList->GetNumberOfIds();
  fprintf (f, "***** polyList:\n");
  for (i=0; i<aux; i++)
        fprintf (f, "  %d", polyList->GetId(i)); 
  fprintf (f, "\n");

  fclose (f);
} 


void remove_intersection_point (int pos)
// Remove reference point from inode[] structure

{
 int prev, next; 

 prev = inode[pos].next1;
 next = inode[pos].next2;

 if (next != -1 && prev != -1) { // The point is not head nor tail of the polyline
	
	if (inode[prev].next1 == pos)
		inode[prev].next1 = next;
	else inode[prev].next2 = next;

	if (inode[next].next1 == pos) 
                inode[next].next1 = prev; 
        else inode[next].next2 = prev;

	inode[pos].next1 = -1;
	inode[pos].next2 = -1;
 }
 else if (prev == -1) { // The point is head of a polyline
	if (inode[next].next1 == pos)
		inode[next].next1 = -1;
	else inode[next].next2 = -1;

	if (inode[pos].next1 != -1)
		inode[pos].next1 = -1;
	else inode[pos].next2 = -1;
 }
 else { // The point is tail of a polyline
	if (inode[prev].next1 == pos) 
                inode[prev].next1 = -1; 
        else inode[prev].next2 = -1;

	if (inode[pos].next1 != -1) 
                inode[pos].next1 = -1; 
        else inode[pos].next2 = -1; 
 }
}

void show_help (void)
{
 int i=0;

 while (*helptxt[i] != '!')
	puts (helptxt[i++]);
}


/* The following function receives two lists as input. The first is a sequence of parametric
   coordinates, while the second is a sequence of corresponding points' ids. These are boundary
   points in the current intersecting triangle and need to be sorted in ascending pcoords
   (that is, parametric coordinates along the s, t or s+t axes) values. This way the list of
   boundary segments in the triangle can be retrieved and used as input for the constrained
   triangulation library.
*/ 

void sort_lists (vtkIdList *idlist, vtkFloatScalars *clist, int n, int *incrmnts, int numinc)
{
 float *p_order;
 int *p_ids;
 int j, pos;


 p_order = (float *) malloc (n * sizeof (float));
 if (p_order == NULL) {
 	printf ("p_order array: unable to allocate memory!\n");
        exit(1);
 }
 
 p_ids = (int *) malloc (n * sizeof (int));
 if (p_ids == NULL) {
        printf ("p_ids array: unable to allocate memory for %dth intersecting triangle!\n");
        exit(1);
 }        

 for (j=0; j<n; j++) // feed p_order with points' pcoords on current axis
 	p_order[j] = clist->GetScalar(j);
 
 fshellsort (p_order, n, incrmnts, numinc); // Sort p_order
 
 // Now build p_ids array containing points' positions in tr_ids list
 
 for (j=0; j<n; j++) {
        pos = fbin_search (clist->GetScalar(j), p_order, n);
                        p_ids[pos] = idlist->GetId(j);
 }
 
 // Build sorted pcoords list
 clist->Reset();
 for (j=0; j<n; j++)
        clist->InsertNextScalar(p_order[j]);
 
 // Build corresponding ids list as well
 idlist->Reset();
 for (j=0; j<n; j++)
        idlist->InsertNextId(p_ids[j]);
 
 free (p_order);
 free (p_ids);
}

float get_angle (float *x1, float *x2, float *x3)
{
 float v1[3], v2[3];
 float N1, N2, angle, temp;
 double rad_to_deg = 180/M_PI;
 int i;

 // Computes vextors v1 and v2
 for (i=0; i<3; i++) {
        v1[i] = x2[i] - x1[i];
        v2[i] = x3[i] - x1[i];
 }
 // Computes their magnitudes
 N1 = (float) sqrt ((double) (v1[0]*v1[0] + v1[1]*v1[1] + v1[2]*v1[2]));
 N2 = (float) sqrt ((double) (v2[0]*v2[0] + v2[1]*v2[1] + v2[2]*v2[2]));

 temp = (v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2]) / (N1 * N2);
 angle = (float) acos ((double) temp) * rad_to_deg;

 // angle = (float) (acos ((double) (v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2])) * rad_to_deg);
 return (angle);

 // return (angle); // dot product
}

// Checks whether triangle segment (xa, xb) crosses polyline segment (x1, x2) 
int cross_polyline (float *p1, float *p2, float *pa, float *pb)
{
 float pp[3]; // projection point
 float v1[3], v2[3];
 float A, B, D;
 int i;


 // creates p1's projection point on the xy plane 
 pp[0] = p1[0]; pp[1] = p1[1]; pp[2] = 0;
	

 // compute vectors v1 and v2 defined by points p1, p2 ad pp
 for (i=0; i<3; i++) { 
        v1[i] = p2[i] - p1[i];
	v2[i] = pp[i] - p1[i];
 }

 // Get equation for the plane defined by points p1, p2 and pp. C coefficient is not needed as the plane is 
 // perpendicular to the xy one (we used p1's projection on xy plane as third point).
 A = v1[1]*v2[2] - v2[1]*v1[2];
 B = v1[2]*v2[0] - v2[2]*v1[0];
 D = -(A*p1[0] + B*p1[1]);

 // Now we check which side of plane points pa and pb are on

 if ((A*pa[0] + B*pa[1] + D) * (A*pb[0] + B*pb[1] + D) < 0) // pa and pb on different sides (cross)
	return (1);
 else return (0);
}