A*算法------利用堆排序

详细讲解

#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下运行通过