基于三角网内插生成DEM

//这是头文件
#ifndef Delaunay_H
#define Delaunay_H
#include <iostream>
#include <stdlib.h> // for C qsort 
#include <cmath>
#include <time.h> // for random

#define PINDEX long int
char Inputfile[]="D:\\Program files\\Project\\BuildDEMBasedOnTin\\input_points.txt";
char Outputfile[]="D:\\Program files\\Project\\BuildDEMBasedOnTin\\output.txt";

const int MaxVertices = 500;
const int MaxTriangles = 1000;
const int n_MaxPoints = 10; // for the test programm
const double EPSILON = 0.000001;

struct ITRIANGLE{
  int p1, p2, p3;
};

struct IEDGE{
  int p1, p2;
};

struct XYZ{
  double x, y, z;
};

int CountNumberOfPoints(char *Inputfile);
void ReadData(XYZ &PointArr ,char *Inputfile, PINDEX *total);
int XYZCompare(const void *v1, const void *v2);
int Triangulate(int nv, XYZ pxyz[], ITRIANGLE v[], int &ntri);
int CircumCircle(double, double, double, double, double, double, double, 
double, double&, double&, double&);

#endif

//.cpp文件

#include<iostream>
#include<fstream>
#include "TIN.h"
#include <stdio.h>
#include <vector>
#include <math.h>
#include <time.h>
using namespace std;

int main()
{
	PINDEX total; Point_PTR PointArr; int InitialPointSize;
	MESH Pmesh;
	clock_t start_time, end_time;
	start_time = clock();

	total = CountNumberOfPoints(Inputfile);

	PointArr = (Point_PTR) malloc (total * sizeof(POINT));

	memset(PointArr,0,total*sizeof(POINT));

	ReadData(PointArr,Inputfile,&total);

	InitMesh(&Pmesh, total);

	FindInitialPoints(PointArr,total,&Pmesh,&InitialPointSize); 

	printf("Initial points size: %d\n",InitialPointSize);

	CreateInitialTin(&Pmesh,InitialPointSize);

	//add other points into the Pmesh.
	for(int i =InitialPointSize; i<total+InitialPointSize; i++)
	{
		(Pmesh.pArr+i)->x =  PointArr[i-InitialPointSize].x;
		(Pmesh.pArr+i)->y =  PointArr[i-InitialPointSize].y;
		(Pmesh.pArr+i)->z =  PointArr[i-InitialPointSize].z;
	
	}

	InsertOtherPoints(&Pmesh, total, InitialPointSize); 
/*
   OutputData(Outputfile, &Pmesh); 
   end_time = clock();
   	cout<<"All done! Consumed "<<double(end_time-start_time)/CLOCKS_PER_SEC<<" seconds"<<endl; */
   return 0;
}


PINDEX CountNumberOfPoints(char *Inputfile)
{
    FILE * fp = NULL; 
    int c, lc=0; 
    long int line = 0; 
    fopen_s(&fp,Inputfile, "r");
    while((c = fgetc(fp)) != EOF) 
    {
        if(c == '\n') 
			line ++; 
		    lc = c; 
    }
    fclose(fp); 
    if(lc != '\n') 
		{line ++;}
   printf("the number of lines in the file: %d\n", line);
    return line;
}

void ReadData(Point_PTR &PointArr,char *Inputfile, PINDEX *total)
{
	FILE* fp = fopen( Inputfile, "r" );

	if ( !fp )
	{
		fprintf( stderr,"Error:%s open failed\n", Inputfile );
		exit( 1 );
	}

	double x, y, z;
	for ( int i = 0; i < *total; ++i ){          
		fscanf(fp, "%lf %lf %lf ", &x, &y, &z);
		PointArr[i].x = x;    
		PointArr[i].y = y; 
		PointArr[i].z = z;
	}
	printf("Read data succeed!\n");
	fclose(fp);
}

 void DeleteExtremPoint(Point_PTR &PointArr,int &total)
 {
	 int begin = total*rate, end = total*(1-rate);
	 int M = 0, N = total-1;
	 quicksort1(PointArr,M, N);

	 for (int i = begin, j = 0; i <end, j<total*2*rate; i++,j++)
	 {
		 PointArr[j]=PointArr[i];
		 printf("coordinates of points: %lf %lf %lf\n",PointArr[j].x,PointArr[j].y,PointArr[j].z);
			 
	 }
	 total = total*(1-2*rate);
	 printf("the total number of points after deleted:%d\n", total);
 }

 void quicksort1(Point_PTR &PointArr,int M, int N)
 {
    int i=M, j=N;
    double z=(PointArr+i)->z,x=(PointArr+i)->x, y = (PointArr+i)->y;
    while(i<j)  
    {  
        // from right to left  
		while(i<j && (PointArr+j)->z >= z)  
        {  
            j--;  
        }  
        if (i<j && (PointArr+j)->z<z)  
        {  
			(PointArr+i)->x=(PointArr+j)->x;  
			(PointArr+i)->y=(PointArr+j)->y; 
			(PointArr+i)->z=(PointArr+j)->z; 
            i++;   
        }  
        // from left to right  
        while(i<j && (PointArr+i)->z<z)  
        {  
            i++;  
        }  
  
        if(i<j && (PointArr+i)->z>z)  
        {  
			(PointArr+j)->x = (PointArr+i)->x;
			(PointArr+j)->y = (PointArr+i)->y;
			(PointArr+j)->z = (PointArr+i)->z;
                j--;  
        }  
    }  
   (PointArr+i)->z = z;  
   (PointArr+i)->x = x;
   (PointArr+i)->y = y;

    if (M<N)  
    {  
        quicksort1(PointArr,M,i-1);  
        quicksort1(PointArr,i+1,N);  
    }  
 }

void FindInitialPoints(Point_PTR &PointArr, int total, Mesh_PTR Pmesh, int *InitialPointSize)
{
	int i, j, k;
	POINT boundary_point1,boundary_point2,boundary_point3,boundary_point4;
	//calculate MinX, MinY, MaxX, MaxY;
	float Min_X,Min_Y,Max_X,Max_Y;
	Min_X = MinX(PointArr,total); Min_Y = MinY(PointArr,total); 
	Max_X = MaxX(PointArr,total); Max_Y = MaxY(PointArr,total);
	boundary_point1.x = Min_X; boundary_point1.y = Max_Y;
	boundary_point2.x = Min_X; boundary_point2.y = Min_Y;
	boundary_point3.x = Max_X; boundary_point3.y = Max_Y;
	boundary_point4.x = Max_X; boundary_point4.y = Min_Y;
	
//	printf("%lf %lf %lf %lf\n",Min_X, Min_Y, Max_X,Max_Y) ;

	int rows, cols; float dx, dy;

	//calculate the number of rows and columns.
	rows = floor((Max_Y-Min_Y)/BUILD_SIZE);
	cols = floor((Max_X-Min_X)/BUILD_SIZE);

	//calcualte the size of every grid
	dx = (Max_X-Min_X)/cols;
	dy = (Max_Y-Min_Y)/rows; 
//	printf("the dx dy:%lf %lf\n", dx, dy);

	double X_Node, Y_Node;
	vector<float> Y_NodeArr;
	vector<float> X_NodeArr;

	// calcuate the X coordinate of Nodes
	for ( i=0; i<cols; i++)
	{
		X_Node = Min_X+i*dx;
		X_NodeArr.push_back(X_Node);
	}
	X_NodeArr.push_back(Max_X);


/*	for ( i=0; i<=cols; i++)
	{
		printf("%lf\n",X_NodeArr[i]);
	}*/

	// calcuate the Y coordinate of Nodes
	for ( j=0; j<rows; j++)
	{
		Y_Node = Min_Y+j*dy;
		Y_NodeArr.push_back(Y_Node);
	}
	Y_NodeArr.push_back(Max_Y);

/*		for ( j=0; j<=rows; j++)
	{
		printf("%lf\n",Y_NodeArr[j]);
	}*/

//	printf("the number of X_NodeArr and Y_NodeArr: %d %d\n",X_NodeArr.size(),Y_NodeArr.size());
	//create the grid; 
	
	vector<POINT> S_Grid;  //the points in the single grid;
	vector<POINT> InitialPoints;
	vector<int> S_GridSize;
	int size = 0; 
	S_GridSize.push_back(0);
	//这点以后优化，n^2的已经很慢了，这里是n^3 更加的慢，得到一个规则格网。
	for (i=0; i<Y_NodeArr.size();i++)
	{	
		for (j=0; j<X_NodeArr.size();j++)
		{
			for ( k=0; k<total; k++)
			{
				if ((PointArr[k].y>Y_NodeArr[i] && PointArr[k].y<=Y_NodeArr[i+1]) && (PointArr[k].x>X_NodeArr[j] && PointArr[k].x<=X_NodeArr[j+1]))
				{
						S_Grid.push_back(PointArr[k]);		
						
					//	printf("%lf %lf %lf \n",PointArr[k].x,PointArr[k].y,PointArr[k].z);
				}
			}
			size = S_Grid.size(); //这里是S_Grid 第一个是1 第二个是第一个GRID里面的点的数量+1, 第三个是
	//		printf("%d \n", size);
			S_GridSize.push_back(size);
			
			//得到上一个数据得索引，然后从上一个数据得索引
	//		Temp = MinZ(S_Grid,S_Grid.size());	
	//		size = S_Grid.size();
		//	printf("the size of vector: %d", S_Grid.size());
		//	S_Grid.clear();
		//	S_Grid.swap(vector<POINT>());
		}
	}
/*	for (int k=0; k < S_Grid.size(); k++)
	{
		printf("%lf \n", S_Grid[k].z );
	}*/

	POINT MPoint; int GridNum;
//	printf("%d\n",S_GridSize.size() );
	double Min_Z=9999;
	vector<double> MinZArr;
	GridNum=Y_NodeArr.size()*X_NodeArr.size();

	//search the lowest height of every grid
	for (i = 0; i< GridNum; i++) //小心边界检查啊
	{
		for (j = S_GridSize[i];j < S_GridSize[i+1];j++) //S_GridSize得开头应该是0；
		{
			if (Min_Z > S_Grid[j].z)
			{Min_Z = S_Grid[j].z;}
			
		}
		MinZArr.push_back(Min_Z);
		Min_Z = 9999;
	}
/*	for (i = 0; i<MinZArr.size(); i++)
	{
		printf("%lf\n",MinZArr[i] );
	}*/
	//search every point of every grid with the lowest height
	for (i = 0; i< MinZArr.size(); i++)
	{
		for (j = S_GridSize[i];j < S_GridSize[i+1];j++)
		{
			if (S_Grid[j].z == MinZArr[i])
			{
				InitialPoints.push_back(S_Grid[j]);
	//			printf("%lf %lf %lf \n",S_Grid[j].x,S_Grid[j].y,S_Grid[j].z);
			}
	
		}
	}	

//	printf("the number of initial points %d\n",InitialPoints.size());
	boundary_point1.z = InitialPoints[InitialPoints.size() - cols].z;
	boundary_point2.z = InitialPoints[0].z;
	boundary_point3.z = InitialPoints[InitialPoints.size()-1].z;
	boundary_point4.z = InitialPoints[ cols - 1 ].z;
	
	InitialPoints.push_back(boundary_point1);
	InitialPoints.push_back(boundary_point2);
	InitialPoints.push_back(boundary_point3);
	InitialPoints.push_back(boundary_point4);
	
	int SeedPointsNum = InitialPoints.size();

	printf("initial points finding succeed! %d\n",SeedPointsNum);

	//提取的几乎是一样的。
	for(i = 0; i<SeedPointsNum; i++)
	{
		(Pmesh->pArr+i)->x =  InitialPoints[SeedPointsNum-1-i].x;
		(Pmesh->pArr+i)->y =  InitialPoints[SeedPointsNum-1-i].y;
		(Pmesh->pArr+i)->z =  InitialPoints[SeedPointsNum-1-i].z;
		printf("%lf %lf %lf \n",(Pmesh->pArr+i)->x,(Pmesh->pArr+i)->y,(Pmesh->pArr+i)->z);
	}
	*InitialPointSize = SeedPointsNum;
	
	//output the points.

/*	FILE* fp = fopen( "C:\\Users\\WanpengShao\\Desktop\\output.txt", "w" );

	if ( !fp )
	{
		fprintf( stderr,"Error:%s open failed\n", Inputfile );
		exit( 1 );
	}

	for ( int i = 0; i < SeedPointsNum; ++i ){          
		fprintf( fp, "%lf %lf %lf \n", (Pmesh->pArr+i)->x, (Pmesh->pArr+i)->y, (Pmesh->pArr+i)->z );
		
	}
	fclose(fp);
*/
}

double MinX(Point_PTR &PointArr, int total)
{
	double Min_X;
	Min_X = PointArr[0].x;
	for (int i=0; i<total; i++)
	{
		if (Min_X > PointArr[i].x)
			Min_X = PointArr[i].x;
	}
	return Min_X;
}


double MinY(Point_PTR &PointArr, int total)
{
	double Min_Y;
	Min_Y = PointArr[0].y;
	for (int i=0; i<total; i++)
	{
		if (Min_Y > PointArr[i].y)
			Min_Y = PointArr[i].y;
	}
	return Min_Y;
}

double MaxX(Point_PTR &PointArr, int total)
{
	double Max_X;
	Max_X = PointArr[0].x;
	for (int i=0; i<total; i++)
	{
		if (Max_X < PointArr[i].x)
			Max_X = PointArr[i].x;
	}
	return Max_X;
}

double MaxY(Point_PTR &PointArr, int total)
{
	double Max_Y;
	Max_Y = PointArr[0].y;
	for (int i=0; i<total; i++)
	{
		if (Max_Y < PointArr[i].y)
			Max_Y = PointArr[i].y;
	}
	return Max_Y;
}
/*
double MinZ(vector<POINT> S_Grid, int GridSize, int size)
{
	double Min_Z; double Temp;
	Min_Z = S_Grid[0].z;
	for (int i=size; i<GridSize; i++ )
	{
		if (Min_Z > S_Grid[i].z)
			Min_Z = S_Grid[i].z;
	}
//	printf("The minimum Z coordiante is : %lf\n", Min_Z);
	for (int j=size; j<GridSize; j++)
	{
		if(S_Grid[j].z == Min_Z )	
		{
			Temp = S_Grid[j].z;

		}
	}
//	printf("%lf  %lf  %lf\n",InitialPoints[0].x, InitialPoints[0].y,InitialPoints[0].z);
	return Temp;
}*/

double MinZ(vector<POINT> S_Grid, int GridSize)
{
	double Min_Z; double Temp;
	Min_Z = S_Grid[0].z;
	for (int i=0; i<GridSize; i++ )
	{
		if (Min_Z > S_Grid[i].z)
			Min_Z = S_Grid[i].z;
	}
//	printf("The minimum Z coordiante is : %lf\n", Min_Z);
/*	for (int j=size; j<GridSize; j++)
	{
		if(S_Grid[j].z == Min_Z )	
		{
			Temp = S_Grid[j].z;

		}
	}*/
//	printf("%lf  %lf  %lf\n",InitialPoints[0].x, InitialPoints[0].y,InitialPoints[0].z);
	return Temp;
}

void InitMesh(Mesh_PTR Pmesh, PINDEX total)
{
	Pmesh->pArr = (Point_PTR)malloc((total+total/100) * sizeof(POINT));
	if (Pmesh->pArr == NULL)
	{
		fprintf(stderr, "Error:Allocate memory for mesh failed\n");
		exit(1);
	}
	Pmesh->PNUM = total;
	Pmesh->TNUM = 0;
}

void CreateInitialTin(Mesh_PTR Pmesh, int InitialPointSize) 
{
	//这四个点是四个角点加上随意的一个其他的点？
	AddTriangleNode( Pmesh, NULL, 4, 3, 2 ); 	 
	AddTriangleNode( Pmesh, Pmesh->TriArr, 4, 2, 0 );
	AddTriangleNode( Pmesh, Pmesh->TriArr->TNext, 4, 0, 1 );
	AddTriangleNode( Pmesh, Pmesh->TriArr->TNext->TNext, 4, 1, 3 ); 

	int vertex_index = 0,r=0;
	printf("the number of Initial points: %d\n",InitialPointSize);
	for (vertex_index = 5; vertex_index <InitialPointSize; vertex_index++)
	{ 
		Point_PTR pVer = Pmesh->pArr+vertex_index; 
		TRIANGLE_ARR pTargetTri = NULL;
		TRIANGLE_ARR pTri = Pmesh->TriArr; 
		while (pTri != NULL)
		{ 
		    r = Intriangle1(Pmesh, pVer, pTri); 
	//		printf("the points in the triangle: %d\n", r);
			if(r > 0) // should be in triangle
			{
				pTargetTri = pTri;
				break;
			}
				pTri = pTri->TNext; 
		}
		if (r>0)
		{
			InsertInTriangle( Pmesh, pTargetTri,vertex_index ); 
	
		}
	}	
	printf("Tin initilization succeed!\n");
}


TRIANGLE_ARR AddTriangleNode (Mesh_PTR Pmesh,TRIANGLE_ARR TriArr, int i, int j, int k) 
{

	if(CounterClockWise((Pmesh->pArr+i), (Pmesh->pArr+j), (Pmesh->pArr+k)) == 0)
	{
		return NULL;
	}
	
	TRIANGLE_ARR pTempTri = (TRIANGLE_ARR) malloc(sizeof(TRIANGLE));
			
	pTempTri->i = i;
	pTempTri->j = j;
	pTempTri->k = k;
//		Pmesh->TriArr->TPre =NULL;
	if (TriArr == NULL)
	{
		Pmesh->TriArr = pTempTri; 
		pTempTri->TNext = NULL;
		pTempTri->TPre = NULL;
	
		(Pmesh->TNUM)++;
	}
	else
	{	
		if (TriArr->TNext == NULL) 
		{
			pTempTri->TNext = TriArr->TNext;
			pTempTri->TPre = TriArr;
			TriArr->TNext = pTempTri;
			(Pmesh->TNUM)++;
		}
		else
		{
			pTempTri->TNext = TriArr->TNext; 
			pTempTri->TPre = TriArr;										 
			TriArr->TNext->TPre = pTempTri;
			TriArr->TNext=pTempTri;

			(Pmesh->TNUM)++;				
		}				
	}

	printf("%d\n",Pmesh->TNUM);
	printf("assign the triangle succeed!\n");

	return pTempTri;
	
}

//
int CounterClockWise(Point_PTR Pa, Point_PTR Pb, Point_PTR Pc)
{
	return ((Pb->x - Pa->x)*(Pc->y - Pb->y) - (Pc->x - Pb->x)*(Pb->y - Pa->y)); //当然异常了！ 那个pre 
}

//judge whether the point(pVer) is located in the triangle or not
int Intriangle(Mesh_PTR Pmesh, Point_PTR pVer, TRIANGLE_ARR pTri) 
{
	int vertex_index;
	Point_PTR pV1, pV2, pV3;
	vertex_index = pTri->i; 
	pV1 = Pmesh->pArr+vertex_index;
	vertex_index = pTri->j;
	pV2 = Pmesh->pArr+vertex_index;
	vertex_index = pTri->k;
	pV3 = Pmesh->pArr+vertex_index;
	int ccw1 = CounterClockWise(pV1, pV2, pVer);
	int ccw2 = CounterClockWise(pV2, pV3, pVer);
	int ccw3 = CounterClockWise(pV3, pV1, pVer);
	
	int r = -1;

	if (ccw1>0 && ccw2>0 && ccw3>0)
	{
		r = 1;
	} 
	else 
	{
		r = 0;
	}

	return r;
}

void InsertInTriangle(Mesh_PTR &Pmesh, TRIANGLE_ARR pTargetTri,	int vertex_index)
{
	int index_a, index_b, index_c;
	TRIANGLE_ARR pTri = NULL;
	TRIANGLE_ARR pNewTri = NULL;

	pTri = pTargetTri;	
	if(pTri == NULL)
	{
		return;
	}

	// Inset p into target triangle
	index_a = pTri->i;
	index_b = pTri->j;
	index_c = pTri->k;

	// Insert edge pa, pb, pc
	for(int i=0; i<3; i++)
	{
		if(i == 0)
		{ 
			pNewTri = AddTriangleNode(Pmesh, pTri, index_a, index_b, vertex_index);
		}
		else if(i == 1)
		{
			pNewTri = AddTriangleNode(Pmesh, pTri, index_b, index_c, vertex_index);
		}
		else
		{
			pNewTri = AddTriangleNode(Pmesh, pTri, index_c, index_a, vertex_index);
		}

		// go to next item
		if (pNewTri != NULL)
		{
			pTri = pNewTri;  
		}
		else
		{
			pTri = pTri;
		}
	}

	// Get the three sub-triangles 
	pTri = pTargetTri;	
	TRIANGLE_ARR pTestTri[3];
	for (int i=0; i< 3; i++)
	{
		pTestTri[i] = pTri->TNext;

		pTri = pTri->TNext;
	}

	// remove the Target Triangle  
	RemoveTriangleNode(Pmesh, pTargetTri); //把大三角形给remove掉了，只剩下连接着的三个小三角形

	for (int i=0; i< 3; i++)
	{
		// Flip test
		FlipTest(Pmesh, pTestTri[i]); //是因为有些点你怎么flipTest都不可能出来最优三角形。所以这个函数就直接停止运行。return false 要是这样的话怎么办呢？创建出来的TIN会很不规则，那么内插的点的高程也会很差劲儿。
	}
}

void RemoveTriangleNode(Mesh_PTR &PMesh, TRIANGLE_ARR pTargetTri)
{	  //再看看双向链表的操作。
	if (pTargetTri == NULL)
	{
		return;
	} //但是如果是落单到第一个三角形内呢？
	if (pTargetTri->TPre ==NULL)
	{
		PMesh->TriArr = pTargetTri->TNext;
	}
	else
	{
	// remove from the triangle list
		pTargetTri->TPre->TNext = pTargetTri->TNext;
	}

	if(pTargetTri->TNext!=NULL)
	{
		pTargetTri->TNext->TPre = pTargetTri->TPre;
	}
	// deallocate memory
	free(pTargetTri);	
	--(PMesh->TNUM);
	printf("the number of triangle after removing: %d\n",PMesh->TNUM);
}

//每次LOP检查都是三角形数组种重头开始进行LOP检查。
bool FlipTest(Mesh_PTR PMesh, TRIANGLE_ARR pTestTri)
{
	bool flipped = false;

	int index_a = pTestTri->i;
	int index_b = pTestTri->j;
	int index_p = pTestTri->k; 

	int statify[3]={0,0,0};
	int vertex_index;
	int *pi;
	int k = 1;

	// find the triangle which has edge consists of start and end  
	TRIANGLE_ARR pTri = PMesh->TriArr;

	int index_d = -1;
	while (pTri != NULL) 
	{
		statify[0] = 0;
		statify[1] = 0;
		statify[2] = 0;

		pi = &(pTri->i); 
		for (int j=0, k = 1; j<3; j++, k*= 2)  
		{
			vertex_index = *pi++;
			if(vertex_index == index_a || vertex_index == index_b) 
			{
				statify[j] = k;
			}
		}

		switch(statify[0] | statify[1] | statify[2] )
		{
			case 3: 
				if(CounterClockWise((Point_PTR)(PMesh->pArr+index_a),(Point_PTR)(PMesh->pArr+index_b),(Point_PTR)(PMesh->pArr+pTri->k)) < 0)
				{
					index_d = pTri->k;
				}
				
				break;
			case 5:
				if(CounterClockWise((Point_PTR)(PMesh->pArr+index_a),(Point_PTR)(PMesh->pArr+index_b),(Point_PTR)(PMesh->pArr+pTri->j)) < 0)
				{
					index_d = pTri->j;
				}
				
				break;
			case 6: 
				if(CounterClockWise((Point_PTR)(PMesh->pArr+index_a),(Point_PTR)(PMesh->pArr+index_b),(Point_PTR)(PMesh->pArr+pTri->i)) < 0)
				{
					index_d = pTri->i;
				}

				break;

			default:
				break;
		}

		if (index_d != -1)
		{
			Point_PTR pa = (Point_PTR)(PMesh->pArr+index_a);
			Point_PTR pb = (Point_PTR)(PMesh->pArr+index_b);
			Point_PTR pd = (Point_PTR)(PMesh->pArr+index_d); //找到了那个C点
			Point_PTR pp = (Point_PTR)(PMesh->pArr+index_p);
			
			if(InCircle( pa, pb, pp, pd) < 0) // not local Delaunay
			{
				flipped = true;
				
				// add new triangle adp,  dbp, remove abp, abd.
				// allocate memory for adp
				TRIANGLE_ARR pT1 = AddTriangleNode(PMesh, pTestTri, pTestTri->i, index_d, pTestTri->k);				
				// allocate memory for dbp
				TRIANGLE_ARR pT2 = AddTriangleNode(PMesh, pT1, index_d, pTestTri->j, index_p);				
				// remove abp
				RemoveTriangleNode(PMesh, pTestTri);
				// remove abd
				RemoveTriangleNode(PMesh, pTri);
				
				/*递归检测运行经常崩！*/
			//	FlipTest(PMesh, pT1); // pNewTestTri satisfies CCW order
			//	FlipTest(PMesh, pT2); // pNewTestTri2  satisfies CCW order

				break;
			}	
		}

		// go to next item	
		pTri = pTri->TNext;
	}
	printf("test succeed\n");
	return flipped;
}

double InCircle(Point_PTR pa, Point_PTR pb, Point_PTR pp, Point_PTR  pd)
{
	double det;
	double alift, blift, plift, bdxpdy, pdxbdy, pdxady, adxpdy, adxbdy, bdxady;

	double adx = pa->x - pd->x;
	double ady = pa->y - pd->y;

	double bdx = pb->x - pd->x;
	double bdy = pb->y - pd->y;

	double pdx = pp->x - pd->x;
	double pdy = pp->y - pd->y;

	bdxpdy = bdx * pdy;
	pdxbdy = pdx * bdy;
	alift = adx * adx + ady * ady;

	pdxady = pdx * ady;
	adxpdy = adx * pdy;
	blift = bdx * bdx + bdy * bdy;

	adxbdy = adx * bdy;
	bdxady = bdx * ady;
	plift = pdx * pdx + pdy * pdy;

	det = alift * (bdxpdy - pdxbdy)
		+ blift * (pdxady - adxpdy)
		+ plift * (adxbdy - bdxady);

	return -det;
}

//把已经插入的点给去除掉！
void InsertOtherPoints(Mesh_PTR Pmesh, int total, int InitialPointSize)
{
	
//	vector<Point_PTR> PointARR;
	int i=0;
	for (int point_index = InitialPointSize; point_index<total+InitialPointSize; point_index++)
	{
	//	printf("point_index:%d \n", point_index);
		Point_PTR pVer = (Point_PTR)(Pmesh->pArr+point_index);
		TRIANGLE_ARR pTri = Pmesh->TriArr;
		TRIANGLE_ARR pTargetTri = NULL;
		while(pTri!=NULL)
		{
			int r = Intriangle(Pmesh,pVer,pTri);
		//	printf("whether in the triangle %d\n", r);
			if(r>0)
			{
				pTargetTri = pTri;
				break;
			}
			pTri = pTri->TNext;
		}
		if(pTargetTri!=NULL)
		{
			printf("the %d points has been inserted\n",i);
			InsertInTriangle( Pmesh, pTargetTri, point_index );
			i++;
		}
	}
	
}

double Distance( Point_PTR px, Point_PTR pa, Point_PTR pb, Point_PTR pc )
{
	double a = (pb->y - pa->y) * (pc->z - pa->z) - (pc->y - pa->y) * (pb->z - pa->z);
	double b = (pb->z - pa->z) * (pc->x - pa->x) - (pc->z - pa->z) * (pb->x - pa->x);
	double c = (pb->x - pa->x) * (pc->y - pa->y) - (pc->x - pa->x) * (pb->y - pa->y);
	double d = -a * pa->x - b * pa->y - c * pa->z;

	double D = abs( a * px->x + b * px->y + c * px->z + d ) / sqrt( a * a + b * b + c * c );

	return D;
}

double Angle( Point_PTR px, Point_PTR pa, Point_PTR pb, Point_PTR pc ){

	double n1n0 = (pb->x - px->x) * (pa->x - px->x) + (pb->y - px->y) * (pa->y - px->y) + (pb->z - px->z) * ( pa->z - px->z);
	double n1n1 = sqrt( (pa->x - px->x) * (pa->x - px->x) + (pa->y - px->y) * (pa->y - px->y) + (pa->z - px->z) * (pa->z - px->z) );
	double n2n2 = sqrt( (pb->x - px->x) * (pb->x - px->x) + (pb->y - px->y) * (pb->y - px->y) + (pb->z - pb->z) * (pb->z - pb->z) );

	double zeta  = acos( n1n0 / (n1n1 - n2n2) );
	
	zeta = zeta/3.1415926 * 180;

	return zeta;
}

double Angle1( Point_PTR px, Point_PTR pa, Point_PTR pb, Point_PTR pc, double d )
{
	double DisXA = sqrt((px->x-pa->x)*(px->x-pa->x)+(px->y-pa->y)*(px->y-pa->y)+(px->z-pa->z)*(px->z-pa->z));
	double DisXB = sqrt((px->x-pb->x)*(px->x-pb->x)+(px->y-pb->y)*(px->y-pb->y)+(px->z-pb->z)*(px->z-pb->z));
	double DisXC = sqrt((px->x-pc->x)*(px->x-pc->x)+(px->y-pc->y)*(px->y-pc->y)+(px->z-pc->z)*(px->z-pc->z));

	double a = (pb->y - pa->y) * (pc->z - pa->z) - (pc->y - pa->y) * (pb->z - pa->z);
	double b = (pb->z - pa->z) * (pc->x - pa->x) - (pc->z - pa->z) * (pb->x - pa->x);
	double c = (pb->x - pa->x) * (pc->y - pa->y) - (pc->x - pa->x) * (pb->y - pa->y);
	d = -a * pa->x - b * pa->y - c * pa->z;

	double Dis = abs( a * px->x + b * px->y + c * px->z + d ) / sqrt( a * a + b * b + c * c );

	double zeta, tempDis = 999, D[3], min; int index;
	D[0] = DisXA;D[1] = DisXB; D[2] = DisXC;
	for(int i = 0; i<3; i++)
	{
		if (D[i]<tempDis)
		{
			min = D[i];
			tempDis= min;
	//		index = i;
		}
	}

	zeta = asin(Dis/min);
/*	
	switch(index)
	{
	case 0: zeta  = acos( min / DisXA );
		break;
	case 1: zeta  = acos( min / DisXA );
		break;
	case 2: zeta  = acos( min / DisXA );	
		break;
	defaut: printf("error\n");
	}*/
	return zeta;

}

void OutputData(char *Outputfile, Mesh_PTR Pmesh)
{
	FILE* fp = fopen(Outputfile, "w");
	if (!fp)
	{
		fprintf(stderr,"Error:%s open failed\n", Outputfile);

		UnInitMesh(Pmesh);
		exit(1);
	}

	
	
	TRIANGLE_ARR pTri = Pmesh->TriArr;
	int* pi;
	int vertex_index;
	int tri_index = 0;
	while(pTri != NULL)	
	{
		pi = &(pTri->i);
		for (int j=0; j<3; j++)	
		{	
			vertex_index = *pi++;		
			fprintf( fp, "%f %f %f\n", (Pmesh->pArr+vertex_index)->x, (Pmesh->pArr+vertex_index)->y,
				(Pmesh->pArr+vertex_index)->z );
		}
		pTri = pTri->TNext;
	}

	fclose(fp);

	UnInitMesh(Pmesh);
}

void UnInitMesh(Mesh_PTR &Pmesh)
{
	// free vertices
	if(Pmesh->pArr != NULL)
		free(Pmesh->pArr);

	// free triangles
	TRIANGLE_ARR pTri = Pmesh->TriArr;
	TRIANGLE_ARR pTemp = NULL;
	while (pTri != NULL)
	{
		pTemp = pTri->TNext;
		free(pTri);
		pTri = pTemp;
	}
}

int Intriangle1(Mesh_PTR Pmesh, Point_PTR pVer, TRIANGLE_ARR pTri) 
{
	int vertex_index;
	Point_PTR pV1, pV2, pV3;
	vertex_index = pTri->i; 
	pV1 = Pmesh->pArr+vertex_index;
	vertex_index = pTri->j;
	pV2 = Pmesh->pArr+vertex_index;
	vertex_index = pTri->k;
	pV3 = Pmesh->pArr+vertex_index;
	int ccw1 = CounterClockWise(pV1, pV2, pVer);
	int ccw2 = CounterClockWise(pV2, pV3, pVer);
	int ccw3 = CounterClockWise(pV3, pV1, pVer);
	
	int r = -1;

	if (ccw1>0 && ccw2>0 && ccw3>0)
	{
		r = 1;
	} 
	else 
	{
		r = 0;
	}
	return r;
}

double SlopeComp( Point_PTR pa, Point_PTR pb, Point_PTR pc)
{
	Point_PTR Point[3],Point1[2]; 
	double a, b, c;
	Point[0] = pa;Point[1] = pb; Point[2] = pc;

	a = sqrt((pa->x-pb->x)*(pa->x-pb->x)+(pa->y-pb->y)*(pa->y-pb->y)+(pa->z-pb->z)*(pa->z-pb->z));
	b = sqrt((pa->x-pc->x)*(pa->x-pc->x)+(pa->y-pc->y)*(pa->y-pc->y)+(pa->z-pc->z)*(pa->z-pc->z));
	c = sqrt((pb->x-pc->x)*(pb->x-pc->x)+(pb->y-pc->y)*(pb->y-pc->y)+(pb->z-pc->z)*(pb->z-pc->z));
	
	double length = a+b+c, p = length/2, area;

	area = p*(p-a)*(p-b)*(p-c);

	double tempDis = 1, D[3], max, height=0, midheight = 0,abheight=0; 
	int index, i , j;
	D[0] = pa->z;D[1] = pb->z; D[2] = pc->z;
	
	for(i = 0; i<3; i++)
	{
		if (D[i]>tempDis)
		{
			max = D[i];
			tempDis= max;
			index = i;
		}
	}
	
	for(i = 0; i<3; i++)
	{
		if(i==index)
		{
			continue;
		}
		height = height + D[i];
	}
	
	midheight = height/2;

	abheight = D[index]-midheight;
	
	for (i = 0; i<3; i++)
	{
		if(i == index)
		{
			continue;
		}
		else
			for (j = 0; j<2; j++)
			{
				Point1[j] = Point[i];
			}
	}
	double bottomLen, triheight, slope;

	bottomLen = sqrt( (Point1[0]->x -Point1[1]->x)*(Point1[0]->x -Point1[1]->x) + (Point1[0]->y -Point1[1]->y)*(Point1[0]->y -Point1[1]->y)+(Point1[0]->z -Point1[1]->z)*(Point1[0]->z -Point1[1]->z));
	
    triheight = area/bottomLen;

	slope = asin(double(abheight/triheight))/3.1415926 * 180;
	
	return slope;

}

Point_PTR MirrorComp(Point_PTR px,Point_PTR pa, Point_PTR pb, Point_PTR pc)
{
	double tempDis = 1, D[3], max; 
	int index, i , j;
	Point_PTR Point[3], TopPoint, MirrPoint;
	
	Point[0] = pa;Point[1] = pb; Point[2] = pc;
	D[0] = pa->z;D[1] = pb->z; D[2] = pc->z;
	
	for(i = 0; i<3; i++)
	{
		if (D[i]>tempDis)
		{
			max = D[i];
			tempDis= max;
			index = i;
		}
	}
	
	TopPoint =Point[index];
	
	MirrPoint->x = 2*TopPoint->x - px->x;
	MirrPoint->y = 2*TopPoint->y - px->y;
	MirrPoint->z = px->z;

	return MirrPoint;
}

void Filtering(vector<Point_PTR> &PointARR,Mesh_PTR Pmesh)
{
	int GroundPointSize = PointARR.size();
	int vertex_index = 0,satisfy = 0 , r=0;
	double EdgeLength = 0;
	for (vertex_index = 0; vertex_index <PointARR.size(); vertex_index++)
	{ 
		Point_PTR pVer = PointARR[vertex_index]; 
		TRIANGLE_ARR pTargetTri = NULL;
		TRIANGLE_ARR pTri = Pmesh->TriArr; 
		while (pTri != NULL)
		{ 
			r = Intriangle1(Pmesh, pVer, pTri);
			printf("the points in the triangle: %d\n", r);
			if(r > 0)
			{
				pTargetTri = pTri;
				break;
			}
			else
			{
				pTri = pTri->TNext; 
			}
		}
		if (r>0)
		{
	//		satisfy = EdgelLengthComp( Pmesh ,pTargetTri, vertex_index);
	//		printf("whether it satisfy? %d\n", satisfy);
	//	if(satisfy)
	//		{	
				InsertInTriangle1( Pmesh, pTargetTri,vertex_index ); 
	//		}
		//	else 
		//	{continue;}
		}
	}
	
}

int EdgelLengthComp(Mesh_PTR Pmesh,TRIANGLE_ARR pTargetTri, int vertex_index)
{
	int index;int satisfy = 1;
	Point_PTR pV1, pV2, pV3,pV;
	double pVV1,pVV2,pVV3,pVV,VV1,VV2,VV3;
	index = pTargetTri->i; 
	pV1 = Pmesh->pArr+index;
	index = pTargetTri->j;
	pV2 = Pmesh->pArr+index;
	index = pTargetTri->k;
	pV3 = Pmesh->pArr+index;

	pV = Pmesh->pArr+vertex_index;

	pVV1 = sqrt((pV->x-pV1->x)*(pV->x-pV1->x) +(pV->y-pV1->y)*(pV->y-pV1->y)+(pV->z-pV1->z)*(pV->z-pV1->z));
	pVV2 = sqrt((pV->x-pV2->x)*(pV->x-pV2->x) +(pV->y-pV2->y)*(pV->y-pV2->y)+(pV->z-pV2->z)*(pV->z-pV2->z));
	pVV3 = sqrt((pV->x-pV3->x)*(pV->x-pV3->x) +(pV->y-pV3->y)*(pV->y-pV3->y)+(pV->z-pV3->z)*(pV->z-pV3->z));

	pVV = Distance( pV, pV1, pV2, pV3);
	
	VV1 = sqrt(pVV1*pVV1-pVV*pVV);
	VV2 = sqrt(pVV2*pVV2-pVV*pVV);
	VV3 = sqrt(pVV3*pVV3-pVV*pVV);

	if(VV1>L && VV2> L && VV3> L )
	{
		return satisfy; 
	}
	else
	{
		satisfy =0;
		return satisfy;
	}
}

void InsertInTriangle1(Mesh_PTR &Pmesh, TRIANGLE_ARR pTargetTri,int vertex_index)
{
	int index_a, index_b, index_c;
	TRIANGLE_ARR pTri = NULL;
	TRIANGLE_ARR pNewTri = NULL;

	pTri = pTargetTri;	
	if(pTri == NULL)
	{
		return;
	}

	// Inset p into target triangle
	index_a = pTri->i;
	index_b = pTri->j;
	index_c = pTri->k;

	// Insert edge pa, pb, pc
	for(int i=0; i<3; i++)
	{
		if(i == 0)
		{ 
			pNewTri = AddTriangleNode1(Pmesh, pTri, index_a, index_b, vertex_index);
		}
		else if(i == 1)
		{
			pNewTri = AddTriangleNode1(Pmesh, pTri, index_b, index_c, vertex_index);
		}
		else
		{
			pNewTri = AddTriangleNode1(Pmesh, pTri, index_c, index_a, vertex_index);
		}

		// go to next item
		if (pNewTri != NULL)
		{
			pTri = pNewTri;  
		}
		else
		{
			pTri = pTri;
		}
	}

	// Get the three sub-triangles 
	pTri = pTargetTri;	
/*	TRIANGLE_ARR pTestTri[3];
	for (int i=0; i< 3; i++)
	{
		pTestTri[i] = pTri->TNext;

		pTri = pTri->TNext;
	}
*/
	// remove the Target Triangle  
	RemoveTriangleNode(Pmesh, pTargetTri); //把大三角形给remove掉了，只剩下连接着的三个小三角形

/*	for (int i=0; i< 2; i++)
	{
		// Flip test
		FlipTest(Pmesh, pTestTri[i]);
	}*/
}

TRIANGLE_ARR AddTriangleNode1 (Mesh_PTR Pmesh,TRIANGLE_ARR TriArr, int i, int j, int k) //改变链表结构不是说可以
{

/*	if(CounterClockWise((Pmesh->pArr+i), (Pmesh->pArr+j), (Pmesh->pArr+k)) == 0)
	{
		return NULL;
	}*/
	//	TRIANGLE_ARR pTempTri = new TRIANGLE;
		//在这里改变了！！为什么我空建立的一个pTemTri，在这里就能让Pmesh混乱呢？
	TRIANGLE_ARR pTempTri = (TRIANGLE_ARR) malloc(sizeof(TRIANGLE));
			
	pTempTri->i = i;
	pTempTri->j = j;
	pTempTri->k = k;

	if (TriArr == NULL)
	{
		Pmesh->TriArr = pTempTri; 
		pTempTri->TNext = NULL;
		pTempTri->TPre = NULL;
	
		(Pmesh->TNUM)++;
	}
	else
	{	
		if (TriArr->TNext == NULL) 
		{
			pTempTri->TNext = TriArr->TNext;
			pTempTri->TPre = TriArr;
			TriArr->TNext = pTempTri;
			(Pmesh->TNUM)++;
		}
		else
		{
			pTempTri->TNext = TriArr->TNext; 
			pTempTri->TPre = TriArr;										 
			TriArr->TNext->TPre = pTempTri;
			TriArr->TNext=pTempTri;

			(Pmesh->TNUM)++;				
		}				
	}

	printf("%d\n",Pmesh->TNUM);
	printf("assign the triangle succeed!\n");

	return pTempTri;
	
}

内测将近40000个点， 时间是33秒！  还是有点慢，里面的路径你们可以自行修改