You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 

298 lines
6.2 KiB

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