/********************************************************
 *  ██████╗  ██████╗████████╗██╗
 * ██╔════╝ ██╔════╝╚══██╔══╝██║
 * ██║  ███╗██║        ██║   ██║
 * ██║   ██║██║        ██║   ██║
 * ╚██████╔╝╚██████╗   ██║   ███████╗
 *  ╚═════╝  ╚═════╝   ╚═╝   ╚══════╝
 * Geophysical Computational Tools & Library (GCTL)
 *
 * Copyright (c) 2023  Yi Zhang (yizhang-geo@zju.edu.cn)
 *
 * GCTL is distributed under a dual licensing scheme. You can redistribute 
 * it and/or modify it under the terms of the GNU Lesser General Public 
 * License as published by the Free Software Foundation, either version 2 
 * of the License, or (at your option) any later version. You should have 
 * received a copy of the GNU Lesser General Public License along with this 
 * program. If not, see <http://www.gnu.org/licenses/>.
 * 
 * If the terms and conditions of the LGPL v.2. would prevent you from using 
 * the GCTL, please consider the option to obtain a commercial license for a 
 * fee. These licenses are offered by the GCTL's original author. As a rule, 
 * licenses are provided "as-is", unlimited in time for a one time fee. Please 
 * send corresponding requests to: yizhang-geo@zju.edu.cn. Please do not forget 
 * to include some description of your company and the realm of its activities. 
 * Also add information on how to contact you by electronic and paper mail.
 ******************************************************/

#include "geometry3d.h"

#include "string"
#include "cmath"
#include "vector"
#include "map"
#include "algorithm"


double gctl::geometry3d::angle(const point3dc &a, const point3dc &b) //向量夹角
{
	double d = GCTL_SetToBox(-1.0, 1.0, dot(a, b)/(a.module() * b.module()));
	return acos(d);
}

double gctl::geometry3d::triangle_area(const point3dc &a, const point3dc &b, const point3dc &c)
{
	double edge_len[3] = {0.0, 0.0, 0.0};
	edge_len[0] = distance(a, b);
	edge_len[1] = distance(a, c);
	edge_len[2] = distance(b, c);

	double p = 0.5*(edge_len[0] + edge_len[1] + edge_len[2]);
	return sqrt(p*(p-edge_len[0])*(p-edge_len[1])*(p-edge_len[2]));
}

double gctl::geometry3d::tetrahedron_volume(const point3dc &a, const point3dc &b, const point3dc &c, const point3dc &d)
{
    point3dc va, vb, vc;
	va = a - d;
	vb = b - d;
	vc = c - d;
	return GCTL_FABS(dot(cross(va,vb),vc))/6.0;
}

double gctl::geometry3d::dot2line(const point3dc &line_start, const point3dc &line_end, 
	const point3dc &dot)
{
	if (line_start == dot)
		return 0.0;
	else if (line_end == dot)
		return 0.0;
	else if (line_start == line_end)
		return (dot - line_start).module();
	else
	{
		gctl::point3dc out_line = dot - line_start;
		return sin(angle(line_end - line_start, out_line))*out_line.module();
	}
}

gctl::point3dc gctl::geometry3d::dot_on_line(const point3dc &line_start, 
	const point3dc &line_end, const point3dc &out_dot)
{
	gctl::point3dc out_p;
	if (line_start == out_dot) out_p = line_start;
	else if (line_end == out_dot) out_p = line_end;
	else if (line_start == line_end) out_p = line_end;
	else
	{
		gctl::point3dc line = line_end - line_start;
		double mod = dot(line, out_dot - line_start)/line.module();
		out_p = mod * line.normal() + line_start;
	}
	return out_p;
}

double gctl::geometry3d::dot2plane(const point3dc &c, const point3dc &n, const point3dc &d)
{
	//点到平面距离等于vec_{cd}在法线方向的投影的绝对值
	return fabs(dot(n, d - c));
}

void gctl::geometry3d::get_plane_coeff(double x1, double x2, double x3, double y1, double y2, double y3, 
	double z1, double z2, double z3, double &A, double &B, double &C, double &D)
{
	A = (y2 - y1)*(z3 - z1) - (z2 - z1)*(y3 - y1);
	B = (x3 - x1)*(z2 - z1) - (x2 - x1)*(z3 - z1);
	C = (x2 - x1)*(y3 - y1) - (x3 - x1)*(y2 - y1);
	D = -1.0*(A*x1 + B*y1 + C*z1);
	return;
}

void gctl::geometry3d::get_plane_coeff(const point3ds &p1, const point3ds &p2, double &A, double &B, double &C, double &D)
{
	gctl::point3dc c1 = p1.s2c(), c2 = p2.s2c();
	get_plane_coeff(c1.x, c2.x, 0.0, c1.y, c2.y, 0.0, c1.z, c2.z, 0.0, A, B, C, D);
	return;
}

gctl::point3dc gctl::geometry3d::line_on_plane(const point3dc &face_p, const point3dc &face_nor, 
	const point3dc &line_p, const point3dc &line_nor) //平面上的一个点c 平面的外法线矢量 平面外一个点 线方向
{
	gctl::point3dc m;
	if (dot(face_nor,line_nor) != 0)
	{
		double t = dot(face_nor, face_p - line_p)/dot(face_nor, line_nor);
		m = line_p + t*line_nor;
	}
	else throw std::runtime_error("The input line and plane are parallel to each other. From gctl::geometry3d::line_on_plane(...)");
	return m;
}

gctl::point3ds gctl::geometry3d::track_sphere_arc(const point3ds &v1, const point3ds &v2, double arc)
{
	if (fabs(v1.rad - v2.rad) > 1e-10)
	{
		throw std::runtime_error("[gctl::geometry3d::track_sphere_arc] Invalid radius.");
	}

	point3dc p1 = v1.s2c(), p2 = v2.s2c();
	double Phi = angle(p1, p2); // Phi为弧度表示
	point3dc pc = cos(arc)*p1 + sin(arc)/sin(Phi)*(p2 - cos(Phi)*p1);

	return pc.c2s();
}

gctl::point3dc gctl::geometry3d::dot_on_plane(const point3dc &face_dot, const point3dc &face_nor, 
	const point3dc &cal_dot)
{
	double t = (dot(face_dot, face_nor) - dot(cal_dot, face_nor))
	/(face_nor.x*face_nor.x+face_nor.y*face_nor.y+face_nor.z*face_nor.z);
	return cal_dot + t*face_nor;
}

//距离平方反比形式的三角形内插值函数
double gctl::geometry3d::tri_interp_dis(const point3dc &p, const point3dc &p1, const point3dc &p2, 
	const point3dc &p3, double d1, double d2, double d3)
{
	double order = 2;
	double dis1 = (p-p1).module();
	double dis2 = (p-p2).module();
	double dis3 = (p-p3).module();
	double sumDis = 1.0/pow(dis1,order) + 1.0/pow(dis2,order) + 1.0/pow(dis3,order);
	return (d1/pow(dis1,order) + d2/pow(dis2,order) + d3/pow(dis3,order))/sumDis;
}
//球心角平方反比形式的三角形内插值函数
double gctl::geometry3d::tri_interp_ang(const point3dc &p, const point3dc &p1, const point3dc &p2, 
	const point3dc &p3, double d1, double d2, double d3)
{
	double order = 1.5;
	double ang1 = angle(p, p1);
	double ang2 = angle(p, p2);
	double ang3 = angle(p, p3);
	double sumAng = 1.0/pow(ang1,order) + 1.0/pow(ang2,order) + 1.0/pow(ang3,order);
	return d1*(1.0/pow(ang1,order))/sumAng + d2*(1.0/pow(ang2,order))/sumAng + d3*(1.0/pow(ang3,order))/sumAng;
}
//细分三角形面积比形式的三角形内插值函数
double gctl::geometry3d::tri_interp_area(const point3dc &p, const point3dc &p1, const point3dc &p2, 
	const point3dc &p3, double d1, double d2, double d3)
{
	point3dc pp1 = p1 - p;
	point3dc pp2 = p2 - p;
	point3dc pp3 = p3 - p;
	//三角形的面积等于叉乘的1/2 这里只计算比值 所以不需要乘以1/2
	double a1 = cross(pp2,pp3).module();
	double a2 = cross(pp3,pp1).module();
	double a3 = cross(pp1,pp2).module();
	return (d1*a1+d2*a2+d3*a3)/(a1+a2+a3);
}

//距离平方反比形式的三角形内插值函数
double gctl::geometry3d::tet_interp_dis(const point3dc &p, const point3dc &p1, const point3dc &p2, 
	const point3dc &p3, const point3dc &p4, double d1, double d2, double d3, double d4)
{
	double order = 2;
	double w1 = 1.0/(pow((p - p1).module(), order) + GCTL_ZERO);
	double w2 = 1.0/(pow((p - p2).module(), order) + GCTL_ZERO);
	double w3 = 1.0/(pow((p - p3).module(), order) + GCTL_ZERO);
	double w4 = 1.0/(pow((p - p4).module(), order) + GCTL_ZERO);
	return (d1 * w1 + d2 * w2 + d3 * w3 + d4 * w4)/(w1 + w2 + w3 + w4);
}

int gctl::geometry3d::sph2car(const point3ds *sp_ptr, point3dc *cp_ptr)
{
	cp_ptr->x = sp_ptr->rad*cos(GCTL_Pi * sp_ptr->lat/180.0)*cos(GCTL_Pi * sp_ptr->lon/180.0);
	cp_ptr->y = sp_ptr->rad*cos(GCTL_Pi * sp_ptr->lat/180.0)*sin(GCTL_Pi * sp_ptr->lon/180.0);
	cp_ptr->z = sp_ptr->rad*sin(GCTL_Pi * sp_ptr->lat/180.0);
	return 0;
}

int gctl::geometry3d::car2sph(const point3dc *cp_ptr, point3ds *sp_ptr)
{
	sp_ptr->rad = sqrt(pow(cp_ptr->x,2)+pow(cp_ptr->y,2)+pow(cp_ptr->z,2));
	if (fabs(sp_ptr->rad)<GCTL_ZERO) //点距离原点极近 将点置于原点
	{
		sp_ptr->lat = sp_ptr->lon = sp_ptr->rad = 0.0;
		return -1;
	}
	else
	{
		sp_ptr->lat = 90.0 - acos(cp_ptr->z/sp_ptr->rad)*180.0/GCTL_Pi;
		sp_ptr->lon = atan2(cp_ptr->y, cp_ptr->x)*180.0/GCTL_Pi;
		return 0;
	}
}

gctl::point3dc gctl::geometry3d::ellip2car(double lon, double lat, double r, double R)
{
	point3dc v;
	v.x = R*cos(GCTL_Pi*lat/180.0)*cos(GCTL_Pi*lon/180.0);
	v.y = R*cos(GCTL_Pi*lat/180.0)*sin(GCTL_Pi*lon/180.0);
    v.z = r*sin(GCTL_Pi*lat/180.0);
	return v;
}

bool gctl::geometry3d::crossed_tri_facet(const point3dc &origin, const point3dc &direct, 
	const triangle &tri_facet, const bool *active_edge, double cutoff_limit, point3dc *cross_loc)
{
	point3dc p_onplane;
	point3dc face_nor = cross(*tri_facet.vert[1] - *tri_facet.vert[0], *tri_facet.vert[2] - *tri_facet.vert[0], cutoff_limit).normal();
	point3dc dir_nor = direct.normal();
	p_onplane = line_on_plane(*tri_facet.vert[0], face_nor, origin, dir_nor);

	if (p_onplane.valid() && dot(direct, p_onplane - origin) > 0)
	{
		for (int k = 0; k < 3; k++)
		{
			if (active_edge[k] && dot(face_nor, cross(*tri_facet.vert[(k+1)%3] - *tri_facet.vert[k], 
				p_onplane - *tri_facet.vert[k], cutoff_limit)) <= 0.0)
			{
				return false;
			}
			else if (dot(face_nor, cross(*tri_facet.vert[(k+1)%3] - *tri_facet.vert[k], 
				p_onplane - *tri_facet.vert[k], cutoff_limit)) < 0.0)
			{
				return false;
			}
		}

		if (cross_loc != nullptr) cross_loc->set(p_onplane);
		return true;
	}
	else return false;
}

bool gctl::geometry3d::same_side(const point3dc &p1, const point3dc &p2, const point3dc &p3, const point3dc &t1, const point3dc &t2)
{
	point3dc f_nor = cross(p2 - p1, p3 - p1);
	double dot1 = dot(f_nor, t1 - p1);
	double dot2 = dot(f_nor, t2 - p1);
	// still count as the same side if t1 and/or t2 are on the plane
	if (sign(dot1) >= 0 && sign(dot2) >= 0) return true;
	else if (sign(dot1) <= 0 && sign(dot2) <= 0) return true;
	else return false;
}

bool gctl::geometry3d::node_in_tetrahedron(const point3dc &p1, const point3dc &p2, 
	const point3dc &p3, const point3dc &p4, const point3dc &test_p)
{
	return same_side(p1, p2, p3, p4, test_p) && same_side(p2, p3, p4, p1, test_p) && 
		same_side(p3, p4, p1, p2, test_p) && same_side(p4, p1, p2, p3, test_p);
}

/*
bool gctl::geometry3d::node_in_tetrahedron(const point3dc &test_p, const point3dc &p1, 
	const point3dc &p2, const point3dc &p3, const point3dc &p4, double cutoff_limit)
{
	// 计算四面体的中心位置
	point3dc cen = 0.25*(p1 + p2 + p3 + p4);
	// 构造四个三角形
	vertex3dc tet_v[4];
	tet_v[0].set(p1, 0);
	tet_v[1].set(p2, 1);
	tet_v[2].set(p3, 2);
	tet_v[3].set(p4, 3);

	triangle tet_fac[4];
	tet_fac[0].set(tet_v[0], tet_v[1], tet_v[2], 0);
	tet_fac[1].set(tet_v[0], tet_v[1], tet_v[3], 1);
	tet_fac[2].set(tet_v[0], tet_v[2], tet_v[3], 2);
	tet_fac[3].set(tet_v[1], tet_v[2], tet_v[3], 3);
	// 设置四面体的边属性
	matrix<bool> active_edge(4, 3, false);
	active_edge[0][0] = true;
	active_edge[0][1] = true;
	active_edge[0][2] = true;
	active_edge[1][1] = true;
	active_edge[2][2] = true;
	active_edge[2][1] = true;
	// 检测从待检测点过四面体中心的射线与四面体的交点个数
	// 交点个数为奇数 则待检测点在四面体内 否在则在四面体外
	int cross_count = 0;
	point3dc dir = cen - test_p;
	for (int i = 0; i < 4; i++)
	{
		if (crossed_tri_facet(test_p, dir, tet_fac[i], active_edge.get(i), cutoff_limit))
			cross_count++;
	}

	if (pow(-1, cross_count) < 0)
		return true;
	else return false;
}
*/

void gctl::geometry3d::order_triangular_surface(array<triangle> &poly_tri, double cutoff_limit)
{
	if (poly_tri.empty())
	{
		std::string err_str = "The input array is empty. From gctl::geometry3d::order_triangular_surface(...)";
		throw runtime_error(err_str);
	}

	int poly_face_num = poly_tri.size();
	matrix<bool> active_edge(poly_face_num, 3, false);

	std::vector<std::string> edge_index;
	std::vector<std::string>::iterator ie;

	// set active edges
	int small_id, big_id;
	std::string tmp_edge_id, mid_id;
	std::stringstream stemp;
	for (int i = 0; i < poly_face_num; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			small_id = std::min(poly_tri[i].vert[j]->id, poly_tri[i].vert[(j+1)%3]->id);
			big_id   = std::max(poly_tri[i].vert[j]->id, poly_tri[i].vert[(j+1)%3]->id);

			stemp.clear();
			stemp << std::setprecision(16) << small_id;
			stemp >> tmp_edge_id;
			stemp.clear();
			stemp << std::setprecision(16) << big_id;
			stemp >> mid_id;
			tmp_edge_id = tmp_edge_id + " " + mid_id;

			if (edge_index.empty())
			{
				edge_index.push_back(tmp_edge_id);
				active_edge[i][j] = true;
			}
			else
			{
				ie = find(edge_index.begin(), edge_index.end(), tmp_edge_id);
				if (ie == edge_index.end())
				{
					edge_index.push_back(tmp_edge_id);
					active_edge[i][j] = true;
				}
			}
		}
	}

	int cross_count;
	vertex3dc *tmp_vert;
	point3dc tri_cen, tri_nor;
	for (int i = 0; i < poly_face_num; i++)
	{
		tri_cen = 1.0/3.0 * (*poly_tri[i].vert[0] + *poly_tri[i].vert[1] + 
			*poly_tri[i].vert[2]);
		tri_nor = cross(*poly_tri[i].vert[1] - *poly_tri[i].vert[0], 
			*poly_tri[i].vert[2] - *poly_tri[i].vert[0], cutoff_limit).normal();

		cross_count = 0;
		for (int j = 1; j < poly_face_num; j++)
		{
			if (crossed_tri_facet(tri_cen, tri_nor, poly_tri[((i+j)%poly_face_num)], active_edge.get(j), cutoff_limit))
				cross_count++;
		}

		if (pow(-1, cross_count) < 0)
		{
			tmp_vert = poly_tri.at(i).vert[1];
			poly_tri.at(i).vert[1] = poly_tri.at(i).vert[2];
			poly_tri.at(i).vert[2] = tmp_vert;
		}
	}

	edge_index.clear();
	return;
}

void gctl::geometry3d::get_tetra_mesh_common_faces(const array<tetrahedron> &ele, 
	array<triangle> &out_face, array<tetrahedron*> *out_neigh_ptr)
{
	// find edge neighbors
	int ele_size = ele.size();
	std::vector<std::vector<int> > face_n;
	face_n.resize(ele_size);
	for (int i = 0; i < ele_size; i++)
	{
		face_n[i].reserve(8); // 每个顶点的list最多8个
	}

	int small_id, big_id;
	for (int i = 0; i < ele_size; i++)
	{
		for (int j = 0; j < 4; j++)
		{
			if (ele[i].neigh[j] != nullptr) // 邻居必须存在
			{
				// 这里我们总是按小-大排序序号，保证不会输入混乱
				small_id = std::min(ele[i].id, ele[i].neigh[j]->id);
				big_id   = std::max(ele[i].id, ele[i].neigh[j]->id);
				face_n[small_id].push_back(big_id);
			}
		}
	}

	// 去除face_n中的重复输入
	std::vector<int>::iterator pos;
	for (int i = 0; i < ele_size; i++)
	{
		if (!face_n[i].empty()) // 很有可能某个边没有输入
		{
			std::sort(face_n[i].begin(), face_n[i].end()); //对顶点序列由小到大排序
			pos = std::unique(face_n[i].begin(), face_n[i].end()); //获取重复序列开始的位置
			face_n[i].erase(pos, face_n[i].end()); //删除重复点
		}
	}

	// 保存三角形的公共边的列表 首先统计公共边的数量
	int face_num = 0;
	for (int i = 0; i < ele_size; i++)
		face_num += face_n[i].size();
	out_face.resize(face_num);
	array<tetrahedron*> tri_neigh(2*face_num, nullptr); // 每个公共三角形有两个邻接四面体

	int count = 0;
	for (int i = 0; i < ele_size; i++)
	{
		if (!face_n[i].empty())
		{
			for (int j = 0; j < face_n[i].size(); j++)
			{
				out_face[count].id = count;
				//out_face[count].tet_ptr[0] = ele.get(i); // triangle现在没有tet_ptr变量了
				//out_face[count].tet_ptr[1] = ele.get(face_n[i][j]);
				tri_neigh[2*count] = ele.get(i);
				tri_neigh[2*count+1] = ele.get(face_n[i][j]);
				count++;
			}
		}
	}

	// 最后找到面的三个顶点
	// 如果相邻的两个四面体中的其中一个四面体的一个顶点不等于另一个四面体的所有顶点，则这个四面体的
	// 另三个顶点连成的边即为公共面
	int tmp_int;
	for (int i = 0; i < face_num; i++)
	{
		for (int j = 0; j < 4; j++)
		{
			tmp_int = tri_neigh[2*i]->vert[j]->id;
			if (tmp_int != tri_neigh[2*i+1]->vert[0]->id && 
				tmp_int != tri_neigh[2*i+1]->vert[1]->id && 
				tmp_int != tri_neigh[2*i+1]->vert[2]->id && 
				tmp_int != tri_neigh[2*i+1]->vert[3]->id)
			{
				out_face[i].vert[0] = tri_neigh[2*i]->vert[(j+1)%4];
				out_face[i].vert[1] = tri_neigh[2*i]->vert[(j+2)%4];
				out_face[i].vert[2] = tri_neigh[2*i]->vert[(j+3)%4];
				break;
			}
		}
	}

	if (out_neigh_ptr != nullptr)
	{
		out_neigh_ptr->resize(2*face_num, nullptr);
		for (int i = 0; i < 2*face_num; i++)
		{
			out_neigh_ptr->at(i) = tri_neigh[i];
		}
	}

	//清除向量
	destroy_vector(face_n);
	tri_neigh.clear();
	return;
}

size_t gctl::geometry3d::sort_node_number(const array<tetrahedron> &ele)
{
	if (ele.empty())
	{
		std::string err_str = "The input array is empty. From gctl::geometry3d::sort_node_number(...)";
		throw runtime_error(err_str);
	}

	int ele_size = ele.size();
	std::vector<int> node_index;
	node_index.reserve(4*ele_size);
	for (int i = 0; i < ele_size; i++)
	{
		for (int j = 0; j < 4; j++)
		{
			node_index.push_back(ele[i].vert[j]->id);
		}
	}

	std::vector<int>::iterator pos;
	std::sort(node_index.begin(), node_index.end()); //对顶点序列由小到大排序
	pos = std::unique(node_index.begin(), node_index.end()); //获取重复序列开始的位置
	node_index.erase(pos, node_index.end()); //删除重复点

	size_t out_num = node_index.size();
	destroy_vector(node_index);
	return out_num;
}

void gctl::geometry3d::sort_node_neighbor(const array<tetrahedron> &ele, 
	std::vector<std::vector<tetrahedron*> > &neigh_list, size_t *node_nump)
{
	size_t node_size;
	if (node_nump != nullptr) node_size = *node_nump;
	else node_size = sort_node_number(ele);

	if (!neigh_list.empty())
	{
		for (int i = 0; i < neigh_list.size(); i++)
		{
			neigh_list[i].clear();
		}
		neigh_list.clear();
	}
	neigh_list.resize(node_size);
	for (int i = 0; i < ele.size(); i++)
	{
		for (int j = 0; j < 4; j++)
		{
			neigh_list[ele[i].vert[j]->id].push_back(ele.get(i));
		}
	}
	return;
}


void gctl::geometry3d::sort_tet_neighbor(array<tetrahedron> &ele, size_t *node_nump)
{
	// we firstly get node neighbors
	std::vector<std::vector<tetrahedron*> > node_neighs;
	sort_node_neighbor(ele, node_neighs, node_nump);

	int loop_size, equal_num;
	for (int i = 0; i < node_neighs.size(); i++)
	{
		loop_size = node_neighs[i].size();
		for (int j = 0; j < loop_size-1; j++)
		{
			for (int k = j+1; k < loop_size; k++)
			{
				equal_num = 0;
				for (int p = 0; p < 4; p++)
				{
					for (int q = 0; q < 4; q++)
					{
						if (node_neighs[i][j]->vert[p] == node_neighs[i][k]->vert[q])
						{
							equal_num++;
							break;
						}
					}
				}

				if (equal_num == 3)
				{
					for (int p = 0; p < 4; p++)
					{
						if (node_neighs[i][j]->neigh[p] == nullptr)
						{
							node_neighs[i][j]->neigh[p] = node_neighs[i][k];
							break;
						}
						else if (node_neighs[i][j]->neigh[p] == node_neighs[i][k])
						{
							break;
						}
					}

					for (int p = 0; p < 4; p++)
					{
						if (node_neighs[i][k]->neigh[p] == nullptr)
						{
							node_neighs[i][k]->neigh[p] = node_neighs[i][j];
							break;
						}
						else if (node_neighs[i][k]->neigh[p] == node_neighs[i][j])
						{
							break;
						}
					}
				}
			}
		}
	}

	destroy_vector(node_neighs);
	return;
}

// 使用平面切割三角网络
void gctl::geometry3d::cut_triangular_mesh(const array<triangle> &in_eles, const point3dc &nor, 
	const point3dc &surf, array<vertex3dc> &out_nodes, array<triangle> &out_eles)
{
	bool created;
	point3dc line_nor, tmp_p, tmp_p2;
	vertex3dc *tmp_vert, *tmp_vert2;
	triangle tmp_tri3d;
	std::vector<triangle> ele_index;
	std::vector<vertex3dc*> extra_nodes; // 注意这里是新生的顶点的指针向量

	int up_count; // 统计三角形顶点在平面上侧的个数
	for (int i = 0; i < in_eles.size(); ++i)
	{
		up_count = 0;
		for (int j = 0; j < 3; ++j)
		{
			// 注意这里包含等号 即点在平面上也等于在面的上侧
			// 此种设计会导致一点或两点在面上，其他面在下侧也会被记入在内
			if (dot(nor, *in_eles[i].vert[j] - surf) >= 0) up_count++;
		}

		if (up_count == 3) // 全在上侧 添加到输出列表
		{
			tmp_tri3d = in_eles[i];
			tmp_tri3d.id = ele_index.size();
			ele_index.push_back(tmp_tri3d);
		}

		if (up_count == 2) // 两个点在上侧或者面上 计算两个交点的位置添加到顶点列表 并新建两个三角形
		{
			for (int j = 0; j < 3; ++j)
			{
				if (dot(nor, *in_eles[i].vert[j] - surf) < 0)
				{
					line_nor = (*in_eles[i].vert[(j+1)%3] - *in_eles[i].vert[j]).normal();
					tmp_p = geometry3d::line_on_plane(surf, nor.normal(), 
						*in_eles[i].vert[j], line_nor); // 计算边与平面的交点

					created = false;
					tmp_p2.x = in_eles[i].vert[(j+1)%3]->x;
					tmp_p2.y = in_eles[i].vert[(j+1)%3]->y;
					tmp_p2.z = in_eles[i].vert[(j+1)%3]->z;
					if (!isequal(tmp_p, tmp_p2, 1e-6)) // 如果交点1等于顶点eles[i].vert[(j+1)%3] 不需要新建顶点以及三角形
					{
						// 检查交点是否已经存在
						for (int e = 0; e < extra_nodes.size(); ++e)
						{
							tmp_p2.x = extra_nodes[e]->x;
							tmp_p2.y = extra_nodes[e]->y;
							tmp_p2.z = extra_nodes[e]->z;
							if (isequal(tmp_p, tmp_p2, 1e-6))
							{
								tmp_vert = extra_nodes[e];
								created = true;
								break;
							}
						}

						// 确定点不存在则新建
						if (!created)
						{
							tmp_vert = new vertex3dc;
							tmp_vert->id = 0; // 先统一把顶点索引设置为0 后面再改
							tmp_vert->x  = tmp_p.x;
							tmp_vert->y  = tmp_p.y;
							tmp_vert->z  = tmp_p.z;
							extra_nodes.push_back(tmp_vert);
						}

						tmp_tri3d.id = ele_index.size();
						tmp_tri3d.vert[0] = tmp_vert;
						tmp_tri3d.vert[1] = in_eles[i].vert[(j+1)%3];
						tmp_tri3d.vert[2] = in_eles[i].vert[(j+2)%3];
						ele_index.push_back(tmp_tri3d);
					}
					else tmp_vert = in_eles[i].vert[(j+1)%3]; // 注意即使不新建顶点也要对临时顶点赋值

					line_nor = (*in_eles[i].vert[(j+2)%3] - *in_eles[i].vert[j]).normal();
					tmp_p = geometry3d::line_on_plane(surf, nor.normal(), 
						*in_eles[i].vert[j], line_nor); // 计算边与平面的交点

					created = false;
					tmp_p2.x = in_eles[i].vert[(j+2)%3]->x;
					tmp_p2.y = in_eles[i].vert[(j+2)%3]->y;
					tmp_p2.z = in_eles[i].vert[(j+2)%3]->z;
					if (!isequal(tmp_p, tmp_p2, 1e-6)) // 如果交点1等于顶点eles[i].vert[(j+2)%3] 不需要新建顶点以及三角形
					{
						for (int e = 0; e < extra_nodes.size(); ++e)
						{
							tmp_p2.x = extra_nodes[e]->x;
							tmp_p2.y = extra_nodes[e]->y;
							tmp_p2.z = extra_nodes[e]->z;
							if (isequal(tmp_p, tmp_p2, 1e-6))
							{
								tmp_vert2 = extra_nodes[e];
								created = true;
								break;
							}
						}

						if (!created)
						{
							tmp_vert2 = new vertex3dc;
							tmp_vert2->id = 0; // 先统一把顶点索引设置为0 后面再改
							tmp_vert2->x  = tmp_p.x;
							tmp_vert2->y  = tmp_p.y;
							tmp_vert2->z  = tmp_p.z;
							extra_nodes.push_back(tmp_vert2);
						}

						// 新建三角形
						tmp_tri3d.id = ele_index.size();
						tmp_tri3d.vert[0] = in_eles[i].vert[(j+2)%3];
						tmp_tri3d.vert[1] = tmp_vert2;
						tmp_tri3d.vert[2] = tmp_vert;
						ele_index.push_back(tmp_tri3d);
					}

					break;
				}
			}
		}

		if (up_count == 1)
		{
			for (int j = 0; j < 3; ++j)
			{
				if (dot(nor, *in_eles[i].vert[j] - surf) > 0) // 找到面上上侧的点 注意这里不包含等号 因为我们需要点完全在面的上面 不包括刚好在面上的情况
				{
					line_nor = (*in_eles[i].vert[(j+1)%3] - *in_eles[i].vert[j]).normal();
					tmp_p = geometry3d::line_on_plane(surf, nor.normal(), 
						*in_eles[i].vert[j], line_nor); // 计算边与平面的交点

					created = false;
					if (!created)
					{
						for (int e = 0; e < extra_nodes.size(); ++e)
						{
							tmp_p2.x = extra_nodes[e]->x;
							tmp_p2.y = extra_nodes[e]->y;
							tmp_p2.z = extra_nodes[e]->z;
							if (isequal(tmp_p, tmp_p2, 1e-6))
							{
								tmp_vert = extra_nodes[e];
								created = true;
								break;
							}
						}
					}

					if (!created)
					{
						tmp_vert = new vertex3dc;
						tmp_vert->id = 0; // 先统一把顶点索引设置为0 后面再改
						tmp_vert->x  = tmp_p.x;
						tmp_vert->y  = tmp_p.y;
						tmp_vert->z  = tmp_p.z;
						extra_nodes.push_back(tmp_vert);
					}
					
					line_nor = (*in_eles[i].vert[(j+2)%3] - *in_eles[i].vert[j]).normal();
					tmp_p = geometry3d::line_on_plane(surf, nor.normal(), 
						*in_eles[i].vert[j], line_nor); // 计算边与平面的交点
					// 新建顶点 把顶点的指针交给 extra_nodes 保管
					created = false;
					if (!created)
					{
						for (int e = 0; e < extra_nodes.size(); ++e)
						{
							tmp_p2.x = extra_nodes[e]->x;
							tmp_p2.y = extra_nodes[e]->y;
							tmp_p2.z = extra_nodes[e]->z;
							if (isequal(tmp_p, tmp_p2, 1e-6))
							{
								tmp_vert2 = extra_nodes[e];
								created = true;
								break;
							}
						}
					}

					if (!created)
					{
						tmp_vert2 = new vertex3dc;
						tmp_vert2->id = 0; // 先统一把顶点索引设置为0 后面再改
						tmp_vert2->x  = tmp_p.x;
						tmp_vert2->y  = tmp_p.y;
						tmp_vert2->z  = tmp_p.z;
						extra_nodes.push_back(tmp_vert2);
					}

					// 新建三角形
					tmp_tri3d.id = ele_index.size();
					tmp_tri3d.vert[0] = in_eles[i].vert[j];
					tmp_tri3d.vert[1] = tmp_vert;
					tmp_tri3d.vert[2] = tmp_vert2;
					ele_index.push_back(tmp_tri3d);

					break; // 完成跳出
				}
			}
		}
	}

	//整理出新的顶点与元素列表
	std::vector<vertex3dc*> node_index;
	out_eles.resize(ele_index.size());
	for (int i = 0; i < ele_index.size(); ++i)
	{
		out_eles[i] = ele_index[i];
		out_eles[i].id = i;
		for (int j = 0; j < 3; ++j)
		{
			node_index.push_back(ele_index[i].vert[j]);
		}
	}

	std::vector<vertex3dc*>::iterator pos;
	std::sort(node_index.begin(), node_index.end()); //对顶点序列由小到大排序
	pos = std::unique(node_index.begin(), node_index.end()); //获取重复序列开始的位置
	node_index.erase(pos, node_index.end()); //删除重复点

	std::map<vertex3dc*, vertex3dc*> vert_map;
	out_nodes.resize(node_index.size());
	for (int i = 0; i < node_index.size(); ++i)
	{
		out_nodes[i] = *node_index[i];
		out_nodes[i].id = i;
		vert_map[node_index[i]] = out_nodes.get(i); // 建立新旧顶点的映射
	}

	for (int i = 0; i < out_eles.size(); ++i)
	{
		for (int j = 0; j < 3; ++j)
		{
			out_eles[i].vert[j] = vert_map[out_eles[i].vert[j]]; // 将旧的顶点映射为新的顶点
		}
	}

	// 手动删除新建的顶点
	for (int i = 0; i < extra_nodes.size(); ++i)
	{
		delete extra_nodes[i];
	}
	return;
}