WireRouting/WireRouting_DLL/BVH.cpp

#include "pch.h" // use stdafx.h in Visual Studio 2017 and earlier
#include "BVH.h"


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);
	return dcmp(area1 + area2 + area3 - Area2(P0, P1, P2)) == 0;
}


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


void Triangle::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 Triangle::print(){
	p[0].print("v1");
	p[1].print("v2");
	p[2].print("v3");
	return;
}


O_AABB::O_AABB() { l = 0; r = 0; }

O_AABB::O_AABB(const P& min, const P& max) { _min = min; _max = max; }

bool O_AABB::insig(double x, double a, double b) {
	return x >= a && x <= b;
}

bool O_AABB::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;
}

bool O_AABB::hit(P& p1, P& p2) {
	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;
}



BVH::BVH() { trinum = tot = root = 0; }

void BVH::update(int x) {
	O_AABB& a = ab[x];
	a._min = P(1e9, 1e9, 1e9);
	a._max = P(-1e9, -1e9, -1e9);
	if (ab[x].l) {
		O_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) {
		O_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 BVH::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;
}

bool BVH::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 �� 
	}
}

bool BVH::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 BVH::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 BVH::iscollect(P& A, P& B) {
	return false;
	return query(1, 0, trinum, A, B);
}

void BVH::read_stl(string filename) {
	ifstream infile;
	infile.open(filename.c_str());//XXXX��csv��·��
	string s;
	int cnt = 0;
	while (getline(infile, s))
	{
		istringstream sin(s);
		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 << "����" << trinum + 1 << "����������Ƭ" << endl;

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