/* 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"<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=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 "<c; int clipNum=pclipSet->clipnum; for(int i=1;i<=clipNum;i++){ int cnt=0; for(int j=0;jbothConnect(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;jbothConnect(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<>bid; cout<clipnum);i++){ if(!bundlesOfClip[i].empty()){ ofs< 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,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

tPath=tempPath.points; vector

rPath; for(int j=0;jclipid[tPath[j]]; block[cid]=1; rPath.push_back(tPath[j]); } for(int i=1;iclipid[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;jclipid[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 vecv=kdtree.search_by_k(points[t],20); vector vec3; for(int i=0;i vec; if(xs!=pnum-1&&!edge[xs].empty())vec=edge[xs]; else{ vector vec1=kdtree.search_by_dis(points[xs],600); vector 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;itransitable(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]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 cid){ int bundleid=pbranchTree->bundleid; Bundle * bundles =pbranchTree->bundles; for(int i=1;i<=bundleid;i++){ Bundle bd=bundles[i]; for(int j=0;jclipid[tp]; block[pid]=1; } } Bundle bd=bundles[bid]; for(int j=0;jclipid[tp]; block[pid]=0; } P start=bd.path[0],goal=bd.path[bd.path.size()-1]; vector

vecP,sortVecP; for(int i=0;i dis_id[100]; P sp[100]; for(int i=0;ibundles; int bundleid=pbranchTree->bundleid; vector * bundlesOfClip =pbranchTree-> bundlesOfClip; for(int i=1;i<=bundleid;i++){ bool flag=false; for(int j=0;jclipid[tp]; if(pid==newCid){ cout<<"Error:Clip illegal"< vecP; P newClipCoord=(pclipSet->c[newCid]).coord; P clipCoord=(pclipSet->c[cid]).coord; for(int i=0;ibundles[bid]; P start=bd.path[0],goal=bd.path[bd.path.size()-1]; if(distan1(clipCoord,start)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 vecv=kdtree.search_by_k(points[pnum],20); vector vec3; for(int i=0;i vec; if(pre[2*(pnum-1)])cout<<"f1.10 "< vec1=kdtree.search_by_dis(points[xs],600); vector vec2=kdtree.search_by_k(points[xs],20); vec1.insert(vec1.begin(),vec2.begin(),vec2.end()); for(int i=0;itransitable(v,2,path.dia)))continue; if(block[v])continue; } if(v==xs)cout<0;i--){ int id=st[i]; if(top>2){ if(idaddwire(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<"; } cout< filenames; getAllFiles(docName,filenames); for(int i=0;i>start.x>>start.y>>start.z; ifs>>goal.x>>goal.y>>goal.z; string st[3]; vector

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 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"<>cid>>newCid; astar.changeBranchEndPoint(cid,newCid); branchtree.printBundleQuality("2branch.txt"); cout<<"Finish"<>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<vecp=astar.search_pair(p1,p2,1).points; for(int j=0;j"; } cout<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< vb; while(pbn!=NULL){ if(pbn->type==0){ P dir=pbn->pc->dir; string name=pbn->pc->name; if(name=="")name="--"; ofs<inOut)<pc->coord; ofs<vb).begin(),(pbn->vb).end()); pbn=pbn->next; } ofs<referClip<<" " <position<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(len1pi/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; }