#include "stdafx.h"
#include "ReadXML.h"
#include<cmath>
#include<vector>
#include<queue>
#include<map>
#include<iostream>
#include<fstream>
#include<sstream>
using namespace std;

inline bool PointInTri(Point3 &P,Point3 &P0,Point3 &P1,Point3 &P2)
{
    double area1=Area2(P,P0,P1);
    double area2=Area2(P,P1,P2);
    double area3=Area2(P,P2,P0);
	//cout<<area1<<"  "<<area2<<"  "<<area3<<"  "<<Area2(P0,P1,P2)<<"  "<<Area2(P2,P0,P1)<<endl;
    return dcmp(area1+area2+area3-Area2(P0,P1,P2))==0;
}
struct Triangle{
	P p[3];
	inline void getbox(P& _min,P& _max){
		_min=P(1e9,1e9,1e9);
		_max=P(-1e9,-1e9,-1e9);
		for(int i=0;i<3;i++){
			_min.x=min(_min.x,p[i].x);
			_min.y=min(_min.y,p[i].y);
			_min.z=min(_min.z,p[i].z);

			_max.x=max(_max.x,p[i].x);
			_max.y=max(_max.y,p[i].y);
			_max.z=max(_max.z,p[i].z);
		}
	}
	void print(){
		p[0].print("v1");
		p[1].print("v2");
		p[2].print("v3");
		return;
	}
};

bool cmpt0(const Triangle &a,const Triangle &b){
	double cena=0,cenb=0;
	for(int i=0;i<3;i++){
		cena+=a.p[i].x;
		cenb+=b.p[i].x;
	}
	return cena<cenb;
}
bool cmpt1(const Triangle &a,const Triangle &b){
	double cena=0,cenb=0;
	for(int i=0;i<3;i++){
		cena+=a.p[i].y;
		cenb+=b.p[i].y;
	}
	return cena<cenb;
}
bool cmpt2(const Triangle &a,const Triangle &b){
	double cena=0,cenb=0;
	for(int i=0;i<3;i++){
		cena+=a.p[i].z;
		cenb+=b.p[i].z;
	}
	return cena<cenb;
}

struct BVH{
	int trinum,tot,root;

	Triangle tri[MaxSTL];
	struct AABB {
  
        P _min; //左下角顶点
        P _max; //右上角顶点
		int l,r;
        AABB() {l=0;r=0;}
        AABB(const P& min, const P& max) { _min = min; _max = max;}

   
		inline bool insig(double x,double a,double b){
			return x>=a&&x<=b;
		}
		inline bool inbox(const P &p){
			return p.x>=_min.x&&p.x<=_max.x
				&&p.y>=_min.y&&p.y<=_max.y
				&&p.z>=_min.z&&p.z<=_max.z;
		}

        //判断射线是否在t区间
        inline bool hit(P &p1,P &p2) {
			// 1.0 / direction
			if(inbox(p1))return true;
			if(inbox(p2))return true;
			double t0=-1e9,t1=1e9;

			P dire=p1-p2;
			for(int i=0;i<3;i++){
				if(dcmp(dire.get(i))==0){
					if(!insig(p1.get(i),_min.get(i),_max.get(i)))return false;
				}
				else {
					double t00=(_min.get(i)-p1.get(i))/dire.get(i);
					double t11=(_max.get(i)-p1.get(i))/dire.get(i);
					if(t00>t11)swap(t00,t11);
					t0=max(t0,t00);
					t1=min(t1,t11);
				}
			}
			return t0<=t1;
			
		}
		
	}ab[400010];
	BVH(){
		trinum=tot=root=0;
	}
	inline void update(int x){
		AABB &a=ab[x];
		a._min=P(1e9,1e9,1e9);
		a._max=P(-1e9,-1e9,-1e9);
		if(ab[x].l){
			AABB &b=ab[ab[x].l];
			a._min.x=min(a._min.x,b._min.x);
			a._min.y=min(a._min.y,b._min.y);
			a._min.z=min(a._min.z,b._min.z);

			a._max.x=max(a._max.x,b._max.x);
			a._max.y=max(a._max.y,b._max.y);
			a._max.z=max(a._max.z,b._max.z);
		}
		if(ab[x].r){
			AABB &b=ab[ab[x].r];
			a._min.x=min(a._min.x,b._min.x);
			a._min.y=min(a._min.y,b._min.y);
			a._min.z=min(a._min.z,b._min.z);

			a._max.x=max(a._max.x,b._max.x);
			a._max.y=max(a._max.y,b._max.y);
			a._max.z=max(a._max.z,b._max.z);
		}
	}
	

	void build(int &x,int l,int r,int k){
		x=++tot;
		if(l==r){
			tri[l].getbox(ab[x]._min,ab[x]._max);
			//cout<<x<<"  "<<l<<endl;
			
			return;
		}
		int mid=(l+r)/2;
		if(k==0)nth_element(tri+l,tri+mid,tri+r+1,cmpt0);
		else if(k==1)nth_element(tri+l,tri+mid,tri+r+1,cmpt1);
		else if(k==2)nth_element(tri+l,tri+mid,tri+r+1,cmpt2);

		
		build(ab[x].l,l,mid,(k+1)%3);
		build(ab[x].r,mid+1,r,(k+1)%3);
		update(x);
		//cout<<x<<"  "<<ab[x].l<<"  "<<ab[x].r<<"  "<<l<<"  "<<r<<endl;
		//ab[x]._min.print("_min");
		//ab[x]._max.print("_max");
		return;
	}
	
	
	
	inline bool TriSegIntersection(Point3& P0,Point3& P1,Point3& P2,Point3& A,Point3& B)
	{
		 
	    Vector3 n=Cross(P1-P0,P2-P0);
	    if(dcmp(Dot(n,B-A))==0)
	        return false;//线段 AB 和平面 P0P1P2 平行或共面
	    else
	    {
	        double t=Dot(n,P0-A)/Dot(n,B-A);
	        if(dcmp(t)<0||dcmp(t-1)>0)
	            return false;//交点不在线段 AB 上
	        P p=A+(B-A)*t;
	        return PointInTri(p,P0,P1,P2);//判断交点是否在三角形 P0-P1-P2 内 
	    } 
	}
	inline bool TriSegIntersection(Triangle &t,Point3 &A,Point3 &B)
	{
		P P0=t.p[0],P1=t.p[1],P2=t.p[2];
	    Vector3 n=Cross(P1-P0,P2-P0);
	    if(dcmp(Dot(n,B-A))==0)
	        return false;//线段 AB 和平面 P0P1P2 平行或共面
	    else
	    {
	        double t=Dot(n,P0-A)/Dot(n,B-A);
	        if(dcmp(t)<0||dcmp(t-1)>0)
	            return false;//交点不在线段 AB 上
			if(dcmp(t)==0)t=0;
			if(dcmp(t-1)==0)t=1;
	        P p=A+(B-A)*t;
	        return PointInTri(p,P0,P1,P2);//判断交点是否在三角形 P0-P1-P2 内 
	    } 
	}
	/*
	bool TriSegIntersection(Triangle t,Point3 A,Point3 B,int mood)
	{
		P P0=t.p[0],P1=t.p[1],P2=t.p[2];
	    Vector3 n=Cross(P1-P0,P2-P0);
		if(dcmp(Dot(n,B-A))==0){
			if(mood)cout<<"平行"<<endl;
	        return false;//线段 AB 和平面 P0P1P2 平行或共面
			
		}
	    else
	    {
	        double t=Dot(n,P0-A)/Dot(n,B-A);
			if(dcmp(t)<0||dcmp(t-1)>0){
				if(mood)cout<<"交点不在AB上"<<t<<endl;
	            return false;//交点不在线段 AB 上
			}
			if(dcmp(t)==0)t=0;
			if(dcmp(t-1)==0)t=1;
	        P p=A+(B-A)*t;
			p.print("p");
	        return PointInTri(p,P0,P1,P2);//判断交点是否在三角形 P0-P1-P2 内 
	    } 
	}
	*/
	bool query(int x,int l,int r,P &A,P &B){
		//cout<<x<<"  "<<l<<"  "<<r<<"  "<<endl;
		if(l==r){
			//tri[l].print();
			
			//cout<<TriSegIntersection(tri[l],A,B,1)<<endl;
			return TriSegIntersection(tri[l],A,B);
		}
		if(!ab[x].hit(A,B))return false;
		int mid=(l+r)/2;
		if(ab[x].l&&query(ab[x].l,l,mid,A,B))return true;
		if(ab[x].r&&query(ab[x].r,mid+1,r,A,B))return true;
		return false;
	}
	/*
	bool iscollect(P A,P B,int mood){
		if(mood==0)return query(1,0,trinum,A,B);
		for(int i=0;i<=trinum;i++)
			if(TriSegIntersection(tri[i],A,B))return true;
		return false;
	}
	*/

	inline bool iscollect(P &A,P &B){
		return false;
		return query(1,0,trinum,A,B);
		
	}
	inline void read_stl(string filename){
		ifstream infile;
		infile.open(filename.c_str());//XXXX是csv的路径
	
	    string s;
		int cnt=0;
		while(getline(infile,s))
			{
				//cout<<s<<endl;
				//cout<<trinum<<"  "<<cnt<<endl;
				istringstream sin(s); //将整行字符串line读入到字符串流istringstream中
				vector<string> fields; //声明一个字符串向量
				string field;
				while(sin>>field){
					if(field=="vertex"){
						if(cnt==3){
							trinum++;
							cnt=0;
						}
						sin>>tri[trinum].p[cnt].x>>tri[trinum].p[cnt].y>>tri[trinum].p[cnt].z;
						cnt++;
					}
				}
				//cout<<fields[0]<<endl;
				
		}
		cout<<"共有"<<trinum+1<<"个三角形面片"<<endl;

		build(root,0,trinum,0);
	}
}bvh;