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.
494 lines
11 KiB
494 lines
11 KiB
#include "stdafx.h"
|
|
#include "BranchTree.h"
|
|
#include<cmath>
|
|
#include<vector>
|
|
#include<queue>
|
|
#include<map>
|
|
#include<iostream>
|
|
#include<fstream>
|
|
#include<sstream>
|
|
using namespace std;
|
|
|
|
|
|
struct DSU{
|
|
int f[N];
|
|
void init(int cnt){
|
|
for(int i=1;i<=cnt;i++)f[i]=i;
|
|
}
|
|
int getrt(int x){
|
|
if(x!=f[x])return f[x]=getrt(f[x]);
|
|
return x;
|
|
}
|
|
inline void merge(int x,int y){
|
|
if(x!=f[x])x=getrt(x);
|
|
if(y!=f[y])y=getrt(y);
|
|
f[x]=y;
|
|
}
|
|
};
|
|
|
|
|
|
struct Node{
|
|
int xs,turncnt;
|
|
double dis;
|
|
bool operator <(Node B)const{
|
|
//if(turncnt!=B.turncnt)return turncnt>B.turncnt;
|
|
return dis>B.dis;
|
|
}
|
|
Node(){
|
|
xs=0;dis=0;turncnt=0;
|
|
}
|
|
Node(int xs1,int turnc,double dis1){
|
|
xs=xs1;dis=dis1;turncnt=turnc;
|
|
}
|
|
};
|
|
|
|
//用于搜索路径的数据结构，每种类型的线束会分配到一个
|
|
struct Astar{
|
|
ClipSet * pclipSet; //指向卡箍集合
|
|
BranchTree *pbranchTree; //指向分支树
|
|
P points[MAXPointNum]; //点的信息，包括卡箍点，分支点和端点
|
|
P start,goal,start_dir,goal_dir; //起点终点以及方向
|
|
int S,T; //起点和终点的标号
|
|
map<int, vector<int> >edge;
|
|
//存储每个点附近不碰撞的其他点，避免重复调用kdtree和bvh
|
|
map<P,pair<int,int> > nearClip; //因为端点处不能分叉，所以标记端点连接到的最近卡箍
|
|
|
|
int searchNum; //搜索次数，用于调试
|
|
int pre[MAXPointNum],vis[MAXPointNum]; //pre存储一个点的上一个点，vis存储一个点是否被访问过
|
|
int st[MAXPointNum],block[MAXPointNum]; //st是临时栈，block标记的点不能被通过
|
|
int pnum; //卡箍点总数
|
|
int tnum; //所有点总数
|
|
Path path; //路径结果
|
|
double bend_r,l; //弯曲半径，总长度
|
|
Node dis[MAXPointNum]; //存储距离信息
|
|
priority_queue<Node>pq; //优先队列
|
|
double preBranchPointPos[MAXPointNum]; //若前一个点是1类型（同进异出）分支点，存储其最优位置
|
|
|
|
Astar(){
|
|
pnum=0;l=bend_r=0;tnum=0;
|
|
pclipSet=&clipSet;
|
|
}
|
|
|
|
void addBranchTree(BranchTree *pbranchTree){
|
|
this->pbranchTree=pbranchTree;
|
|
}
|
|
|
|
//初始化
|
|
inline void init(){
|
|
pnum+=2;
|
|
}
|
|
|
|
//添加一个卡箍点
|
|
inline void add_clip(P &p,P &d){
|
|
pnum++;tnum++;
|
|
points[pnum]=p;
|
|
points[pnum].ref=pnum;
|
|
|
|
P dir=p-d;
|
|
points[pnum].dx=dir.x;
|
|
points[pnum].dy=dir.y;
|
|
points[pnum].dz=dir.z;
|
|
block[pnum]=0;
|
|
}
|
|
inline void add_clip(Clip& cp){
|
|
pnum++;tnum++;
|
|
points[pnum]=cp.coord;
|
|
block[pnum]=0;
|
|
}
|
|
|
|
//添加一个分支点
|
|
inline void add_branchPoint(BranchPoint& bp){
|
|
tnum=max(tnum,clipSet.clipnum+2);
|
|
tnum++;
|
|
points[tnum]=bp.coord;
|
|
block[tnum]=0;
|
|
}
|
|
|
|
/*
|
|
inline double cost(P &p1,P &p2){
|
|
if(bvh.iscollect(p1,p2))return 1e9;
|
|
//return distan(p1,p2);
|
|
return angelAndLength(p1,p2);
|
|
}
|
|
*/
|
|
|
|
|
|
|
|
|
|
inline Path search_pair(P &start,P &goal,P &start_dir,P &goal_dir,double dia){
|
|
this->start=start;
|
|
this->goal=goal;
|
|
this->start_dir=start_dir;
|
|
this->goal_dir=goal_dir;
|
|
path.dia=dia;
|
|
pnum-=2;
|
|
path.points.clear();
|
|
path.wirelist.clear();
|
|
path.dia=dia;
|
|
search();
|
|
return path;
|
|
}
|
|
inline Path search_pair(P &start,P &goal,double dia){
|
|
this->start=start;
|
|
this->goal=goal;
|
|
path.dia=dia;
|
|
|
|
pnum-=2;
|
|
path.points.clear();
|
|
path.wirelist.clear();
|
|
path.dia=dia;
|
|
search();
|
|
return path;
|
|
}
|
|
|
|
|
|
inline Path search_pair(Bund & bd){
|
|
this->start=bd.start;
|
|
this->goal=bd.goal;
|
|
pnum-=2;
|
|
path.points.clear();
|
|
path.inOut.clear();
|
|
path.wirelist=bd.wirenamelist;
|
|
path.dia=bd.dia;
|
|
tnum=max(tnum,clipSet.clipnum+2);
|
|
search();
|
|
return path;
|
|
}
|
|
|
|
inline void searchForPreProcess(){
|
|
while(!pq.empty())pq.pop();
|
|
points[++pnum]=start;
|
|
points[++pnum]=goal;
|
|
S=pnum-1;T=pnum;
|
|
|
|
for(int i=1;i<=2*tnum+2;i++)vis[i]=0,dis[i]=Node(i,1000,1e9),pre[i]=0;
|
|
|
|
|
|
dis[S*2]=Node(S*2,0,0);
|
|
|
|
assert(pre[2*S]==0);
|
|
|
|
pq.push(dis[S*2]);
|
|
vector<pair<int,int> > tempvec=basicEdge.getEXTNextPoint(points[T],pnum,1);
|
|
vector<pair<int,int> >pointsConnectedToGoal;
|
|
for(int i=0;i<tempvec.size();i++){
|
|
int xs=tempvec[i].first;
|
|
if(!bvh.iscollect(points[xs],points[T]))
|
|
pointsConnectedToGoal.push_back(tempvec[i]);
|
|
}
|
|
bool testflag=true;
|
|
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;
|
|
|
|
pair<int,int> currentNode=make_pair(xs,inOut);
|
|
|
|
if(xs==T)break;
|
|
|
|
vector<pair<int,int> > vec;
|
|
if(xs!=S)vec=basicEdge.getNextPoint(xs,inOut,pnum);
|
|
else if(xs==S&&nearClip[points[xs]]!=make_pair(0,0))vec.push_back(nearClip[points[S]]);
|
|
else {
|
|
vec=basicEdge.getEXTNextPoint(points[S],pnum,0);
|
|
//cout<<vec.size()<<endl;
|
|
}
|
|
|
|
if(nearClip[points[T]]==make_pair(0,0)){
|
|
for(int i=0;i<pointsConnectedToGoal.size();i++){
|
|
if(pointsConnectedToGoal[i]==currentNode)
|
|
vec.push_back(make_pair(T,0));
|
|
}
|
|
}
|
|
else if(nearClip[points[T]]==currentNode)vec.push_back(make_pair(T,0));
|
|
|
|
for(int i=0;i<vec.size();i++){
|
|
int v=vec[i].first;
|
|
int mode=vec[i].second;
|
|
if(v<pnum-1)
|
|
{
|
|
//if(!(pclipSet->transitable(v,0,path.dia)))continue;
|
|
//if(block[v])continue;
|
|
;
|
|
|
|
}
|
|
//cout<<v<<endl;
|
|
if(v==xs)cout<<"v==xs "<<v<<endl;
|
|
/*
|
|
if(points[xs]==points[v]){
|
|
cout<<xs<<" "<<v<<endl;
|
|
cout<<points[xs].x<<" "<<points[xs].y<<" "<<points[xs].z<<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,mode);
|
|
if(points[xs].type==1||points[xs].type==2){
|
|
int bid=points[xs].ref;
|
|
BranchPoint *pb=branchPointSet.getBranchPointPointer(bid);
|
|
cst=pb->computeWeight(points[v],mode,0);
|
|
}
|
|
if(points[v].type==1||points[v].type==2){
|
|
int bid=points[v].ref;
|
|
BranchPoint *pb=branchPointSet.getBranchPointPointer(bid);
|
|
cst=pb->computeWeight(points[xs],inOut,1);
|
|
}
|
|
cst+=+dis[xs*2+inOut].dis;
|
|
|
|
if(dis[2*v+mode]<Node(2*v+mode,0,cst)){
|
|
dis[2*v+mode]=Node(2*v+mode,0,cst);
|
|
assert(v!=pnum-1);
|
|
pre[2*v+mode]=xs*2+inOut;
|
|
pq.push(dis[2*v+mode]);
|
|
}
|
|
}
|
|
|
|
}
|
|
int top=0;
|
|
int tag=T*2;
|
|
while(pre[tag]){
|
|
st[++top]=tag;
|
|
tag=pre[tag];
|
|
if(testflag)cout<<"nodeid "<<tag<<endl;
|
|
}
|
|
if(top){
|
|
st[++top]=S*2;
|
|
}
|
|
for(int i=top;i>0;i--){
|
|
int id=st[i];
|
|
if(top>2){
|
|
if(id/2!=S&&id/2!=T){
|
|
if(i==top-1)nearClip[points[S]]=make_pair(id/2,id%2);
|
|
if(i==2)nearClip[points[T]]=make_pair(id/2,id%2);
|
|
}
|
|
}
|
|
//if(i!=top&&i!=1)pclipSet->addwire(id,dia);
|
|
path.points.push_back(points[id/2]);
|
|
if(i!=top&&i!=1&&id/2<pnum)path.inOut.push_back(id%2);
|
|
else path.inOut.push_back(-1-id%2);
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
inline void search(){
|
|
searchNum++;
|
|
cout<<"searchNum "<<searchNum<<endl;
|
|
|
|
while(!pq.empty())pq.pop();
|
|
points[++pnum]=start;
|
|
points[++pnum]=goal;
|
|
S=pnum-1;T=pnum;
|
|
|
|
for(int i=1;i<=2*tnum+2;i++)vis[i]=0,dis[i]=Node(i,1000,1e9),pre[i]=0;
|
|
|
|
|
|
dis[S*2]=Node(S*2,0,0);
|
|
|
|
assert(pre[2*S]==0);
|
|
|
|
pq.push(dis[S*2]);
|
|
vector<pair<int,int> > tempvec=basicEdge.getEXTNextPoint(points[T],pnum,1);
|
|
vector<pair<int,int> >pointsConnectedToGoal;
|
|
for(int i=0;i<tempvec.size();i++){
|
|
int xs=tempvec[i].first;
|
|
if(!bvh.iscollect(points[xs],points[T]))
|
|
pointsConnectedToGoal.push_back(tempvec[i]);
|
|
}
|
|
bool testflag=true;
|
|
int whilenum=0;
|
|
while(!pq.empty()){
|
|
Node node=pq.top();
|
|
//cout<<pq.top().xs<<endl;
|
|
whilenum++;
|
|
if(searchNum==105)cout<<"whilenum "<<whilenum<<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;
|
|
|
|
pair<int,int> currentNode=make_pair(xs,inOut);
|
|
|
|
if(xs==T)break;
|
|
|
|
vector<pair<int,int> > vec;
|
|
if(xs!=S)vec=basicEdge.getNextPoint(xs,inOut,pnum);
|
|
else if(xs==S&&nearClip[points[xs]]!=make_pair(0,0))vec.push_back(nearClip[points[S]]);
|
|
else {
|
|
vec=basicEdge.getEXTNextPoint(points[S],pnum,0);
|
|
//cout<<vec.size()<<endl;
|
|
}
|
|
|
|
if(nearClip[points[T]]==make_pair(0,0)){
|
|
for(int i=0;i<pointsConnectedToGoal.size();i++){
|
|
if(pointsConnectedToGoal[i]==currentNode)
|
|
vec.push_back(make_pair(T,0));
|
|
}
|
|
}
|
|
else if(nearClip[points[T]]==currentNode)vec.push_back(make_pair(T,0));
|
|
|
|
for(int i=0;i<vec.size();i++){
|
|
int v=vec[i].first;
|
|
int mode=vec[i].second;
|
|
if(v<pnum-1)
|
|
{
|
|
//if(!(pclipSet->transitable(v,0,path.dia)))continue;
|
|
//if(block[v])continue;
|
|
;
|
|
|
|
}
|
|
//cout<<v<<endl;
|
|
if(v==xs)cout<<"v==xs "<<v<<" "<<points[v].type<<endl;
|
|
|
|
if(vis[v*2+mode])continue;
|
|
/*
|
|
if(points[xs]==points[v]){
|
|
cout<<xs<<" "<<v<<endl;
|
|
cout<<points[xs].x<<" "<<points[xs].y<<" "<<points[xs].z<<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,mode);
|
|
|
|
bool updatePreBranchPoint=false;
|
|
bool updateNextBranchPoint=false;
|
|
double tempPos1=0,tempPos2=0;
|
|
if(points[xs].type==1||points[xs].type==2){
|
|
int bid=points[xs].ref;
|
|
if(inOut==0){
|
|
BranchPoint *pb=branchPointSet.getBranchPointPointer(bid);
|
|
cst=(pb->Dis)/2;
|
|
}
|
|
else {
|
|
BranchPoint *pb=branchPointSet.getBranchPointPointer(bid);
|
|
cst=pb->computeWeight(points[v],mode,0);
|
|
updatePreBranchPoint=true;
|
|
tempPos1=pb->t;
|
|
}
|
|
}
|
|
if(points[v].type==1||points[v].type==2){
|
|
int bid=points[v].ref;
|
|
BranchPoint *pb=branchPointSet.getBranchPointPointer(bid);
|
|
if(mode==1){
|
|
|
|
cst=(pb->Dis)/2;
|
|
}
|
|
else {
|
|
cst=pb->computeWeight(points[xs],inOut,1);
|
|
updateNextBranchPoint=true;
|
|
tempPos2=pb->t;
|
|
}
|
|
}
|
|
cst+=dis[xs*2+inOut].dis;
|
|
|
|
if(dis[2*v+mode]<Node(2*v+mode,0,cst)){
|
|
if(updatePreBranchPoint){
|
|
this->preBranchPointPos[2*v+mode]=tempPos1;
|
|
}
|
|
if(updateNextBranchPoint){
|
|
int bid=points[v].ref;
|
|
BranchPoint *pb=branchPointSet.getBranchPointPointer(bid);
|
|
pb->recordPos(0);
|
|
}
|
|
|
|
dis[2*v+mode]=Node(2*v+mode,0,cst);
|
|
assert(v!=pnum-1);
|
|
pre[2*v+mode]=xs*2+inOut;
|
|
pq.push(dis[2*v+mode]);
|
|
}
|
|
}
|
|
|
|
}
|
|
int top=0;
|
|
int tag=T*2;
|
|
while(pre[tag]){
|
|
st[++top]=tag;
|
|
tag=pre[tag];
|
|
if(testflag){
|
|
cout<<"nodeid "<<tag<<" ";
|
|
if(tag<(pnum-1)*2)
|
|
cout<<basicChannel.clipType(tag/2)<<endl;
|
|
else cout<<-1<<endl;
|
|
|
|
}
|
|
}
|
|
if(top){
|
|
st[++top]=S*2;
|
|
}
|
|
vector<double> prePos;
|
|
for(int i=top;i>0;i--){
|
|
int id=st[i];
|
|
if(top>2){
|
|
if(id/2!=S&&id/2!=T){
|
|
if(i==top-1)nearClip[points[S]]=make_pair(id/2,id%2);
|
|
if(i==2)nearClip[points[T]]=make_pair(id/2,id%2);
|
|
}
|
|
}
|
|
//if(i!=top&&i!=1)pclipSet->addwire(id,dia);
|
|
path.points.push_back(points[id/2]);
|
|
if(i!=top&&i!=1&&id/2<pnum)path.inOut.push_back(id%2);
|
|
else {
|
|
path.inOut.push_back(-1-id%2);
|
|
}
|
|
prePos.push_back(preBranchPointPos[id]);
|
|
}
|
|
for(int i=0;i<path.points.size();i++){
|
|
P &p=path.points[i];
|
|
if(p.type==1||p.type==2){
|
|
int mode=-1-path.inOut[i];
|
|
int bid=p.ref;
|
|
BranchPoint *pb=branchPointSet.getBranchPointPointer(bid);
|
|
pb->mode=mode;
|
|
if(mode==1){
|
|
pb->t=prePos[i+1];
|
|
pb->recordPos(1);
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
}astar;
|
|
|
|
|
|
|
|
|
|
|
|
|