用了一周时间将A*算法的操作流程理解并编程实现,整个过程从开始理解算法的具体内容到将程序调试好并能够在20*20的方格上实现寻路,时间分配如下:
1.周一~周三:理解A*算法含义
2.周四~周日:编程、调试
注:最开始学习A*算法的时候,刚理解了A*大概的思路就感觉很简单,真正编程去实现时和概念上的A*格格不入。
先聊一下
A*的概念:
就是在保证从起点开始向外扩展点的代价g(n)最小的同时还要兼顾从扩展点到终点的代价h(n)最小,能够明确向哪个方向走预计的代价会小,这样就有了
启发性(heuristic)
。
说到A*算法,它的公式是f(n)=g(n)+h(n),f(n)是起点经过第n点到终点的代价函数,g(n)是起点到第n点的代价函数,h(n)是第n点到终点的启发函数(heuristic)
下面说一下它的
适用情况
:
1.对于启发函数h(n)来说:
1).期望值h(n)<实际值h'(n),则
保证能找到最短路径,但是h(n)越小,需要扩展的点越多,效率越低。
2).期望值h(n)==实际值h'(n),A*算法会之
遵循最佳路径而不会扩展到其他任何结点,效率很高,但是几乎不可能找到。
3).期望值h(n)>实际值h'(n),A*
不能保证找到一条最短路径,但运行的很快。
2.极端情况下,当g(n)==0时,f(n)=h(n)就只考虑了从第n点到终点这段代价对于整个代价函数的影响,这时A*算法变成了贪婪最佳优先搜索算法(Greedy Best-First-search),
A*不能保证找到一条最短路径,但是它效率非常高。
3.极端情况下,当h(n)==0时,f(n)=g(n)就只考虑了从起点到第n个点的代价对于整体代价函数的影响,A*变成迪杰斯特拉算法,就能
保证找到最短路径,但是运行效率最慢。
程序执行的前提约定:
1.从当前结点可以向邻近8个方向移动,即可以沿对角线移动,其中横纵移动代价为10,沿对角线移动代价为14。
2.先在txt文件中设计起点's'、终点'e'、障碍物'b'的位置,普通路径结点用'#'表示,再通过读取txt文件将棋盘数据读入到棋盘二维数组中进行后续处理,找到路径'*'后将路径打印到控制台。
原始棋盘 寻路后的棋盘
3.估价函数使用曼哈顿距离,即第n个结点与终点的横坐标差的绝对值加上纵坐标差的绝对值。
具体工作逻辑:
1.建立openlist和closelist,openlist中存放根据
当前结点扩展的
邻近结点,closelist中存放的是每次扩展后openlist按f值由大到小排序后,f值最小的那个结点。
2.将起点存入openlist
3.将openlist中的元素按照f值由大到小排列。
4.取出openlist中f值最小的元素,先将其存入closelist中,再将其作为
当前结点
5.对
当前结点8个方向的
邻近结点从右结点开始顺时针依次进行判断并计算:
1).该
邻近结点是否是
终点,如果是,直接返回“已找到终点”
2).该
邻近结点是否超出边界(现在定棋盘大小是20*20),如果是,结束对该
邻近点的后续判断
3).该
邻近结点是否是障碍物结点,如果是,结束对该
邻近点的后续判断
4).该
邻近结点是否是closelist中的结点,如果是,结束对该
邻近点的后续判断
5).该
邻近结点是否是openlist中的结点,如果是,继续判断:
a).该
邻近结点的g值是否小于
当前结点的g值加上当前结点到该
邻近结点的距离值,如果小于,则结束对该
邻近点的后续判断,因为我们就是要保证每个点g值最小,结束后续判断。
b).该
邻近结点的g值是否大于
当前结点的g值加上
当前结点到该
邻近结点的距离值,如果大于,则将该
邻近结点的g值修改为
当前结点的g值加上当前结点到该
邻近结点的距离值,并将该
邻近结点父指针指向
当前结点,因为该
邻近结点原来的父路径g值比以
当前结点作为父路径g值要大,而最终目的是要选择g值小的,所以要将父指针修改为
当前结点,并修改该
邻近结点的g值和f值,结束后续判断。
6).该
邻近结点是否是“可作为路径”的结点,如果是,则将其坐标值和f值存入openlist中,
6.返回第3步循环执行,直到openlist中为空。
7.如果找到
终点,根据终点的父指针找到它的父节点,在由父节点的父指针找到父父结点......直到找到起点,结束。
关键问题:
上面所说的是程序的具体流程,而下面我要把整个编程过程中遇到的一些关键问题详细叙述一下:
对于数据结构的定义:
开始,我是想用
链表来定义openlist、closelist和最短路径,用20*20
二维数组定义棋盘,openlist中有一个对元素按f值大小排序的过程,所以习惯上改数组来定义openlist,但是用链表是完全可以的,可以继续优化。由于每次要f最小值的元素,所以我按由大到小排列,这样只需移动一次末尾指针即可得到f最小元素,反之则所有元素都要移位,closelist也同理。存放最短路径我本来想用一条链表来做,后来才发现是根本不可能的,因为
这种算法是只有找到终点之后,才能根据其父指针将最短路径逆序求出,想用链表正序去记根本不可能。所以没有专门给最短路径加一条链表。只是将棋盘定义为结构体数组,每个元素都包含f,h,g,父指针这四个信息。
openlist列表按f值由大到小排序
定义棋盘方格单元结构体
对于第5步有必要描述一下,第5步中的5个判断本应该都是
互斥的,所以应该用if(){}...else if(){}...else{}语句,但是在4)和5)步中,对于openlist和closelist的判断不能一次就执行完,因为列表中的值不止一个,所以需要将该邻近结点的坐标与openlist中存放的所有坐标都比较一次,这样必须循环判断,我这里用了for循环,但是这样就破坏了if(){}...else if(){}...else{}结构,无法互斥,对于第5步中的5个判断都改用了if(){}条件判断语句,这样也不互斥,会造成同一个邻近结点进行了多次处理,这是不合乎逻辑的,这里的处理我添加了两个标志位
OpenListFlag和
CloseListFlag,一旦确定该邻近结点是openlist或closelist中的值即将OpenListFlag或CloseListFlag置1,然后下一步continue结束对该节点的后续判断,如果不设标志位,则当第5)步已经判断该点是openlist中的点后,会继续执行到第6)步,它仍然满足普通路径结点的条件,这样会重复存入openlist列表中,产生错误。这块处理其实可以在棋盘方格的结构体数组中再加上一个标志以区分openlist、closelist和未加入前两个列表的普通节点这三种类别,但是每一个方格都要多加一步处理还是很麻烦,所以前一种方式还是可以的。
for语句对openlist和closelist中元素循环比较
OpenListFlag或CloseListFlag置1,结束对该邻近结点后续操作
沿着对角线移动时,遇到障碍物需要特殊处理,比如对于下图来说绿色代表起点,蓝色代表障碍物,红色代表终点,当前起点的右下方方格不可能直接斜方向移动到蓝色障碍物下方的方格,所以这种情况需要特殊处理。如果不处理则会忽略蓝色障碍物直接斜向移动,这在实际中是行不通的,必须处理成如图3方式绕过障碍物。
隔着蓝色障碍物如箭头所示移动(错误)
绕过障碍物移动(正确)
处理的方式如下:即在第5步对当前结点的邻近8个节点依次判断时,一旦东、南、西、北四个方向上出现障碍物、则必须进行特殊处理,比如:判断出东方邻近点是障碍物,则停止判断东北、东南两个斜角方向的邻近点,即将这两个点视为障碍物,其他南、西、北三个方向结点同理。但是由于第5步的判断顺序是先判断东方邻近点之后顺时针判断其余各个邻近点,所以判断顺序要改,即先判断东、南、西、北四个方向的邻近点,再根据判断的结果去判断东南、西南、西北、东北四个方向上的邻近点。所以将其拆开为两组,每组4个去判断。
对东、南、西、北四个方向上的邻近结点进行判断
障碍物方向对应flag标志置1
根据对东、南、西、北四个方向标志位的判断决定剩余四个方向是否继续判断
这样就完成了对障碍物边角的处理,下图是处理障碍物边角前后的对比,需要说明的是这虽然表面是bug被修正了,但是其实是一个承上启下的过程,我是开始意识到会有这部分bug存在,但是为了把整体寻路过程能够调试通过,所以优先实现边角未处理的寻路,再修正边角。
处理障碍物边角 未处理障碍物边角
下面贴出全部源码,希望大家能多交流,还会有后续改进!
注:运行前必须在d盘建立一个AstarFindWay.txt文件(路径及文件名程序中可修改),并将开头注释的棋盘拷贝到文件中去(去掉注释)
//定义棋盘、起点、终点、障碍物
//约定:棋盘外框都视为障碍物
//########b###########
//########b###########
//##s#####b###########
//########b########e##
//########b###########
//########b###########
//######b#b###########
//######b#bbbbbb######
//######b######b######
//#########b###b######
//#########b###b######
//#########b###b#bbb##
//#########b##########
//####################
//####################
//####################
//####################
//####################
//####################
//####################
#include "stdafx.h"
#include "iostream"
#include "fstream"
using namespace std;
#define chess_size 20
#define FILE_STORAGE_WAY "d:\\AstarFindWay.txt" //随机数据文件存放路径
struct PARENT_POSITION//父指针
{
int x;
int y;
};
struct CHESS_BOARD_UNIT
{
//public:
// CHESS_BOARD_UNIT() {}//构造函数
// ~CHESS_BOARD_UNIT() {}//析构函数
// void setGValue(int g_value) { this->g_value = g_value; }//设置g值
// void setHValue(int h_value) { this->h_value = h_value; }//设置h值
// void setFValue(int f_value) { this->f_value = f_value; }//设置f值
// void setFlag(int flag) { this->flag = flag; }//设置状态值
//
// int getGValue() { return g_value; }//获取g值
// int getHValue() { return h_value; }//获取h值
// int getFValue() { return f_value; }//获取f值
// int getFlag() { return flag; }//获取状态值
//protected:
//struct CHESS_BOARD_UNIT *parent;//指向父节点指针
PARENT_POSITION ParentPosition;
int g_value;
int h_value;
int f_value;
char flag;//标识此单元是:2起点/3终点/1障碍物/0通路
};
struct OpenListUnit //定义openlist元素结构体
{
int ChessUnit_x;
int ChessUnit_y;
//int pointer;
int f_value;
};
struct CloseListUnit //定义closelist元素结构体
{
int ChessUnit_x;
int ChessUnit_y;
};
struct CurrentNeiborUnit //定义当前结点结构体
{
int ChessUnit_x;
int ChessUnit_y;
};
struct PrintPathUnit
{
int ChessUnit_x;
int ChessUnit_y;
};
struct ObstacleUnit
{
int ChessUnit_x;
int ChessUnit_y;
};
CHESS_BOARD_UNIT chessboard[chess_size][chess_size] = { 0 };//chessboard初始化为0
int Start_x, Start_y, End_x, End_y;//起点,终点坐标
//ObstacleUnit Obstacle[chess_size*chess_size];//障碍物点坐标
int OpenListPosition = 0; //指针标记open列表数组当前存放元素个数(position总指向最后一个元素的后一位置)
int CloseListPosition = 0; //指针标记Close列表数组当前存放元素个数(position总指向最后一个元素的后一位置)
OpenListUnit OpenList[chess_size*chess_size];//open列表(定义成数组形式)
CloseListUnit CloseList[chess_size*chess_size];//close列表
void openListIncraseSort()
{
int flag = 0;
OpenListUnit temp;
for (int i = 0; i < OpenListPosition; i++)//冒泡法由大到小排序
{
for (int j = 0; j < OpenListPosition - i; j++)
{
if (OpenList[j].f_value < OpenList[j + 1].f_value)//若OpenList[j].f_value < OpenList[j + 1].f_value,交换,从大到小排列
{
temp.ChessUnit_x = OpenList[j].ChessUnit_x;
temp.ChessUnit_y = OpenList[j].ChessUnit_y;
temp.f_value = OpenList[j].f_value;
OpenList[j].ChessUnit_x = OpenList[j + 1].ChessUnit_x;
OpenList[j].ChessUnit_y = OpenList[j + 1].ChessUnit_y;
OpenList[j].f_value = OpenList[j + 1].f_value;
OpenList[j + 1].ChessUnit_x = temp.ChessUnit_x;
OpenList[j + 1].ChessUnit_y = temp.ChessUnit_y;
OpenList[j + 1].f_value = temp.f_value;
flag = 1;//将交换标志位置1
}//if
}//for2
if (0 == flag) break;//内层循环一次都没交换,说明已经排好序
flag = 0;//将标志位重新清0
}//for1
//for(int i = 0; i < OpenListPosition; i++)
//{
// cout << OpenList[i].ChessUnit_x << ","<< OpenList[i].ChessUnit_y << endl;
//}
}
int dealCurrentNeibors(CloseListUnit &CurrentUnit)//判断当前结点周围8个点状态,计算g、h、f值,将周围每个点的父指针指向当前结点
{
int OpenListFlag = 0, CloseListFlag = 0;
int ObstacleEast = 0; //标识当前点东边是否有障碍物
int ObstacleSouth = 0; //标识当前点南边是否有障碍物
int ObstacleWest = 0; //标识当前点西边是否有障碍物
int ObstacleNorth = 0; //标识当前点北边是否有障碍物
//CloseListUnit EastUnit, SourthEastUnit, SourthUnit, SourthWestUnit, WestUnit, NorthWestUnit, NorthUnit, NorthEastUnit;//东,东南,南,西南,西,西北,北,东北
CurrentNeiborUnit CurrentNeibors[8];//按顺序存放当前结点的东,东南,南,西南,西,西北,北,东北方向的邻结点
//int OpenListPosition = 0; //指针标记open列表数组当前存放元素个数(position总指向最后一个元素的后一位置)
//int CloseListPosition = 0; //指针标记Close列表数组当前存放元素个数(position总指向最后一个元素的后一位置)
//东方y+1
CurrentNeibors[0].ChessUnit_x = CurrentUnit.ChessUnit_x;
CurrentNeibors[0].ChessUnit_y = CurrentUnit.ChessUnit_y + 1;
//东南方x+1,y+1
CurrentNeibors[1].ChessUnit_x = CurrentUnit.ChessUnit_x + 1;
CurrentNeibors[1].ChessUnit_y = CurrentUnit.ChessUnit_y + 1;
//南方x+1
CurrentNeibors[2].ChessUnit_x = CurrentUnit.ChessUnit_x + 1;
CurrentNeibors[2].ChessUnit_y = CurrentUnit.ChessUnit_y;
//西南方x+1,y-1
CurrentNeibors[3].ChessUnit_x = CurrentUnit.ChessUnit_x + 1;
CurrentNeibors[3].ChessUnit_y = CurrentUnit.ChessUnit_y - 1;
//西方y-1
CurrentNeibors[4].ChessUnit_x = CurrentUnit.ChessUnit_x;
CurrentNeibors[4].ChessUnit_y = CurrentUnit.ChessUnit_y - 1;
//西北方x-1,y-1
CurrentNeibors[5].ChessUnit_x = CurrentUnit.ChessUnit_x - 1;
CurrentNeibors[5].ChessUnit_y = CurrentUnit.ChessUnit_y - 1;
//北方x-1
CurrentNeibors[6].ChessUnit_x = CurrentUnit.ChessUnit_x - 1;
CurrentNeibors[6].ChessUnit_y = CurrentUnit.ChessUnit_y;
//东北方x-1,y+1
CurrentNeibors[7].ChessUnit_x = CurrentUnit.ChessUnit_x - 1;
CurrentNeibors[7].ChessUnit_y = CurrentUnit.ChessUnit_y + 1;
for (int i = 0; i < 8; i = i + 2)//对当前方格东、南、西、北四个方向的临近方格依次检测
{
//终点
if ('e' == chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].flag)
{
chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].ParentPosition.x = CurrentUnit.ChessUnit_x;
chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].ParentPosition.y = CurrentUnit.ChessUnit_y;
return 1;
//break;//找到终点,结束循环
}
//超出边界点
if (CurrentNeibors[i].ChessUnit_x < 0 || CurrentNeibors[i].ChessUnit_x>19 || CurrentNeibors[i].ChessUnit_y < 0 || CurrentNeibors[i].ChessUnit_y>19)
{
continue;//结束判断
}
//障碍物点
if ('b' == chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].flag)
{
switch (i)
{
case 0: ObstacleEast = 1; break;//标识当前点东边有障碍物
case 2: ObstacleSouth = 1; break;//标识当前点南边有障碍物
case 4: ObstacleWest = 1; break;//标识当前点西边有障碍物
case 6: ObstacleNorth = 1; break;//标识当前点北边有障碍物
default:break;
}
continue;//结束判断
}
//将该临近点与closelist中的点逐个比较
for (int j = 0; j < CloseListPosition; j++)
{
if ((CloseList[j].ChessUnit_x == CurrentNeibors[i].ChessUnit_x) && (CloseList[j].ChessUnit_y == CurrentNeibors[i].ChessUnit_y))//是closelist中的点
{
CloseListFlag = 1;只要是closelist中的点,就将标志位置1
break;
}
}
//将该临近点与openlist中的点逐个比较
for (int j = 0; j < OpenListPosition; j++)
{
if ((OpenList[j].ChessUnit_x == CurrentNeibors[i].ChessUnit_x) && (OpenList[j].ChessUnit_y == CurrentNeibors[i].ChessUnit_y))//是openlist中的点
{
OpenListFlag = 1;//只要是openlist中的点,就将标志位置1
if (chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].g_value < chessboard[CurrentUnit.ChessUnit_x][CurrentUnit.ChessUnit_y].g_value + (i % 2) ? 14 : 10)
{
break;//若该临近点的g值小于从起点经由当前结点到该临近结点的g值,不做任何操作
}
else//若该临近点的g值大于从起点经由当前结点到该临近结点的g值,将该临近结点的父指针指向当前结点,并更改该临近结点的g值,f值
{
//将该临近结点的父指针指向当前结点
chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].ParentPosition.x = CurrentUnit.ChessUnit_x;
chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].ParentPosition.y = CurrentUnit.ChessUnit_y;
//更改该临近结点的g值,f值
chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].g_value = chessboard[CurrentUnit.ChessUnit_x][CurrentUnit.ChessUnit_y].g_value + (i % 2) ? 14 : 10;
chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].f_value = chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].g_value + chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].h_value;
}
}
}
if ((1 == OpenListFlag) || (1 == CloseListFlag))//该邻近点是openlist或closelist中的点
{
OpenListFlag = 0;//清0
CloseListFlag = 0;//清0
continue;//结束对此邻近点的继续判断(防止把该临近点当做通路点重复加入到openlist中)
}
//可作为通路点
if ('#' == chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].flag)
{
//将邻居结点的指针指向当前结点
chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].ParentPosition.x = CurrentUnit.ChessUnit_x;
chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].ParentPosition.y = CurrentUnit.ChessUnit_y;
//计算该临近结点的g值,h值(曼哈顿距离),f值
chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].g_value = chessboard[CurrentUnit.ChessUnit_x][CurrentUnit.ChessUnit_y].g_value + (i % 2) ? 14 : 10;
chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].h_value = 10 * (abs(CurrentNeibors[i].ChessUnit_x - End_x) + abs(CurrentNeibors[i].ChessUnit_y - End_y));
chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].f_value = chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].g_value + chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].h_value;
//将该临近点存入openlist中
OpenList[OpenListPosition].ChessUnit_x = CurrentNeibors[i].ChessUnit_x;
OpenList[OpenListPosition].ChessUnit_y = CurrentNeibors[i].ChessUnit_y;
OpenList[OpenListPosition].f_value = chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].f_value;
OpenListPosition++;
}//if
}//for
for (int i = 1; i < 8; i = i + 2)//对当前方格东南、西南、西北、东北四个方向的临近方格依次检测
{
if ((1 == ObstacleEast) && ((1 == i) || (7 == i)))//若东方格是障碍物,则东南、东北都不能通行
{
continue;
}
if ((1 == ObstacleSouth) && ((1 == i) || (3 == i)))//若南方格是障碍物,则东南、西南都不能通行
{
continue;
}
if ((1 == ObstacleWest) && ((3 == i) || (5 == i)))//若西方格是障碍物,则西南、西北都不能通行
{
continue;
}
if ((1 == ObstacleNorth) && ((5 == i) || (7 == i)))//若北方格是障碍物,则西北、东北都不能通行
{
continue;
}
//终点
if ('e' == chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].flag)
{
chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].ParentPosition.x = CurrentUnit.ChessUnit_x;
chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].ParentPosition.y = CurrentUnit.ChessUnit_y;
return 1;
//break;//找到终点,结束循环
}
//超出边界点
if (CurrentNeibors[i].ChessUnit_x < 0 || CurrentNeibors[i].ChessUnit_x>19 || CurrentNeibors[i].ChessUnit_y < 0 || CurrentNeibors[i].ChessUnit_y>19)
{
continue;//结束判断
}
//障碍物点
if ('b' == chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].flag)
{
continue;//结束判断
}
//将该临近点与closelist中的点逐个比较
for (int j = 0; j < CloseListPosition; j++)
{
if ((CloseList[j].ChessUnit_x == CurrentNeibors[i].ChessUnit_x) && (CloseList[j].ChessUnit_y == CurrentNeibors[i].ChessUnit_y))//是closelist中的点
{
CloseListFlag = 1;只要是closelist中的点,就将标志位置1
break;
}
}
//将该临近点与openlist中的点逐个比较
for (int j = 0; j < OpenListPosition; j++)
{
if ((OpenList[j].ChessUnit_x == CurrentNeibors[i].ChessUnit_x) && (OpenList[j].ChessUnit_y == CurrentNeibors[i].ChessUnit_y))//是openlist中的点
{
OpenListFlag = 1;//只要是openlist中的点,就将标志位置1
if (chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].g_value < chessboard[CurrentUnit.ChessUnit_x][CurrentUnit.ChessUnit_y].g_value + (i % 2) ? 14 : 10)
{
break;//若该临近点的g值小于从起点经由当前结点到该临近结点的g值,不做任何操作
}
else//若该临近点的g值大于从起点经由当前结点到该临近结点的g值,将该临近结点的父指针指向当前结点,并更改该临近结点的g值,f值
{
//将该临近结点的父指针指向当前结点
chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].ParentPosition.x = CurrentUnit.ChessUnit_x;
chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].ParentPosition.y = CurrentUnit.ChessUnit_y;
//更改该临近结点的g值,f值
chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].g_value = chessboard[CurrentUnit.ChessUnit_x][CurrentUnit.ChessUnit_y].g_value + (i % 2) ? 14 : 10;
chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].f_value = chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].g_value + chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].h_value;
}
}
}
if ((1 == OpenListFlag) || (1 == CloseListFlag))//该邻近点是openlist或closelist中的点
{
OpenListFlag = 0;//清0
CloseListFlag = 0;//清0
continue;//结束对此邻近点的继续判断(防止把该临近点当做通路点重复加入到openlist中)
}
//可作为通路点
if ('#' == chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].flag)
{
//将邻居结点的指针指向当前结点
chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].ParentPosition.x = CurrentUnit.ChessUnit_x;
chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].ParentPosition.y = CurrentUnit.ChessUnit_y;
//计算该临近结点的g值,h值(曼哈顿距离),f值
chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].g_value = chessboard[CurrentUnit.ChessUnit_x][CurrentUnit.ChessUnit_y].g_value + (i % 2) ? 14 : 10;
chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].h_value = 10 * (abs(CurrentNeibors[i].ChessUnit_x - End_x) + abs(CurrentNeibors[i].ChessUnit_y - End_y));
chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].f_value = chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].g_value + chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].h_value;
//将该临近点存入openlist中
OpenList[OpenListPosition].ChessUnit_x = CurrentNeibors[i].ChessUnit_x;
OpenList[OpenListPosition].ChessUnit_y = CurrentNeibors[i].ChessUnit_y;
OpenList[OpenListPosition].f_value = chessboard[CurrentNeibors[i].ChessUnit_x][CurrentNeibors[i].ChessUnit_y].f_value;
OpenListPosition++;
}//if
}//for
return 0;
}
void printPath()
{
int i = 0;//循环变量
PrintPathUnit PathUnit;//逆序存放单路径结点(终点->起点)
PrintPathUnit PathUnitList[chess_size*chess_size];//逆序存放所有路径结点(终点->起点)
//获取终点的坐标
PathUnit.ChessUnit_x = End_x;
PathUnit.ChessUnit_y = End_y;
cout << "(" << PathUnit.ChessUnit_x << "," << PathUnit.ChessUnit_y << ")" << endl;//输出终点坐标
//记录从end起第一个最佳路径结点
PathUnit.ChessUnit_x = chessboard[End_x][End_y].ParentPosition.x;
PathUnit.ChessUnit_y = chessboard[End_x][End_y].ParentPosition.y;
while (!((PathUnit.ChessUnit_x == Start_x) && (PathUnit.ChessUnit_y == Start_y))) //记录从终点到起点之间的最佳路径
{
PathUnitList[i].ChessUnit_x = PathUnit.ChessUnit_x;
PathUnitList[i].ChessUnit_y = PathUnit.ChessUnit_y;
chessboard[PathUnitList[i].ChessUnit_x][PathUnitList[i].ChessUnit_y].flag = '*';//将最佳路径点用"*"表示
cout << "(" << PathUnitList[i].ChessUnit_x << "," << PathUnitList[i].ChessUnit_y << ")" << endl;//输出路径结点坐标
//获取当前结点的父节点坐标
PathUnit.ChessUnit_x = chessboard[PathUnitList[i].ChessUnit_x][PathUnitList[i].ChessUnit_y].ParentPosition.x;
PathUnit.ChessUnit_y = chessboard[PathUnitList[i].ChessUnit_x][PathUnitList[i].ChessUnit_y].ParentPosition.y;
i++;
}
cout << "(" << Start_x << "," << Start_y << ")" << endl;//输出终点坐标
for (int i = 0; i < chess_size; i++)
{
for (int j = 0; j < chess_size; j++)
{
cout << chessboard[i][j].flag;
}
cout << endl;
}
}
void FileReadMatrix() //将文件中的数据读回至chess_size*chess_size矩阵中
{
int i = 0;
int j = 0;
//uint uiTemp; //定义变量存放从文件中读出的数据
ifstream ifile; //定义输入文件
ifile.open(FILE_STORAGE_WAY); //作为输入文件打开
while (1)
{
if (ifile.eof() != 0) break; //当读到文件结束时,ifile.eof()为真
ifile >> chessboard[i][j].flag; //将文件中的数据依次存放到数组中
if ('s' == chessboard[i][j].flag)//检测并记录起始点坐标
{
Start_x = i;
Start_y = j;
}
if ('e' == chessboard[i][j].flag)//检测并记录终点坐标
{
End_x = i;
End_y = j;
}
j++;
if (chess_size == j)//检测到达行尾
{
j = 0;
i++;//换行
}
}
for (int i = 0; i < 20; i++)
{
for (int j = 0; j < 20; j++)
{
cout << chessboard[i][j].flag;
}
cout << endl;
}
}
int main()
{
int PauseCin;
FileReadMatrix();
CloseListUnit CurrentUnit;
//chessboard[4][3].flag = 's';//起点
//chessboard[3][17].flag = 'e';//终点
//chessboard[2][8].flag = 'b';//障碍物
//chessboard[3][8].flag = 'b';//障碍物
//chessboard[4][8].flag = 'b';//障碍物
//chessboard[5][8].flag = 'b';//障碍物
//将起点加入open列表
OpenList[OpenListPosition].ChessUnit_x = Start_x;
OpenList[OpenListPosition].ChessUnit_y = Start_y;
OpenList[OpenListPosition].f_value = 0;
//OpenList[0].pointer = 1;//指针标记open列表数组当前存放元素个数
OpenListPosition++;//将起始结点存入openlist,position标记为1(position总指向最后一个元素的后一位置)
while (OpenListPosition > 0)//若openlist不为空
{
openListIncraseSort();//openlist列表按f值大小降序排序(将最小f值点放在最后,这样只需将position减1就代表移出该点)
//将openlist中f值最小的点移入closelist中
CloseList[CloseListPosition].ChessUnit_x = OpenList[OpenListPosition - 1].ChessUnit_x;
CloseList[CloseListPosition].ChessUnit_y = OpenList[OpenListPosition - 1].ChessUnit_y;
//openlist移出f值最小元素,清除原位置该元素信息
OpenList[OpenListPosition - 1].ChessUnit_x = 0;
OpenList[OpenListPosition - 1].ChessUnit_y = 0;
OpenList[OpenListPosition - 1].f_value = 0;
OpenListPosition--;//将OpenListPosition减1,表示从openlist中移出最后一点,即f值最小的点
CloseListPosition++;//将OpenListPosition加1,记录closelist中增加一个元素
CurrentUnit.ChessUnit_x = CloseList[CloseListPosition - 1].ChessUnit_x; //获得当前结点的x坐标
CurrentUnit.ChessUnit_y = CloseList[CloseListPosition - 1].ChessUnit_y; //获得当前结点的y坐标
if (dealCurrentNeibors(CurrentUnit)) break;//判断当前结点周围8个点状态,计算g、h、f值,将周围每个点的父指针指向当前结点
}//while
printPath();
cin >> PauseCin;
//FileReadMatrix();
return 0;
}