WireRouting/lib/BVH.cpp

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

BVH bvh;

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,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 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,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;
}
*/
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);
}