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

程序代码#include#include#include #include#include#include#include#define MAX 100#define maxD 99999class CFixedMapstruct Coordinateint x;int y;int num;/记录点的结构体struct combpint x1,y1;int x2,y2;/连通点坐标struct Combnameint num1;int num2;/连通点名public:int mVexNum; / 顶点数int mEdgNum; / 边数double mMatrixMAXMAX; / 邻接矩阵Combname combMAX; /连通点名combp cpMAX; /连通点坐标Coordinate mVexsMAX; / 顶点集合int getx(int n1);int gety(int n2);void Input();/以文件方式输入地图信息void ShowMap();/easyx显示地图;int CFixedMap:getx(int n1)int x=0;for(int i=0;imVexNum;i+)if(n1=mVexsi.num)x=mVexsi.x;return x;int CFixedMap:gety(int n2)int y=0;for(int i=0;imVexNum;i+)if(n2=mVexsi.num)y=mVexsi.y;return y;void CFixedMap:Input()int panju;char fname10=map2.txt;coutpanju;if(panju=2)coutfname;ifstream file(fname);char use1100;char use2100;char use3100;char use4100; /use数组用来储存提示信息。fileuse1;filemVexNum; /端点个数fileuse2;filemEdgNum; /边数fileuse3;for(int i=0;imVexsi.nummVexsi.xmVexsi.y;fileuse4;for(i=0;icombi.num1combi.num2;file.close();/easyx显示地图void CFixedMap:ShowMap()initgraph(800,600,SHOWCONSOLE| NOCLOSE); /初始化绘图环境setbkcolor(WHITE); /设置背景色cleardevice(); /用背景色清空屏幕setfillcolor(BLACK); /设置填充色/画顶点for(int i=0;imVexNum;i+) fillcircle(mVexsi.x,mVexsi.y,5);settextcolor(BLACK);char sMAX;sprintf(s, %d, i+1);outtextxy(mVexsi.x-10,mVexsi.y-20,s);/给实际连通的顶点连线setlinecolor(RGB(0,0,0);setlinestyle(PS_SOLID,6,NULL,0);for(i=0;imEdgNum;i+)cpi.x1=getx(combi.num1);cpi.y1=gety(combi.num1);cpi.x2=getx(combi.num2);cpi.y2=gety(combi.num2);line(cpi.x1,cpi.y1,cpi.x2,cpi.y2);class CPathPlan:public CFixedMappublic:int pathMAX; /存放最后一个点double distMAX; /存放路径距离int v0; /起始点int route40; /路径int rnum; /路径顶点数Coordinate rpointsMAX;/路径点集void MaptoGraph();void Dijstra();void Putpath();void CPathPlan:MaptoGraph()rnum=0;int a,b,x,y;for(int i=0;imVexNum;i+)for(int j=0;jmVexNum;j+)if(i=j)mMatrixij=0;elsemMatrixij=maxD;for(i=0;imEdgNum;i+)if (combi.num1)a=combi.num1-1;b=combi.num2-1;x=mVexsa.x-mVexsb.x;y=mVexsa.y-mVexsb.y;mMatrixab=sqrt(x*x+y*y);mMatrixba=sqrt(x*x+y*y);void CPathPlan:Dijstra()coutv0;v0-;int sMAX;int v;int i;int j;int w;double min;for(v=0;vmVexNum;v+) /初始化S,dist,pathsv=0; /0表示未求出最短路径distv=mMatrixv0v; /开始时假定为最短路径if(distvmaxD & v!=v0)pathv=v0; /直达情况else pathv=-1; /无直达路径distv0=0; /初始时v0属于s集，v0到v0路径最短sv0=1;for(i=1;imVexNum;i+) /假设由一个中转点到达会近些，验证。再假设由两个中转点到达会近些，验证。穷举min=maxD;for(w=0;wmVexNum;w+) /找一个最近到的点，下标装入vif(sw=0 & distwmin) /点w不属于s集且离v0更近v=w; /经点w中转min=distw;sv=1; /将v并入S,由v0到达v点最短距离为min,假设由v0到v再由v到其余各点，更新当前最后一个点及距离for(j=0;jmVexNum;j+)if(sj=0 & (min+mMatrixvjdistj) /如果多经过一个点v到点j的距离更短，更新distj=min+mMatrixvj;pathj=v; /点的序号void CPathPlan:Putpath()int opp20;int z;int i=0;coutz;z-;int next;ofstream file2(fops.txt);if(distzmaxD & z!=v0) /如果有路径oppi+=mVexsz.num; /终点next=pathz; /第一个点while(next!=v0)oppi+=mVexsnext.num;next=pathnext; /下一个点oppi=mVexsv0.num;file2路径长度为:distzn; /起点v0和最短路径else if(z!=v0) /无路径file2mVexsz.num-mVexsv0.numno pathn;for(int j=0;ji+1;j+)routej=oppi-j;file2路径为:;for(j=0;ji;j+)file2routej;rnum=i+1;file2routei;file2.close();for(i=0;irnum;i+)rpointsi.x=getx(routei);rpointsi.y=gety(routei);rpointsi.num=routei;class CAGVint speed; /AGV速度int width; /AGV宽度int length; /AGV长度public:CAGV();int getl();int getw();int gets();CAGV:CAGV()int panju;coutpanju;speed=8; width=6;length=6; if(panju=2)coutlength;coutwidth;coutspeed;int CAGV:getl()return length;int CAGV:getw()return width;int CAGV:gets()return speed;class CAGVShow:public CPathPlan,public CAGVpublic:#define k (abs(y2-y1)/(abs(x1-x2)int a,b,c,d;int x1,x2,y1,y2;int i,j;int l,w,s;void show();void CAGVShow:show()l=getl();w=getw();s=11-gets();for(int o=0;ox1&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(x2y1)for(j=1;jx1&y2y1)for(j=1;jx1)for(j=1;j(x2-x1);j+)setfillcolor(RGB(0,0,0);rectangle(x1+j-l,y1+w,x1+j+l,y1-w);clearrectangle(x1+j-l-1,y1+w,x1+j+l-1,y1-w);setlinecolor(RED);line(x1+j,y1,x2,y2);setlinecolor(GREEN);line(x1,y1,x1+j,y1);if(j%2=0)setfillcolor(RGB(255,0,255);circle(x2,y2,5);Sleep(s);if(x2x1)for(j=1;jy1)for(j=1;j(y2-y1);j+)setfillcolor(BLUE);rectangle(x1-l,y1+j+w,x1+l,y1+j-w);setfillcolor(WHITE);rectangle(x1,y1+j+w-1,x1+1,y1+j-1-w);clearrectangle(x1-l-1,y1+(j-1)+w,x1+l-1,y1+(j-1)-w);setlinecolor(RED);line(x1,y1+j,x2,y2);setlinecolor(GREEN);line(x1,y1,x1,y1+j);if(j%2=0)setfillcolor(RGB(255,255,0);circle(x2,y2,5);Sleep(s);if(y2y1)for(j=1;j(y1-y2);j+)setfillcolor(BLUE);rectangle(x1-l,y1-j+w,x1+l,y1-j-w);setfillcolor(WHITE);rectangle(x1,y1-j+w-1,x1+1,y1-j-1-w);clearrectangle(x1-l-1,y1-(j-1)+w,x1+l-1,y1-(j-1)-w);setlinecolor(RED);line(x1,y1-j,x2,y2);setlinecolor(GREEN);line(x1,y1,x1,y1-j);if(j%2=0)setfillcolor(RGB(255,255,0);circle(x2,y2,5);Sleep(s);cout成功显示!n;getch(); / 按任意键继续closegraph(); / 关闭图形界面void main()CAGVShow a;int panju;a.Input();coutpanju;if(panju=1)for(int i=0;i20;i+)a.ShowMap();a.MaptoGraph();a.Dijstra();a.Putpath();a.show();panju=0;coutpanju;if(panju=1)break;elsea.ShowMap();a.MaptoGraph();a.Dijstra();a.Putpath();a.show();相关文件/map2.txt点的个数(输入所有数据前请换行): 16连线条数：26依次输入所有点的编号，x,y坐标（用空格隔开）：1 50 50 2