WireRouting/include/BVH.h

#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;