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.
1049 lines
23 KiB
1049 lines
23 KiB
/*
|
|
inline void addPath(Path &path,bool isclips,bool isclipt){
|
|
if(path.points.empty())return;
|
|
addPath(path);
|
|
int s=getid(path.points[0]);
|
|
int t=getid(path.points[path.points.size()-1]);
|
|
if(isclips)endpoint[s]=0;
|
|
if(isclipt)endpoint[t]=0;
|
|
}
|
|
|
|
|
|
|
|
void dfs(int x,bool root,double rdis,int fa){
|
|
vis[x]=bfs;
|
|
bundles[bundleid].path.push_back(points[x]);
|
|
|
|
if(!endpoint[x]&&edge[x].size()==1){
|
|
cout<<"warning"<<endl;
|
|
points[x].print("warning");
|
|
|
|
}
|
|
|
|
|
|
if(edge[x].size()==1){
|
|
if(!root){
|
|
bundles[bundleid].end_node=points[x];
|
|
bundleid++;
|
|
bundles[bundleid].clear();
|
|
return;
|
|
}
|
|
}
|
|
int minv=-1;
|
|
double maxreverse=-1;
|
|
for(int i=0;i<edge[x].size();i++){
|
|
int y=edge[x][i].to;
|
|
if(vis[y]!=bfs){
|
|
double reverse=0;
|
|
P dir=dirP(points[x]);
|
|
if(fa!=0)reverse+=pi/2-Angle(points[x]-points[fa],dir);
|
|
|
|
reverse+=pi/2-Angle(points[y]-points[x],dir);
|
|
if(reverse<=0)reverse*=-1;
|
|
|
|
|
|
|
|
if(root||maxreverse<reverse){
|
|
maxreverse=reverse;
|
|
minv=y;
|
|
bundles[bundleid].wirelist=edge[x][i].wirenames;
|
|
bundles[bundleid].d=edge[x][i].d;
|
|
}
|
|
if(minv==-1){
|
|
cout<<reverse<<endl;
|
|
cout<<maxreverse<<endl;
|
|
cout<<points[x].isend<<endl;
|
|
cout<<points[x].x<<" "<<points[x].y<<" "<<points[x].z<<endl;
|
|
cout<<Length(dir)<<" "<<Length(points[y]-points[x]);
|
|
if(fa!=0)cout<<" "<<Length(points[x]-points[fa])<<endl;
|
|
while(1);
|
|
}
|
|
|
|
}
|
|
}
|
|
int thisBranchPoint=-1;
|
|
if(minv!=-1){
|
|
if(edge[x].size()>2){
|
|
branchPointNum++;
|
|
thisBranchPoint=branchPointNum;
|
|
|
|
int cid=pclipSet->clipid[points[x]];
|
|
|
|
BranchPoint bp;
|
|
bp.id=branchPointNum;bp.position=10;
|
|
bp.referClip=(pclipSet->c[cid]).name;
|
|
bundles[bundleid].branchPointSet.push_back(bp);
|
|
}
|
|
bundles[bundleid].length+=distan1(points[x],points[minv]);
|
|
|
|
dfs(minv,false,bundles[bundleid].length,x);
|
|
}
|
|
|
|
for(int i=0;i<edge[x].size();i++){
|
|
int y=edge[x][i].to;
|
|
if(vis[y]!=bfs){
|
|
if(y!=minv){
|
|
//bundles[bundleid].path.push_back(points[x]);
|
|
//bundles[bundleid].start_node=points[x];
|
|
bundles[bundleid].length=0;
|
|
bundles[bundleid].wirelist=edge[x][i].wirenames;
|
|
bundles[bundleid].d=edge[x][i].d;
|
|
bundles[bundleid].startBranchPoint=thisBranchPoint;
|
|
bundles[bundleid].start_node=points[x];
|
|
bundles[bundleid].length+=distan(points[x],points[y]);
|
|
dfs(y,false,bundles[bundleid].length,x);
|
|
}
|
|
else continue;
|
|
|
|
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
bool dfsh(int x){
|
|
int huan=0;
|
|
vis[x]=bfs;
|
|
for(int i=0;i<edge[x].size();i++){
|
|
int y=edge[x][i].to;
|
|
if(vis[y]!=bfs){
|
|
if(dfsh(y))return true;
|
|
}
|
|
else huan++;
|
|
}
|
|
if(huan>=2)return true;
|
|
return false;
|
|
}
|
|
bool testhuan(){
|
|
bfs++;
|
|
for(int i=1;i<=cnt;i++){
|
|
if(vis[i]!=bfs&&dfsh(i))return true;
|
|
}
|
|
return false;
|
|
}
|
|
*/
|
|
|
|
/*
|
|
|
|
//����������Ϣ�Զ�����bundle
|
|
inline void buildBundle(){
|
|
bfs++;
|
|
bundleid=1;
|
|
bundles[bundleid].clear();
|
|
for(int i=1;i<=cnt;i++){
|
|
//cout<<"bundle size "<<bundles[bundleid].path.size()<<endl;
|
|
if(vis[i]!=bfs&&edge[i].size()==1)dfs(i,true,0,0);
|
|
}
|
|
}
|
|
|
|
|
|
//�������ݿ���������λ�ô������Ƶ�ͨ��
|
|
inline void createChannel(){
|
|
Clip *c = pclipSet->c;
|
|
int clipNum=pclipSet->clipnum;
|
|
for(int i=1;i<=clipNum;i++){
|
|
int cnt=0;
|
|
for(int j=0;j<c[i].channelEdge.size();j++){
|
|
int cid=c[i].channelEdge[j];
|
|
if(pclipSet->bothConnect(i,cid))cnt++;
|
|
}
|
|
if(cnt==1){
|
|
Path path;
|
|
path.dia=5;
|
|
int lastCid=0;
|
|
int cid=i;
|
|
while(cid){
|
|
bool flag=true;
|
|
path.points.push_back(c[cid].coord);
|
|
for(int j=0;j<c[cid].channelEdge.size();j++){
|
|
int cid2=c[cid].channelEdge[j];
|
|
if(cid2==lastCid)continue;
|
|
if(pclipSet->bothConnect(cid,cid2)){
|
|
lastCid=cid;
|
|
cid=cid2;
|
|
flag=false;
|
|
break;
|
|
}
|
|
}
|
|
if(flag)break;
|
|
}
|
|
addPath(path);
|
|
}
|
|
}
|
|
buildBundle();
|
|
|
|
}
|
|
|
|
|
|
inline void readBundle(string sourcefile){
|
|
ifstream ifs;
|
|
ifs.open(sourcefile.c_str(), ios::in);
|
|
int pnum=0;
|
|
double d=0;
|
|
int cnt=0;
|
|
|
|
while(ifs>>pnum>>d){
|
|
cnt++;
|
|
|
|
for(int i=1;i<=pnum;i++){
|
|
P tp;
|
|
double quality;
|
|
ifs>>tp.x>>tp.y>>tp.z>>tp.dx>>tp.dy>>tp.dz>>quality;
|
|
bundles[cnt].path.push_back(tp);
|
|
bundles[cnt].dia=d;
|
|
|
|
}
|
|
bundles[cnt].d.push_back(d);
|
|
if(pnum==-1)break;
|
|
}
|
|
int cid,bnum;
|
|
while(ifs>>cid>>bnum){
|
|
int bid;
|
|
cout<<cid<<" "<<bnum<<endl;
|
|
for(int i=1;i<=bnum;i++){
|
|
ifs>>bid;
|
|
cout<<bid<<" ";
|
|
bundlesOfClip[cid].push_back(bid);
|
|
|
|
}
|
|
cout<<endl;
|
|
|
|
}
|
|
bundleid=cnt;
|
|
|
|
}
|
|
|
|
//��bundle��Ϣ����ʽҪ����ӡ����
|
|
inline void printBundle(string resultfile,bool printClipName=true){
|
|
ofstream ofs;
|
|
ofs.open(resultfile.c_str(), ios::out);
|
|
ofs<<bundleid-1<<endl;
|
|
for(int i=1;i<bundleid;i++)
|
|
bundles[i].print();
|
|
|
|
}
|
|
|
|
|
|
inline void printBundleQuality(string resultFile){
|
|
double averageDis=0;
|
|
int routeNum=0;
|
|
ofstream ofs,rfs;
|
|
ofs.open(resultFile.c_str(), ios::out);
|
|
string testFile="test"+resultFile;
|
|
rfs.open(testFile.c_str(), ios::out);
|
|
for(int i=1;i<bundleid;i++){
|
|
Bundle & bd=bundles[i];
|
|
bd.dia=0;
|
|
//cout<<"--------------"<<bd.d.size()<<"----------------"<<endl;
|
|
|
|
for(int j=0;j<bd.d.size();j++){
|
|
bd.dia+=bd.d[j]*bd.d[j];
|
|
}
|
|
bd.dia=sqrt(bd.dia);
|
|
for(int j=0;j<bd.wirelist.size();j++)cout<<bd.wirelist[j]<<" ";
|
|
cout<<endl;
|
|
printPath(bd.path);
|
|
//ofs<<bd.path.size()<<endl;
|
|
for(int j=0;j<bd.path.size()-1;j++){
|
|
P tp=bd.path[j];
|
|
averageDis+=distan(bd.path[j],bd.path[j+1]);
|
|
routeNum++;
|
|
double reverse=0;
|
|
P dir=dirP(bd.path[j]);
|
|
if(j!=0){
|
|
|
|
reverse+=pi/2-Angle(bd.path[j]-bd.path[j-1],dir);
|
|
}
|
|
if(j!=bd.path.size()-1){
|
|
reverse+=pi/2-Angle(bd.path[j+1]-bd.path[j],dir);
|
|
}
|
|
if(reverse<0)bd.path[j].reverse();
|
|
|
|
}
|
|
|
|
cout<<endl;
|
|
|
|
}
|
|
rfs<<bundleid-1<<endl;
|
|
averageDis/=routeNum;
|
|
for(int i=1;i<bundleid;i++){
|
|
Bundle bd=bundles[i];
|
|
printPath(bd.path);
|
|
//ofs<<bd.path.size()<<endl;
|
|
ofs<<bd.path.size()<<" "<<bd.dia<<endl;
|
|
rfs<<bd.dia<<endl;
|
|
rfs<<bd.path.size()<<endl;
|
|
|
|
for(int j=0;j<bd.path.size();j++){
|
|
P tp=bd.path[j];
|
|
ofs<<tp.x<<" "<<tp.y<<" "<<tp.z<<" ";
|
|
ofs<<tp.dx<<" "<<tp.dy<<" "<<tp.dz<<" ";
|
|
|
|
rfs<<tp.x<<" "<<tp.y<<" "<<tp.z<<" ";
|
|
rfs<<tp.dx<<" "<<tp.dy<<" "<<tp.dz<<endl;
|
|
if(j!=bd.path.size()-1)ofs<<distan(tp,bd.path[j+1])/averageDis<<endl;
|
|
else ofs<<0<<endl;
|
|
|
|
}
|
|
ofs<<endl;
|
|
|
|
}
|
|
ofs<<"-1 -1"<<endl;
|
|
for(int i=1;i<=(pclipSet->clipnum);i++){
|
|
if(!bundlesOfClip[i].empty()){
|
|
ofs<<i<<" "<<bundlesOfClip[i].size()<<endl;
|
|
for(int j=0;j<bundlesOfClip[i].size();j++){
|
|
ofs<<bundlesOfClip[i][j]<<" ";
|
|
}
|
|
ofs<<endl;
|
|
}
|
|
}
|
|
|
|
}
|
|
*/
|
|
|
|
//branchTree
|
|
|
|
|
|
|
|
/*
|
|
inline void search_pair(Astar& astar,BranchTree &branchtree,P &start,P &goal,double dia,vector<P> vecp){
|
|
astar.start=start;
|
|
astar.goal=goal;
|
|
astar.pnum-=2;
|
|
astar.points[++astar.pnum]=start;
|
|
astar.points[++astar.pnum]=goal;
|
|
|
|
astar.path.points.clear();
|
|
astar.path.dia=dia;
|
|
astar.path.wirelist.clear();
|
|
if(vecp.size()==0){
|
|
astar.search();
|
|
branchtree.addPath(astar.path);
|
|
return;
|
|
}
|
|
branchtree.startRecord();
|
|
Path tempPath=astar.search(start,vecp[0],0,1);
|
|
tempPath.dia=dia;
|
|
branchtree.addPath(tempPath,0,1);
|
|
|
|
for(int i=1;i<vecp.size();i++){
|
|
tempPath=astar.search(vecp[i-1],vecp[i],1,1);
|
|
tempPath.dia=dia;
|
|
branchtree.addPath(tempPath,1,1);
|
|
|
|
}
|
|
tempPath=astar.search(vecp[vecp.size()-1],goal,1,0);
|
|
tempPath.dia=dia;
|
|
branchtree.addPath(tempPath,1,0);
|
|
branchtree.endRecord();
|
|
|
|
return;
|
|
}
|
|
|
|
*/
|
|
|
|
/*
|
|
inline void search_pair(P &start,P &goal,double dia,vector<P> vecp,int bid){
|
|
this->start=start;
|
|
this->goal=goal;
|
|
pnum-=2;
|
|
points[++pnum]=start;
|
|
points[++pnum]=goal;
|
|
|
|
path.points.clear();
|
|
path.dia=dia;
|
|
path.wirelist.clear();
|
|
if(vecp.size()==0){
|
|
search();
|
|
pbranchTree->bundles[bid].path=path.points;
|
|
return;
|
|
}
|
|
Path tempPath=search(start,vecp[0],0,1);
|
|
vector<P> tPath=tempPath.points;
|
|
vector<P> rPath;
|
|
for(int j=0;j<tPath.size()-1;j++){
|
|
int cid=pclipSet->clipid[tPath[j]];
|
|
block[cid]=1;
|
|
rPath.push_back(tPath[j]);
|
|
}
|
|
|
|
for(int i=1;i<vecp.size();i++){
|
|
tempPath=search(vecp[i-1],vecp[i],1,1);
|
|
tPath=tempPath.points;
|
|
for(int j=0;j<tPath.size()-1;j++){
|
|
int cid=pclipSet->clipid[tPath[j]];
|
|
block[cid]=1;
|
|
rPath.push_back(tPath[j]);
|
|
}
|
|
|
|
}
|
|
tempPath=search(vecp[vecp.size()-1],goal,1,0);
|
|
tPath=tempPath.points;
|
|
for(int j=0;j<tPath.size();j++){
|
|
int cid=pclipSet->clipid[tPath[j]];
|
|
block[cid]=1;
|
|
rPath.push_back(tPath[j]);
|
|
}
|
|
pbranchTree->bundles[bid].path=rPath;
|
|
|
|
return;
|
|
}
|
|
|
|
inline Path search(P &start,P &goal,bool isclips,bool isclipt){
|
|
while(!pq.empty())pq.pop();
|
|
path.points.clear();
|
|
int s=pnum-1,t=pnum;
|
|
if(isclips){
|
|
s=kdtree.search_by_k(start,1)[0];
|
|
}
|
|
if(isclipt){
|
|
t=kdtree.search_by_k(goal,1)[0];
|
|
}
|
|
|
|
|
|
for(int i=1;i<=pnum;i++)vis[i]=0,dis[i]=Node(i,1000,1e9),pre[i]=0;
|
|
|
|
|
|
|
|
dis[s]=Node(s,0,0);
|
|
|
|
pq.push(dis[s]);
|
|
vector<int> vecv=kdtree.search_by_k(points[t],20);
|
|
vector<int> vec3;
|
|
for(int i=0;i<vecv.size();i++){
|
|
int xs=vecv[i];
|
|
if(!bvh.iscollect(points[xs],points[t]))vec3.push_back(xs);
|
|
}
|
|
while(!pq.empty()){
|
|
Node node=pq.top();
|
|
//cout<<pq.top().xs<<endl;
|
|
pq.pop();
|
|
while(vis[node.xs]&&!pq.empty())node=pq.top(),pq.pop();
|
|
vis[node.xs]=1;
|
|
int xs=node.xs;
|
|
//cout<<xs<<endl;
|
|
if(xs==t)break;
|
|
//cout<<"stage1"<<endl;
|
|
|
|
vector<int> vec;
|
|
if(xs!=pnum-1&&!edge[xs].empty())vec=edge[xs];
|
|
else{
|
|
|
|
vector<int> vec1=kdtree.search_by_dis(points[xs],600);
|
|
vector<int> vec2=kdtree.search_by_k(points[xs],20);
|
|
//if(cost(points[vec2[0]],points[xs])>=abdis)vec2.push_back(v);
|
|
vec1.insert(vec1.begin(),vec2.begin(),vec2.end());
|
|
for(int i=0;i<vec1.size();i++){
|
|
if(!bvh.iscollect(points[xs],points[vec1[i]]))vec.push_back(vec1[i]);
|
|
}
|
|
if(xs!=pnum-1)edge[xs]=vec;
|
|
|
|
}
|
|
|
|
for(int i=0;i<vec3.size();i++){
|
|
if(vec3[i]==xs)vec.push_back(t);
|
|
}
|
|
double abdis=cost(points[xs],points[t]);
|
|
if(abdis<=600)vec.push_back(t);
|
|
//cout<<"stage2"<<endl;
|
|
for(int i=0;i<vec.size();i++){
|
|
int v=vec[i];
|
|
if(v!=t)
|
|
if(!(pclipSet->transitable(v,2,path.dia)))continue;
|
|
|
|
//if(pbranchTree->check(points[v])==false)continue;
|
|
|
|
if(block[v])continue;
|
|
|
|
double cst=distan(points[xs],points[v])+dis[xs].dis;
|
|
int turnc=dis[xs].turncnt;
|
|
if(BadTurn(points[xs],points[v]))turnc++;
|
|
|
|
if(dis[v]<Node(v,turnc,cst)){
|
|
dis[v]=Node(v,turnc,cst);
|
|
pre[v]=xs;
|
|
pq.push(dis[v]);
|
|
}
|
|
}
|
|
//cout<<"stage3"<<endl;
|
|
|
|
|
|
|
|
//cout<<"stage4"<<endl;
|
|
}
|
|
//cout<<"stage5"<<endl;
|
|
//l=dis[pnum];
|
|
int top=0;
|
|
int tag=t;
|
|
while(pre[tag]){
|
|
//cout<<tag<<" "<<pre[tag]<<endl;
|
|
st[++top]=tag;
|
|
tag=pre[tag];
|
|
}
|
|
if(top){
|
|
st[++top]=s;
|
|
}
|
|
//cout<<"stage6"<<endl;
|
|
for(int i=top;i>0;i--){
|
|
int id=st[i];
|
|
//if(i!=top&&i!=1)pclipSet->addwire(id,dia);
|
|
path.points.push_back(points[id]);
|
|
}
|
|
return path;
|
|
|
|
}
|
|
|
|
|
|
inline void changeRoute(int bid,vector<int> cid){
|
|
int bundleid=pbranchTree->bundleid;
|
|
Bundle * bundles =pbranchTree->bundles;
|
|
|
|
for(int i=1;i<=bundleid;i++){
|
|
Bundle bd=bundles[i];
|
|
for(int j=0;j<bd.path.size();j++){
|
|
P tp=bd.path[j];
|
|
int pid=pclipSet->clipid[tp];
|
|
block[pid]=1;
|
|
}
|
|
}
|
|
Bundle bd=bundles[bid];
|
|
for(int j=0;j<bd.path.size();j++){
|
|
P tp=bd.path[j];
|
|
int pid=pclipSet->clipid[tp];
|
|
block[pid]=0;
|
|
}
|
|
P start=bd.path[0],goal=bd.path[bd.path.size()-1];
|
|
vector<P> vecP,sortVecP;
|
|
for(int i=0;i<cid.size();i++)vecP.push_back(points[cid[i]]);
|
|
pair<double,int> dis_id[100];
|
|
P sp[100];
|
|
for(int i=0;i<vecP.size();i++){
|
|
dis_id[i].first=distan1(vecP[i],start)-distan1(vecP[i],goal);
|
|
dis_id[i].second=i;
|
|
}
|
|
sort(dis_id,dis_id+vecP.size());
|
|
for(int i=0;i<vecP.size();i++)sp[i]=vecP[dis_id[i].second];
|
|
for(int i=0;i<vecP.size();i++)sortVecP.push_back(sp[i]);
|
|
|
|
search_pair(start,goal,bd.dia,sortVecP,bid);
|
|
|
|
}
|
|
|
|
inline void changeBranchEndPoint(int cid,int newCid){
|
|
|
|
Bundle * bundles =pbranchTree->bundles;
|
|
int bundleid=pbranchTree->bundleid;
|
|
vector<int> * bundlesOfClip =pbranchTree-> bundlesOfClip;
|
|
|
|
|
|
for(int i=1;i<=bundleid;i++){
|
|
bool flag=false;
|
|
for(int j=0;j<bundlesOfClip[cid].size();j++){
|
|
if(bundlesOfClip[cid][j]==i){
|
|
flag=true;
|
|
break;
|
|
}
|
|
}
|
|
if(flag)continue;
|
|
Bundle bd=bundles[i];
|
|
for(int j=0;j<bd.path.size();j++){
|
|
P tp=bd.path[j];
|
|
int pid=pclipSet->clipid[tp];
|
|
if(pid==newCid){
|
|
cout<<"Error:Clip illegal"<<endl;
|
|
return;
|
|
}
|
|
block[pid]=1;
|
|
}
|
|
}
|
|
vector<P> vecP;
|
|
P newClipCoord=(pclipSet->c[newCid]).coord;
|
|
P clipCoord=(pclipSet->c[cid]).coord;
|
|
for(int i=0;i<bundlesOfClip[cid].size();i++){
|
|
int bid=bundlesOfClip[cid][i];
|
|
Bundle &bd= pbranchTree->bundles[bid];
|
|
P start=bd.path[0],goal=bd.path[bd.path.size()-1];
|
|
if(distan1(clipCoord,start)<distan1(clipCoord,goal)){
|
|
start=newClipCoord;
|
|
}
|
|
else goal=newClipCoord;
|
|
search_pair(start,goal,bd.dia,vecP,bid);
|
|
for(int j=0;j<bd.path.size();j++){
|
|
P tp=bd.path[j];
|
|
int pid=pclipSet->clipid[tp];
|
|
block[pid]=1;
|
|
}
|
|
}
|
|
bundlesOfClip[newCid]=bundlesOfClip[cid];
|
|
bundlesOfClip[cid].clear();
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
inline void search(){
|
|
while(!pq.empty())pq.pop();
|
|
points[++pnum]=start;
|
|
points[++pnum]=goal;
|
|
|
|
for(int i=1;i<=2*tnum+2;i++)vis[i]=0,dis[i]=Node(i,1000,1e9),pre[i]=0;
|
|
|
|
|
|
dis[(pnum-1)*2]=Node((pnum-1)*2,0,0);
|
|
|
|
assert(pre[2*(pnum-1)]==0);
|
|
|
|
pq.push(dis[(pnum-1)*2]);
|
|
vector<int> vecv=kdtree.search_by_k(points[pnum],20);
|
|
vector<int> vec3;
|
|
for(int i=0;i<vecv.size();i++){
|
|
int xs=vecv[i];
|
|
if(!bvh.iscollect(points[xs],points[pnum]))vec3.push_back(xs);
|
|
}
|
|
bool testflag=false;
|
|
while(!pq.empty()){
|
|
Node node=pq.top();
|
|
//cout<<pq.top().xs<<endl;
|
|
|
|
pq.pop();
|
|
while(vis[node.xs]&&!pq.empty())node=pq.top(),pq.pop();
|
|
vis[node.xs]=1;
|
|
|
|
|
|
|
|
int xs=node.xs;
|
|
int inOut=xs%2;
|
|
xs/=2;
|
|
|
|
if(xs==pnum)break;
|
|
|
|
|
|
if(dcmp(points[xs].x-10920.50693)==0){
|
|
cout<<"------------------------------"<<endl;
|
|
cout<<pnum<<" "<<xs*2+inOut<<endl;
|
|
cout<<"------------------------------"<<endl;
|
|
testflag=true;
|
|
|
|
}
|
|
|
|
|
|
|
|
if(pre[2*(pnum-1)])cout<<"f1.9 "<<pre[2*(pnum-1)]<<endl;
|
|
|
|
|
|
|
|
vector<int> vec;
|
|
if(pre[2*(pnum-1)])cout<<"f1.10 "<<pre[2*(pnum-1)]<<endl;
|
|
if(xs!=pnum-1&&!edge[xs].empty())vec=edge[xs];
|
|
else if(xs==pnum-1&&nearClip[points[xs]]!=0)vec.push_back(nearClip[points[xs]]);
|
|
else{
|
|
|
|
vector<int> vec1=kdtree.search_by_dis(points[xs],600);
|
|
vector<int> vec2=kdtree.search_by_k(points[xs],20);
|
|
vec1.insert(vec1.begin(),vec2.begin(),vec2.end());
|
|
for(int i=0;i<vec1.size();i++){
|
|
int v=vec1[i];
|
|
if(v==xs)continue;
|
|
|
|
|
|
if(!bvh.iscollect(points[xs],points[v])){
|
|
|
|
int pov=ParallelorVertical(dir[xs],dir[v]);
|
|
if(pov==1){
|
|
pov=ParallelorVertical(points[xs]-points[v],dir[v]);
|
|
if(pov==2)continue;
|
|
}
|
|
|
|
|
|
vec.push_back(v);
|
|
}
|
|
|
|
|
|
}
|
|
if(xs!=pnum-1)edge[xs]=vec;
|
|
|
|
}
|
|
|
|
if(nearClip[points[pnum]]==0){
|
|
for(int i=0;i<vec3.size();i++){
|
|
if(vec3[i]==xs)vec.push_back(pnum);
|
|
}
|
|
double abdis=cost(points[xs],points[pnum]);
|
|
if(abdis<=600)vec.push_back(pnum);
|
|
}
|
|
else if(nearClip[points[pnum]]==xs)vec.push_back(pnum);
|
|
|
|
for(int i=0;i<vec.size();i++){
|
|
int v=vec[i];
|
|
if(v!=pnum)
|
|
{
|
|
if(!(pclipSet->transitable(v,2,path.dia)))continue;
|
|
if(block[v])continue;
|
|
|
|
}
|
|
if(v==xs)cout<<v<<endl;
|
|
|
|
|
|
if(points[xs]==points[v])continue;
|
|
assert(xs!=v);
|
|
assert(points[xs]!=points[v]);
|
|
assert(v!=pnum-1);
|
|
//assert(pnum==2833);
|
|
assert(pre[2*(pnum-1)]==0);
|
|
assert(pre[2*(pnum-1)+1]==0);
|
|
|
|
double cst=distan(points[xs],points[v],inOut,0)+dis[xs*2+inOut].dis;
|
|
double cst1=distan(points[xs],points[v],inOut,1)+dis[xs*2+inOut].dis;
|
|
//int turnc=dis[xs].turncnt;
|
|
//if(BadTurn(points[xs],points[v]))turnc++;
|
|
|
|
if(dis[2*v]<Node(2*v,0,cst)){
|
|
dis[2*v]=Node(2*v,0,cst);
|
|
assert(v!=pnum-1);
|
|
pre[2*v]=xs*2+inOut;
|
|
pq.push(dis[2*v]);
|
|
}
|
|
if(dis[2*v+1]<Node(2*v+1,0,cst1)){
|
|
dis[2*v+1]=Node(2*v+1,0,cst1);
|
|
assert(v!=pnum-1);
|
|
pre[2*v+1]=xs*2+inOut;
|
|
pq.push(dis[2*v+1]);
|
|
}
|
|
}
|
|
|
|
}
|
|
int top=0;
|
|
int tag=pnum*2;
|
|
while(pre[tag]){
|
|
st[++top]=tag/2;
|
|
tag=pre[tag];
|
|
if(testflag)cout<<tag<<endl;
|
|
}
|
|
if(top){
|
|
st[++top]=pnum-1;
|
|
}
|
|
for(int i=top;i>0;i--){
|
|
int id=st[i];
|
|
if(top>2){
|
|
if(id<pnum-1){
|
|
if(i==top-1)nearClip[points[pnum-1]]=id;
|
|
if(i==2)nearClip[points[pnum]]=id;
|
|
}
|
|
}
|
|
//if(i!=top&&i!=1)pclipSet->addwire(id,dia);
|
|
path.points.push_back(points[id]);
|
|
}
|
|
|
|
|
|
}
|
|
*/
|
|
|
|
//Astar
|
|
|
|
if(mode=="findRoute"){
|
|
//�������������ļ��������յ����꣬����Ѱ·
|
|
string clipFileName,endpointFileName,outputFileName;
|
|
cin>>clipFileName>>endpointFileName>>outputFileName;
|
|
read_points(clipFileName,pastar);
|
|
kdtree.build();
|
|
|
|
astar.init();
|
|
bvh.read_stl("PFM_L_simple.stl");
|
|
|
|
clipSet.computeCenter();
|
|
ifstream iendpointFile;
|
|
ofstream oFile;
|
|
iendpointFile.open(endpointFileName.c_str(),ios::in);
|
|
oFile.open(outputFileName.c_str(),ios::out);
|
|
int pairNum;
|
|
iendpointFile>>pairNum;
|
|
for(int i=1;i<=pairNum;i++){
|
|
P p1,p2;
|
|
iendpointFile>>p1.x>>p1.y>>p1.z>>p1.dx>>p1.dy>>p1.dz;
|
|
iendpointFile>>p2.x>>p2.y>>p2.z>>p2.dx>>p2.dy>>p2.dz;
|
|
Path path=astar.search_pair(p1,p2,0);
|
|
oFile<<path.points.size()<<endl;
|
|
for(int j=0;j<path.points.size();j++){
|
|
P pp=path.points[j];
|
|
oFile<<pp.x<<" "<<pp.y<<" "<<pp.z<<endl;
|
|
cout<<setprecision(10)<<"("<<pp.x<<","<<pp.y<<","<<pp.z<<")";
|
|
if(j!=path.points.size()-1)cout<<"->";
|
|
}
|
|
cout<<endl;
|
|
|
|
}
|
|
cout<<"Finsh"<<endl;
|
|
return 0;
|
|
}
|
|
else if(mode=="test"){
|
|
//������Ҫ�κ����룬ʹ�ò������ݽ���һ�β���
|
|
read_points("data_02.xml",pastar);
|
|
read_points("data_03.xml",pastar);
|
|
read_points("data_15.xml",pastar);
|
|
|
|
cout<<"read points finish"<<endl;
|
|
astar.init();
|
|
bvh.read_stl("PFM_L_simple.stl");
|
|
|
|
//clipset.print();
|
|
clipSet.computeCenter();
|
|
//clipset.printDis();
|
|
|
|
//changeRoute();
|
|
//changeBranchEndPoint();
|
|
//return 0;
|
|
time_t r1,r2;
|
|
|
|
time(&r1);
|
|
//testPoint();
|
|
//char * xml="W024019B.xml";
|
|
//produceXML(xml);
|
|
|
|
|
|
string docName="wirexml";
|
|
vector<string> filenames;
|
|
getAllFiles(docName,filenames);
|
|
|
|
for(int i=0;i<filenames.size();i++){
|
|
string wirebh;
|
|
stringstream ss;
|
|
ss<<i;
|
|
wirebh=ss.str();
|
|
string filename=filenames[i];
|
|
cout<<filename<<endl;
|
|
cout<<endl;
|
|
string resultname="branchg"+wirebh+".txt";
|
|
produceXML(filename.c_str(),"connector.csv",resultname);
|
|
}
|
|
|
|
|
|
|
|
|
|
time(&r2);
|
|
cout<<"Astar time "<<(r2-r1)<<endl;
|
|
cout<<"ALL FINISH"<<endl;
|
|
}
|
|
/*
|
|
����һ��·��
|
|
void testPoint(){
|
|
astar.addBranchTree(&branchtree);
|
|
ifstream ifs;
|
|
ifs.open("point.txt",ios::in);
|
|
P start,goal;
|
|
ifs>>start.x>>start.y>>start.z;
|
|
ifs>>goal.x>>goal.y>>goal.z;
|
|
string st[3];
|
|
vector<P> vecp;
|
|
start.print("start");
|
|
goal.print("goal");
|
|
while(ifs>>st[0]){
|
|
for(int i=1;i<=2;i++)ifs>>st[i];
|
|
P p;
|
|
for(int i=0;i<3;i++)p.set(i,atof(st[i].c_str()));
|
|
vecp.push_back(p);
|
|
p.print("vecp");
|
|
}
|
|
search_pair(astar,branchtree,start,goal,10,vecp);
|
|
|
|
branchtree.buildBundle();
|
|
branchtree.printBundle("onebranch.txt");
|
|
|
|
|
|
}
|
|
|
|
|
|
void changeRoute(){
|
|
astar.addBranchTree(&branchtree);
|
|
int bid;
|
|
int cnum;
|
|
vector<int> clipid;
|
|
branchtree.readBundle("onebranch3.txt");
|
|
|
|
cin>>bid>>cnum;
|
|
for(int i=1;i<=cnum;i++){
|
|
int cid;
|
|
cin>>cid;
|
|
clipid.push_back(cid);
|
|
}
|
|
astar.changeRoute(bid,clipid);
|
|
|
|
|
|
branchtree.printBundleQuality("onebranch5.txt");
|
|
cout<<"Finish"<<endl;
|
|
|
|
|
|
}
|
|
void changeBranchEndPoint(){
|
|
astar.addBranchTree(&branchtree);
|
|
int cid,newCid;
|
|
branchtree.readBundle("branchc1.txt");
|
|
|
|
cin>>cid>>newCid;
|
|
|
|
astar.changeBranchEndPoint(cid,newCid);
|
|
|
|
|
|
branchtree.printBundleQuality("2branch.txt");
|
|
cout<<"Finish"<<endl;
|
|
|
|
|
|
}
|
|
*/
|
|
|
|
void produce_pair(){
|
|
ofstream ofs;
|
|
ofs.open("result.txt", ios::out);
|
|
ifstream ifs;
|
|
ifs.open("pair.txt",ios::in);
|
|
string st[6];
|
|
int wireid=0;
|
|
while(ifs>>st[0]){
|
|
for(int i=1;i<=5;i++)ifs>>st[i];
|
|
P p1,p2;
|
|
for(int i=0;i<3;i++)p1.set(i,atof(st[i].c_str()));
|
|
for(int i=0;i<3;i++)p2.set(i,atof(st[i+3].c_str()));
|
|
pmp[p1]=st[0]+" "+st[1]+" "+st[2];
|
|
pmp[p2]=st[3]+" "+st[4]+" "+st[5];
|
|
|
|
cout<<bvh.iscollect(p1,p2)<<endl;
|
|
|
|
vector<P>vecp=astar.search_pair(p1,p2,1).points;
|
|
for(int j=0;j<vecp.size();j++){
|
|
P pp=vecp[j];
|
|
ofs<<pmp[pp]<<endl;
|
|
cout<<setprecision(10)<<"("<<pp.x<<","<<pp.y<<","<<pp.z<<")";
|
|
if(j!=vecp.size()-1)cout<<"->";
|
|
}
|
|
cout<<endl;
|
|
ofs<<"#"<<endl;
|
|
ofs<<endl;
|
|
|
|
string name,wirebh;
|
|
stringstream ss;
|
|
ss<<wireid;
|
|
wirebh=ss.str();
|
|
name="wire"+wirebh;
|
|
Path path;
|
|
path.points=vecp;
|
|
path.wirelist.push_back(name);
|
|
branchTree.addPath(path);
|
|
|
|
wireid++;
|
|
}
|
|
basicChannel.printBundle("branch.txt");
|
|
}
|
|
|
|
//��Bundle��Ϣ�������ļ���ofs
|
|
void print(ofstream& ofs){
|
|
|
|
if(head==NULL)return;
|
|
Clip *pc=head->pc;
|
|
double dia=pc->getDia();
|
|
if(dcmp(dia)==0)dia=5;
|
|
int nodeNum=0;
|
|
BundleNode *pbn=head;
|
|
while(pbn!=NULL){
|
|
nodeNum++;
|
|
pbn=pbn->next;
|
|
}
|
|
|
|
ofs<<nodeNum<<" "<<dia<<" "
|
|
<<startBranchPoint()<<" "<<endBranchPoint()<<endl;
|
|
|
|
pbn=head;
|
|
vector<BranchPoint* > vb;
|
|
while(pbn!=NULL){
|
|
if(pbn->type==0){
|
|
P dir=pbn->pc->dir;
|
|
string name=pbn->pc->name;
|
|
if(name=="")name="--";
|
|
ofs<<name<<" "<<!(pbn->inOut)<<endl;
|
|
|
|
P tp=pbn->pc->coord;
|
|
ofs<<tp.x<<" "<<tp.y<<" "<<tp.z<<" ";
|
|
ofs<<tp.dx<<" "<<tp.dy<<" "<<tp.dz<<endl;
|
|
}
|
|
vb.insert(vb.end(),(pbn->vb).begin(),(pbn->vb).end());
|
|
pbn=pbn->next;
|
|
|
|
}
|
|
ofs<<vb.size()<<endl;
|
|
for(int j=0;j<vb.size();j++){
|
|
ofs<<vb[j]->referClip<<" "
|
|
<<vb[j]->position<<endl;
|
|
}
|
|
ofs<<endl;
|
|
|
|
}
|
|
|
|
inline double distan(P &A,P &B){
|
|
double angel=Angle(A-B,DX);
|
|
if(angel>pi/2)angel=pi-angel;
|
|
angel=min(angel,pi/2-angel);
|
|
double len=distan1(A,B);
|
|
|
|
double len1=sqrt((A.y-Ycenter)*(A.y-Ycenter)+(A.z-Zcenter)*(A.z-Zcenter));
|
|
double len2=sqrt((B.y-Ycenter)*(B.y-Ycenter)+(B.z-Zcenter)*(B.z-Zcenter));
|
|
if(len1<R||len2<R){
|
|
double angel1=Angle(A-B,DX);
|
|
if(angel1>pi/2)angel1=pi-angel1;
|
|
|
|
double angel2=Angle(A-B,DY);
|
|
if(angel2>pi/2)angel2=pi-angel2;
|
|
|
|
double angel3=Angle(A-B,DZ);
|
|
if(angel3>pi/2)angel3=pi-angel3;
|
|
|
|
angel=min(angel1,min(angel2,angel3));
|
|
}
|
|
|
|
|
|
P C;
|
|
C.x=B.dx;C.y=B.dy;C.z=B.dz;
|
|
double angel2=Angle(A-B,C);
|
|
if(angel2>pi/2)angel2=pi-angel2;
|
|
if(B.isend==1||B.type)angel2=0;
|
|
|
|
C.x=A.dx;C.y=A.dy;C.z=A.dz;
|
|
double angel3=Angle(A-B,C);
|
|
if(angel3>pi/2)angel3=pi-angel3;
|
|
if(A.isend==1||A.type)angel3=0;
|
|
|
|
|
|
|
|
//else if(angel>=pi/2-minAngel&&angel<=pi/2+minAngel)return false;
|
|
/*
|
|
P C(A.dx,A.dy,A.dz);
|
|
double len=Dot(C,A-B)/Length(C);
|
|
if(len>=-minDis&&len<=minDis)return false;
|
|
*/
|
|
|
|
//return pow(len,angel*3+1);
|
|
return len*(angel*4+1)+300*600*(angel2+angel3)/len;
|
|
|
|
}
|
|
|
|
inline bool BadTurn(P &A,P & B){
|
|
return false;
|
|
double angel=Angle(A-B,DX);
|
|
if(angel<=minAngle||angel>=pi-minAngle)return false;
|
|
angel=Angle(A-B,DY);
|
|
if(angel<=minAngle||angel>=pi-minAngle)return false;
|
|
angel=Angle(A-B,DZ);
|
|
if(angel<=minAngle||angel>=pi-minAngle)return false;
|
|
//else if(angel>=pi/2-minAngel&&angel<=pi/2+minAngel)return false;
|
|
/*
|
|
P C(A.dx,A.dy,A.dz);
|
|
double len=Dot(C,A-B)/Length(C);
|
|
if(len>=-minDis&&len<=minDis)return false;
|
|
*/
|
|
|
|
return true;
|
|
|
|
|
|
}
|