#include <stdio.h>
#include <stdlib.h>
#define ROWSIZE 10 //地图行数
#define COLSIZE 10 //地图列数
#define STARTNODE 1 //代表起点
#define ENDNODE 2 //代表终点
#define BARRIER 3 //障碍物
#define SIDELENGTH 10 //方块边长
typedef struct
{
int row; //行下标
int col; //列下标
}PosType;
typedef struct AStarNode
{
PosType seat; //下标
int s_g; // 起点到此点的距离( 由g和h可以得到f,此处f省略,f=g+h )
int s_h; // 启发函数预测的此点到终点的距离
int s_f; //权值
int s_style;// 结点类型:起始点,终点,障碍物
struct AStarNode * s_parent; // 父节点
bool s_is_in_closelist; // 是否在close表中
bool s_is_in_openlist; // 是否在open表中
}AStarNode, *pAStarNode;
AStarNode map_maze[ROWSIZE][COLSIZE]; // 结点数组
typedef struct openList // openlist结构体声明
{
pAStarNode table[ROWSIZE*COLSIZE];
int node_count; //节点数量
}OpenList;
OpenList open_list; //全局变量
typedef struct closeList // closelist结构体声明
{
pAStarNode table[ROWSIZE*COLSIZE];
int node_count; //节点数量
}CloseList;
CloseList close_list; //全局变量
typedef struct pathStack
{
pAStarNode arrypath[100]; // 保存路径的栈
int top = -1; // 栈顶
}PathStack;
PathStack path_stack;
// 交换openlist中的两个元素,参数是下标
void swap( int idx1, int idx2 )
{
pAStarNode tmp = open_list.table[idx1];
open_list.table[idx1] =open_list.table[idx2];
open_list.table[idx2] = tmp;
}
// 堆调整
//将openlist中的结点按升序排好
void adjust_heap( int nIndex ) //开始调整的下标
{
int curr = nIndex;
int child = curr * 2 + 1; // 得到左孩子idx( 下标从0开始,所有左孩子是curr*2+1 )
int parent = ( curr - 1 ) / 2; // 得到双亲idx
if (nIndex < 0 || nIndex >= open_list.node_count)
{
return;
}
// 往下调整( 要比较左右孩子和curr parent )
//
while ( child < open_list.node_count)//孩子要全部遍历完
{
// 小根堆是双亲值小于孩子值
// 比较F值的大小,F=G+H
if ( child + 1 < open_list.node_count && open_list.table[child]->s_f > open_list.table[child+1]->s_f )
{
++child; // 判断左右孩子大小
}
if (open_list.table[curr]->s_f <= open_list.table[child]->s_f) //已经满足小根堆的条件
{
break;
}
else
{
swap( child, curr ); // 交换节点
curr = child; // 再判断当前孩子节点
child = curr * 2 + 1; // 再判断左孩子
}
}
if (curr != nIndex)
{
return;
}
// 往上调整( 只需要比较curr child和parent )
//
while (curr != 0)
{
if (open_list.table[curr]->s_f >= open_list.table[parent]->s_f)
{
break;
}
else
{
swap( curr, parent );
curr = parent;
parent = (curr-1)/2;
}
}
}
// 判断邻居点是否可以进入open list
// (x,y)是当前结点的邻居的下标
void insert_to_opentable( int x, int y, pAStarNode curr_node, pAStarNode end_node, int w ) //w是相邻方块的距离
{
int i;
if ( map_maze[x][y].s_style != BARRIER ) // 不是障碍物
{
if ( !map_maze[x][y].s_is_in_closelist ) // 不在close list中
{
if ( map_maze[x][y].s_is_in_openlist ) // 在open list中
{
// 需要判断是否是一条更优化的路径
//
if ( map_maze[x][y].s_g > curr_node->s_g + w ) // 如果更优化
{
map_maze[x][y].s_g = curr_node->s_g + w; //更新G值
map_maze[x][y].s_f = map_maze[x][y].s_g + map_maze[x][y].s_h; //更新F值
map_maze[x][y].s_parent = curr_node; //将相邻方格的父亲设置为当前方格
for ( i = 0; i < open_list.node_count; ++i )
{
if ( open_list.table[i]->seat.col == map_maze[x][y].seat.col && open_list.table[i]->seat.row == map_maze[x][y].seat.row )
{
break;
}
} //找到开始进行最小堆调整的下标
adjust_heap( i ); // 向下调整的开始下标
}
}
else // 不在open中
{
map_maze[x][y].s_g = curr_node->s_g + w;
map_maze[x][y].s_h = abs(end_node->seat.row- x ) + abs(end_node->seat.col - y); //计算曼哈顿距离
map_maze[x][y].s_f = map_maze[x][y].s_g + map_maze[x][y].s_h; //F=G+H
map_maze[x][y].s_parent = curr_node;
open_list.table[open_list.node_count++] = &(map_maze[x][y]);
map_maze[x][y].s_is_in_openlist = true;
}
}
}
}
// 查找邻居
// 对周边8个邻居进行查找
//
void get_neighbors( pAStarNode curr_node, pAStarNode end_node )
{
int x = curr_node->seat.row;
int y = curr_node->seat.col;
// 下面对于8个邻居进行处理!
//
if ( ( x + 1 ) >= 0 && ( x + 1 ) < ROWSIZE && y >= 0 && y < COLSIZE ) //东
{
insert_to_opentable( x+1, y, curr_node, end_node, SIDELENGTH );
}
if ( ( x - 1 ) >= 0 && ( x - 1 ) < ROWSIZE && y >= 0 && y < COLSIZE ) //西
{
insert_to_opentable( x-1, y, curr_node, end_node, SIDELENGTH );
}
if ( x >= 0 && x < ROWSIZE && ( y + 1 ) >= 0 && ( y + 1 ) < COLSIZE ) //南
{
insert_to_opentable( x, y+1, curr_node, end_node, SIDELENGTH );
}
if ( x >= 0 && x < ROWSIZE && ( y - 1 ) >= 0 && ( y - 1 ) < COLSIZE ) //北
{
insert_to_opentable( x, y-1, curr_node, end_node, SIDELENGTH );
}
if ( ( x + 1 ) >= 0 && ( x + 1 ) < ROWSIZE && ( y + 1 ) >= 0 && ( y + 1 ) < COLSIZE ) //东南
{
insert_to_opentable( x+1, y+1, curr_node, end_node, 1.4*SIDELENGTH );
}
if ( ( x + 1 ) >= 0 && ( x + 1 ) < ROWSIZE && ( y - 1 ) >= 0 && ( y - 1 ) < COLSIZE ) //东北
{
insert_to_opentable( x+1, y-1, curr_node, end_node, 1.4*SIDELENGTH );
}
if ( ( x - 1 ) >= 0 && ( x - 1 ) < ROWSIZE && ( y + 1 ) >= 0 && ( y + 1 ) < COLSIZE ) //西南
{
insert_to_opentable( x-1, y+1, curr_node, end_node, 1.4*SIDELENGTH );
}
if ( ( x - 1 ) >= 0 && ( x - 1 ) < ROWSIZE && ( y - 1 ) >= 0 && ( y - 1 ) < COLSIZE ) //西北
{
insert_to_opentable( x-1, y-1, curr_node, end_node, 1.4*SIDELENGTH );
}
}
int main()
{
// 地图数组的定义
//
AStarNode *start_node; // 起始点
AStarNode *end_node; // 结束点
AStarNode *curr_node; // 当前点
bool is_found=false; // 是否找到路径
int maze[][10] ={
{ 1,0,0,3,0,3,0,0,0,0 },
{ 0,0,3,0,0,3,0,3,0,3 },
{ 3,0,0,0,0,3,3,3,0,3 },
{ 3,0,3,0,0,0,0,0,0,3 },
{ 3,0,0,0,0,3,0,0,0,3 },
{ 3,0,0,3,0,0,0,3,0,3 },
{ 3,0,0,0,0,3,3,0,0,0 },
{ 0,0,2,0,0,0,0,0,0,0 },
{ 3,3,3,0,0,3,0,3,0,3 },
{ 3,0,0,0,0,3,3,3,0,3 },
};
int i,j,x;
// 下面准备点
//
for( i = 0; i < ROWSIZE; ++i )
{
for ( j = 0; j < COLSIZE; ++j )
{
map_maze[i][j].s_g = 0;
map_maze[i][j].s_h = 0;
map_maze[i][j].s_f = 0;
map_maze[i][j].s_is_in_openlist = false;
map_maze[i][j].s_is_in_closelist = false;
map_maze[i][j].s_style = maze[i][j];
map_maze[i][j].seat.row = i;
map_maze[i][j].seat.col = j;
map_maze[i][j].s_parent = NULL;
if ( map_maze[i][j].s_style == STARTNODE ) // 起点
{
start_node = &(map_maze[i][j]);
}
else if( map_maze[i][j].s_style == ENDNODE ) // 终点
{
end_node = &(map_maze[i][j]);
}
printf("%d ", maze[i][j]);
}
printf("\n");
}
// 下面使用A*算法得到路径
//
open_list.table[open_list.node_count++] = start_node; // 起始点加入open表
start_node->s_is_in_openlist = true; // 加入open表
start_node->s_g = 0;
start_node->s_h = abs(end_node->seat.row - start_node->seat.row) + abs(end_node->seat.col - start_node->seat.col);
start_node->s_f = start_node->s_g + start_node->s_h; //F=G+H
start_node->s_parent = NULL;
if ( start_node->seat.row == end_node->seat.row && start_node->seat.col == end_node->seat.col )
{
printf("起点==终点!\n");
return 0;
}
while( 1 )
{
curr_node = open_list.table[0]; // open表的第一个点一定是f值最小的点(通过堆排序得到的)
open_list.table[0] = open_list.table[--open_list.node_count]; // 最后一个点放到第一个点,然后进行堆调整
adjust_heap( 0 ); // 调整堆
close_list.table[close_list.node_count++] = curr_node; // 将F值最小的点加入到closelist中
curr_node->s_is_in_closelist = true; // 放在在close表中
if ( curr_node->seat.row == end_node->seat.row && curr_node->seat.col == end_node->seat.col )// 终点在close中,结束
{
is_found = true;
break;
}
get_neighbors( curr_node, end_node ); // 对邻居的处理,满足条件的放入openlist
if ( open_list.node_count == 0 ) // 没有路径到达
{
is_found = false;
break;
}
}
if ( is_found )
{
curr_node = end_node;
while( curr_node!=NULL )
{
path_stack.top += 1;
path_stack.arrypath[path_stack.top] = curr_node;
curr_node = curr_node->s_parent;
}
while( path_stack.top >= 0 ) // 输出路径
{
if ( path_stack.top > 0 )
{
printf("(%d,%d)-->", path_stack.arrypath[path_stack.top]->seat.row, path_stack.arrypath[path_stack.top]->seat.col);
}
else
{
printf("(%d,%d)", path_stack.arrypath[path_stack.top]->seat.row, path_stack.arrypath[path_stack.top]->seat.col);
}
path_stack.top -= 1;
}
}
else
{
printf("未找到路径");
}
puts("");
return 0;
}
本程序在VS2017下运行通过