//这是头文件
#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
#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秒! 还是有点慢,里面的路径你们可以自行修改