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