求牛人指点 A星寻路算法最后一步
仿照别人的程序写了个A星的代码，所有路径都寻找出来了，就差之后一步如何把遍历出来的路径给按照最短路径输出，求助各位大牛了；
// FindPath.cpp : 定义控制台应用程序的入口点。
//
#include "stdafx.h"
#include "iostream"
/************************************************************************/
/* 产量定义 */
/************************************************************************/
struct _XY
{
int x;
int y;
};
struct MAP
{
bool RUN; //是否可以通过
int ocw;
int F; //评估值 F = G + H
int G; //到起点的移动值 10 14
int H; //到目标的绝对值 abs(now.x - target.x) + abs(now.y - target.y)
_XY XY;
};
const int mapW = 18;
const int mapH = 16;
const int Defaule0 = 0; //可以通过
const int Open1 = 1; //障碍物
const int Close4 = 4; // 使用过的
const int SUCC = 7; //路径
const int Start= 2;
const int End = 3;
_XY perentList[mapW][mapH]; //列表
_XY pathLc[mapW][mapH]; //路径位置
MAP mapList[mapW][mapH]; //记录地图属性
_XY openList[mapW*mapH];
int onOpenList =0;
_XY parent;
bool isFindPath =false;
bool Obstacle =false; //判断是否有墙
int t=0;
int X;
int Y;
int tg =0;
_XY StartPoint;
_XY TargetPoint;
_XY tgBf; // '记录找到目标的父节点
void FindPath()
{
memset((void*)&openList,0,sizeof(openList));
//初始化
for ( int x=0;x<mapW;x++)
{
for (int y=0;y<mapH;y++)
{
mapList[x][y].RUN = true;
mapList[x][y].ocw = Defaule0;
}
}
//设置起点
mapList[2][7].ocw= Start;
//设置终点
mapList[15][7].ocw = End;
//设置障碍
for (int i=3;i<mapH-3;i++)
{
int tmpw = mapW /2;
mapList[tmpw][i].ocw = Close4;
mapList[tmpw][i].RUN = false;
parent.x = tmpw;
parent.y = i;
}
std::cout<<" --------输出地图列表------\r\n";
for (int y=0;y<mapH;y++)
{
for ( int x=0;x<mapW;x++)
{
if (mapList[x][y].ocw == Start)
{
StartPoint.x = x;
StartPoint.y = y;
}
if (mapList[x][y].ocw == End)
{
TargetPoint.x = x;
TargetPoint.y = y;
}
std::cout<<mapList[x][y].ocw<<" ";
}
std::cout<<"\r\n";
}
getchar();
if (StartPoint.x==TargetPoint.x && StartPoint.y==TargetPoint.y)
{
std::cout<<"开始 = 结束 寻路结束\r\n";
return;
}
//初始化第一个位置
onOpenList = 1;
openList[1].x = StartPoint.x;
openList[1].y = StartPoint.y;
mapList[openList[1].x][openList[1].y].G = 0;
mapList[openList[1].x][openList[1].y].ocw=0;
mapList[openList[1].x][openList[1].y].RUN = true;
isFindPath = false;
while (!isFindPath)
{
t = onOpenList;
std::cout<<"当前节点t:"<<t<<" x:" << parent.x<< " y:" << parent.y<<"\r\n";
//取出F值最早的位置
for(int i=onOpenList;i>0;i--)
{
std::cout<<"遍历节点"<<i<<": " <<openList[i].x<<" " << openList[i].y<<" F:"<<mapList[openList[i].x][openList[i].y].F<<"\r\n";
if (mapList[openList[onOpenList].x][openList[onOpenList].y].F > mapList[openList[i].x][openList[i].y].F)
{
//if (openList[i].x != StartPoint.x && openList[i].y!=StartPoint.y)
{
std::cout<<"------------更新节点------------\r\n";
parent.x = openList[i].x;
parent.y = openList[i].y;
t =i;
}
}
}
std::cout<<"节t:"<<t<<" onOpenList:"<<onOpenList<<" x:"<< parent.x << " y:" <<parent.y<<"\r\n";
//抽出第i个元素后，把后面的元素接上
if (t!=onOpenList)
{
for (int j=t;j!=onOpenList;j++)
{
openList[j].x = openList[j+1].x;
openList[j].y = openList[j+1].y;
}
openList[onOpenList].x = parent.x;
openList[onOpenList].y = parent.y;
}
parent.x = openList[onOpenList].x;
parent.y = openList[onOpenList].y;
X = parent.x;
Y = parent.y;
std::cout<<"parent:"<<X<<" "<<Y<<" onOpenList:" <<onOpenList<<"\r\n";
// for (int i=0;i<mapW * mapH;i++)
// {
// if (x)
// {
// }
// }
onOpenList--;
mapList[parent.x][parent.y].ocw = SUCC;
/*'--------------------------------
' 判断父节点上下左右四个方向有墙
'--------------------------------
' 1 2 3 5为父节点
' 4 5 6 分别对 1,7,3,9 四个位置的 (上,下,左,右)其中2个方向进行判断
' 7 8 9 如果是有障碍物就不设置从该父节点到这该点的路径
'0:左上 1:右上 2:左下 3:右下 4:上 5:左 6:右 7:下
'--------------------------------*/
for (int b=parent.y-1;b<=parent.y+1;b++)
{
for (int a=parent.x-1;a<=parent.x+1;a++)
{
if (a!=-1 && a<=mapW && b!=-1 && b<=mapH)//'判断有没有超出范围
{
if (mapList[a][b].RUN) //判断是否可行
{
Obstacle = false; //强制没有墙
if ( a==parent.x-1)
{
if (b==parent.y-1) // '左上角(1)
{
if (!mapList[parent.x][parent.y-1].RUN || !mapList[parent.x-1][parent.y].RUN )// '判断2,4是否有墙
{
Obstacle = true;
}
}
else if(b==parent.y+1)// '左下角(7)
{
if (!mapList[parent.x][parent.y+1].RUN || !mapList[parent.x-1][parent.y].RUN )// '判断8,4是否有墙
{
Obstacle = true;
}
}
}
else if(a==parent.x +1)
{
if (b==parent.y-1) // '右上角(3)
{
if (!mapList[parent.x][parent.y-1].RUN || !mapList[parent.x+1][parent.y].RUN )// '判断2,6是否有墙
{
Obstacle = true;
}
}
else if(b==parent.y+1)// 右下角(9)
{
if (!mapList[parent.x][parent.y+1].RUN || !mapList[parent.x+1][parent.y].RUN )// '判断8,6是否有墙
{
Obstacle = true;
}
}
}
if (!Obstacle)//没有墙
{
if (mapList[a][b].ocw ==Open1) //判断是否在开启列表中
{
if (abs(parent.x-a)==1 && abs(parent.y-b)==1)
{
t = 14;
}
else
{
t = 10;
}
tg = mapList[parent.x][parent.y].G + t;
if (tg<mapList[a][b].G)// ' 如果把现在的节点当父节点的话.距离近了就设这为新的父节点
{
perentList[a][b].x = parent.x;
perentList[a][b].y = parent.y;
}
}
else if (mapList[a][b].ocw == Defaule0)
{
if (abs(parent.x-a)==1 && abs(parent.y-b)==1)
{
t = 14;
}
else
{
t = 10;
}
//设置GHF
mapList[a][b].G = mapList[parent.x][parent.y].G + t;
mapList[a][b].H = 10*(abs(a-TargetPoint.x) + abs(b-TargetPoint.y));
mapList[a][b].F = mapList[a][b].G + mapList[a][b].H;
perentList[a][b].x = parent.x;
perentList[a][b].y = parent.y;
onOpenList++;
openList[onOpenList].x = a;
openList[onOpenList].y = b;
mapList[a][b].ocw = Open1; // '设置路径为开启
std::cout<<"计算节点:"<<parent.x<<" "<<parent.y<<" ---> " <<a<<" " <<b <<" onOpenList:" <<onOpenList<<"\r\n";
for(int i=0;i<mapW*mapH;i++)
{
if (a!=TargetPoint.x || b!=TargetPoint.y)
{
if (a!=StartPoint.x ||b!=StartPoint.y)
{
mapList[a][b].ocw = SUCC;//成功通过
}
}
}
}
//判断是否到达终点
if (a==TargetPoint.x && b==TargetPoint.y)
{
tgBf.x = a ; //'记录找到目标的父节点
tgBf.y = b;
isFindPath = true;
std::cout<<"开始 = 结束 寻路结束2 \r\n";
}
}
}
}
}
}
std::cout<<" --------完成 输出地图列表------\r\n";
for (int y=0;y<mapH;y++)
{
for ( int x=0;x<mapW;x++)
{
std::cout<<mapList[x][y].ocw<<" ";
}
std::cout<<"\r\n";
}getchar();
}
std::cout<<" --------完成 输出路径列表------\r\n";
for (int y=0;y<mapH;y++)
{
for ( int x=0;x<mapW;x++)
{
//std::cout<<"|"<<perentList[x][y].x<<"."<<perentList[x][y].y;
}
std::cout<<"\r\n";
}
getchar();
std::cout<<"寻路完成输出路径：\r\n";
if (isFindPath)
{
// 输 出 路 径
X = TargetPoint.x;
Y = TargetPoint.y;
int pathLcNumber = 1;
std::cout<<"第"<< pathLcNumber <<"步: " << X << " " << Y <<"\r\n";
while (( perentList[X][Y].x ==StartPoint.x && perentList[X][Y].y==StartPoint.y ) == false)
{
t = perentList[X][Y].x;
Y = perentList[X][Y].y;
X = t;
pathLcNumber++;//保存位置
std::cout<<"第"<< pathLcNumber <<"步: " << X << " " << Y <<"\r\n";
//pathLc[pathLcNumber]->x = X;
//pathLc[pathLcNumber]->y = Y;
}
for (int i=0;i<pathLcNumber;i++)
{
//X = pathLc[i]->x;
//Y = pathLc[i]->y;
//std::cout<<"第"<<i<<"步: " << X << " " << Y <<"\r\n";
}
getchar();
}
}
int _tmain(int argc, _TCHAR* argv[])
{
FindPath();
getchar();
return 0;
}
//设置起点 mapList[2][7].ocw= Start;
//设置终点 mapList[15][7].ocw = End;
