C++AGV路径规划与运行仿真程序

程序代码#includeiostream.h#includefstream.h#includegraphics.h#includeconio.h#includemath.h#includestring.h#includestdio.h#defineMAX100#definemaxD99999classCFixedMap{structCoordinate{intx;inty;intnum;};//记录点的结构体structcombp{intx1,y1;intx2,y2;};//连通点坐标structCombname{intnum1;intnum2;};//连通点名public:intmVexNum;//顶点数intmEdgNum;//边数doublemMatrix[MAX][MAX];//邻接矩阵Combnamecomb[MAX];//连通点名combpcp[MAX];//连通点坐标CoordinatemVexs[MAX];//顶点集合intgetx(intn1);intgety(intn2);voidInput();//以文件方式输入地图信息voidShowMap();//easyx显示地图};intCFixedMap::getx(intn1){intx=0;for(inti=0;imVexNum;i++)if(n1==mVexs[i].num)x=mVexs[i].x;returnx;}intCFixedMap::gety(intn2){inty=0;for(inti=0;imVexNum;i++)if(n2==mVexs[i].num)y=mVexs[i].y;returny;}voidCFixedMap::Input(){intpanju;charfname[10]={map2.txt};cout是否载入默认地图？（1:是，2:否）;cinpanju;if(panju==2){cout请输入地图信息文件名:;cinfname;}ifstreamfile(fname);charuse1[100];charuse2[100];charuse3[100];charuse4[100];//use数组用来储存提示信息。fileuse1;filemVexNum;//端点个数fileuse2;filemEdgNum;//边数fileuse3;for(inti=0;imVexNum;i++)//输入所有点的信息{filemVexs[i].nummVexs[i].xmVexs[i].y;}fileuse4;for(i=0;imEdgNum;i++)//输入连通点编号{filecomb[i].num1comb[i].num2;}file.close();}//easyx显示地图voidCFixedMap::ShowMap(){initgraph(800,600,SHOWCONSOLE|NOCLOSE);//初始化绘图环境setbkcolor(WHITE);//设置背景色cleardevice();//用背景色清空屏幕setfillcolor(BLACK);//设置填充色//画顶点for(inti=0;imVexNum;i++){fillcircle(mVexs[i].x,mVexs[i].y,5);settextcolor(BLACK);chars[MAX];sprintf(s,%d,i+1);outtextxy(mVexs[i].x-10,mVexs[i].y-20,s);}//给实际连通的顶点连线setlinecolor(RGB(0,0,0));setlinestyle(PS_SOLID,6,NULL,0);for(i=0;imEdgNum;i++){cp[i].x1=getx(comb[i].num1);cp[i].y1=gety(comb[i].num1);cp[i].x2=getx(comb[i].num2);cp[i].y2=gety(comb[i].num2);line(cp[i].x1,cp[i].y1,cp[i].x2,cp[i].y2);}}classCPathPlan:publicCFixedMap{public:intpath[MAX];//存放最后一个点doubledist[MAX];//存放路径距离intv0;//起始点introute[40];//路径intrnum;//路径顶点数Coordinaterpoints[MAX];//路径点集voidMaptoGraph();voidDijstra();voidPutpath();};voidCPathPlan::MaptoGraph(){rnum=0;inta,b,x,y;for(inti=0;imVexNum;i++){for(intj=0;jmVexNum;j++){if(i==j){mMatrix[i][j]=0;}else{mMatrix[i][j]=maxD;}}}for(i=0;imEdgNum;i++){if(comb[i].num1){a=comb[i].num1-1;b=comb[i].num2-1;x=mVexs[a].x-mVexs[b].x;y=mVexs[a].y-mVexs[b].y;mMatrix[a][b]=sqrt(x*x+y*y);mMatrix[b][a]=sqrt(x*x+y*y);}}}voidCPathPlan::Dijstra(){cout输入起点:;cinv0;v0--;ints[MAX];intv;inti;intj;intw;doublemin;for(v=0;vmVexNum;v++)//初始化S,dist,path{s[v]=0;//0表示未求出最短路径dist[v]=mMatrix[v0][v];//开始时假定为最短路径if(dist[v]maxD&&v!=v0)path[v]=v0;//直达情况elsepath[v]=-1;//无直达路径}dist[v0]=0;//初始时v0属于s集，v0到v0路径最短s[v0]=1;for(i=1;imVexNum;i++)//假设由一个中转点到达会近些，验证。再假设由两个中转点到达会近些，验证。穷举{min=maxD;for(w=0;wmVexNum;w++)//找一个最近到的点，下标装入vif(s[w]==0&&dist[w]min)//点w不属于s集且离v0更近{v=w;//经点w中转min=dist[w];}s[v]=1;//将v并入S,由v0到达v点最短距离为min,假设由v0到v再由v到其余各点，更新当前最后一个点及距离for(j=0;jmVexNum;j++)if(s[j]==0&&(min+mMatrix[v][j]dist[j]))//如果多经过一个点v到点j的距离更短，更新{dist[j]=min+mMatrix[v][j];path[j]=v;//点的序号}}}voidCPathPlan::Putpath(){intopp[20];intz;inti=0;cout输入终点:;cinz;z--;intnext;ofstreamfile2(fops.txt);if(dist[z]maxD&&z!=v0)//如果有路径{opp[i++]=mVexs[z].num;//终点next=path[z];//第一个点while(next!=v0){opp[i++]=mVexs[next].num;next=path[next];//下一个点}opp[i]=mVexs[v0].num;file2路径长度为:dist[z]'\n';//起点v0和最短路径}elseif(z!=v0)//无路径file2mVexs[z].num--mVexs[v0].numnopath'\n';for(intj=0;ji+1;j++)route[j]=opp[i-j];file2路径为:;for(j=0;ji;j++)file2route[j]--;rnum=i+1;file2route[i];file2.close();for(i=0;irnum;i++){rpoints[i].x=getx(route[i]);rpoints[i].y=gety(route[i]);rpoints[i].num=route[i];}}classCAGV{intspeed;//AGV速度intwidth;//AGV宽度intlength;//AGV长度public:CAGV();intgetl();intgetw();intgets();};CAGV::CAGV(){intpanju;cout是否自动设置AGV信息?（1:是,2:否）;cinpanju;speed=8;width=6;length=6;if(panju==2){cout输入AGV的长:;cinlength;cout输入AGV的宽:;cinwidth;cout输入AGV的速度(1-10):;cinspeed;}}intCAGV::getl(){returnlength;}intCAGV::getw(){returnwidth;}intCAGV::gets(){returnspeed;}classCAGVShow:publicCPathPlan,publicCAGV{public:#definek(abs(y2-y1))/(abs(x1-x2))inta,b,c,d;intx1,x2,y1,y2;inti,j;intl,w,s;voidshow();};voidCAGVShow::show(){l=getl();w=getw();s=11-gets();for(into=0;ornum;o++){if(o+1==rnum)break;x1=rpoints[o].x;x2=rpoints[o+1].x;y1=rpoints[o].y;y2=rpoints[o+1].y;if(y1!=y2&&x1!=x2){if(x2x1&&y2y1){for(j=1;jx2-x1;j++){a=x1+j-l;b=y1+j*k+w;c=x1+j+l;d=y1+j*k-w;setfillcolor(RGB(0,0,0));rectangle(a,b,c,d);clearrectangle(a-1,b-k,c,d-k);setlinecolor(RED);line(x1+j,y1+j*k,x2,y2);setlinecolor(GREEN);line(x1,y1,x1+j,y1+j*k);if(j%2==0){setfillcolor(RGB(255,255,0));circle(x2,y2,5);}Sleep(s);}}if(x2x1&&y2y1){for(j=1;jx1-x2;j++){a=x1-j-l;b=y1-j*k+w;c=x1-j+l;d=y1-j*k-w;setfillcolor(RGB(0,0,0));rectangle(a,b,c,d);clearrectangle(a-1,b-k,c,d-k);setlinecolor(RED);line(x1-j,y1-j*k,x2,y2);setlinecolor(GREEN);line(x1,y1,x1-j,y1-j*k);if(j%2==0){setfillcolor(RGB(255,255,0));circle(x2,y2,5);}Sleep(s);}}if(x2x1&&y2y1){for(j=1;jx1-x2;j++){a=x1-j-l;b=y1+j*k+w;c=x1-j+l;d=y1+j*k-w;setfillcolor(RGB(0,0,0));rectangle(a,b,c,d);clearrectangle(a-1,b-k,c,d