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.
 
 
 

289 lines
6.5 KiB

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