c语言实现A*寻路算法
2021-01-31 本文已影响0人
一路向后
1.算法简介
A算法与最好优先贪婪算法一样都通过计算一个值来判断探索的方向。对于节点N，计算公式如下：F(N)=G(N)+H(N) 其中G(N)就是Dijkstra算法中计算的，从起点到当前节点N的移动消耗，而H(N)，在只允许上下左右移动的前提下，就是最好优先贪婪算法中当前节点N到目标节点E的曼哈顿距离。因此，当节点间移动消耗非常小时，G对F的影响也会微乎其微，A算法就退化为最好优先贪婪算法；当节点间移动消耗非常大以至于H对F的影响微乎其微时，A*算法就退化为Dijkstra算法。
2.源码实现
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
int map[20][20] =
{
{ 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0 },
{ 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 0 },
{ 1, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 0, 1, 0, 0, 0, 0 },
{ 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0 },
{ 0, 0, 0, 1, 0, 1, 1, 1, 1, 1, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0 },
{ 0, 0, 0, 1, 0, 1, 0, 0, 0, 1, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0 },
{ 0, 0, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0 },
{ 0, 0, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0 },
{ 0, 0, 0, 1, 0, 1, 0, 1, 1, 1, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0 },
{ 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0 },
{ 0, 0, 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 0, 1, 0, 1, 0, 0, 0, 0 },
{ 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0 },
{ 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 0, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }
};
//节点结构体
typedef struct Node
{
int f, g, h;
int row; //该节点所在行
int col; //该节点所在列
int direction; //parent节点要移动的方向就能到达本节点
struct Node *parent;
} Node, *PNode;
//OPEN CLOSED 表结构体
typedef struct Stack
{
PNode npoint;
struct Stack *next;
} Stack, *PStack;
int directionIndex = 0; //方向
int direction[256]; //方向数组
int rows = 20; //地图行数
int cols = 20; //地图列数
int G_OFFSET = 1; //每个图块G值的增加值
int destinationRow; //目标所在行
int destinationCol; //目标所在列
int canMoveIndex = 0; //可以通行的地图图块索引
int tileSize = 1; //图块大小
PStack Open = NULL;
PStack Closed = NULL;
//选取OPEN表上f值最小的节点，返回该节点地址
Node *getNodeFromOpen()
{
PStack temp = Open->next, min = Open->next, minp = Open;
PNode minx;
if(temp == NULL)
return NULL;
while(temp->next != NULL)
{
if((temp->next->npoint->f) < (min->npoint->f))
{
min = temp->next;
minp = temp;
}
temp = temp->next;
}
minx = min->npoint;
temp = minp->next;
minp->next = minp->next->next;
free(temp);
return minx;
}
//判断节点是否相等，相等，不相等
short Equal(PNode a, PNode b)
{
if((a->row == b->row) && (a->col == b->col))
{
return 1;
}
else
{
return 0;
}
}
//判断节点是否属于OPEN表，是则返回节点地址，否则返回空地址
PNode BelongInOpen(PNode u)
{
PStack temp = Open->next;
if(temp == NULL)
return NULL;
while(temp != NULL)
{
if(Equal(u, temp->npoint))
{
return temp->npoint;
}
else
{
temp = temp->next;
}
}
return NULL;
}
//判断节点是否属于CLOSED表，是则返回节点地址，否则返回空地址
PNode BelongInClosed(PNode u)
{
PStack temp = Closed->next;
if(temp == NULL)
return NULL;
while(temp != NULL)
{
if(Equal(u, temp->npoint))
{
return temp->npoint;
}
else
{
temp = temp->next;
}
}
return NULL;
}
//把节点放入OPEN表中
void PutintoOpen(Node *u)
{
Stack *temp;
temp =(Stack *)malloc(sizeof(Stack));
temp->npoint = u;
temp->next = Open->next;
Open->next = temp;
}
//把节点放入CLOSED表中
void PutintoClosed(Node *u)
{
Stack *temp;
temp =(Stack *)malloc(sizeof(Stack));
temp->npoint = u;
temp->next = Closed->next;
Closed->next = temp;
}
//得到该图块的H值
int getH(int row, int col)
{
return (abs(destinationRow - row) + abs(destinationCol - col));
}
//得到该位置所在地图行
int getRowPosition(int y)
{
return (y / tileSize);
}
//得到该位置所在地图列
int getColPosition(int x)
{
return (x / tileSize);
}
//检测该图块是否可通行
short isCanMove(int col, int row)
{
if(col < 0 || col >= cols)
return 0;
if(row < 0 || row >= rows)
return 0;
return map[col][row] == canMoveIndex;
}
/*检查坐标是否在open列表里*/
PNode checkOpen(int row, int col)
{
PStack temp = Open->next;
if(temp == NULL)
return NULL;
while(temp != NULL)
{
if((temp->npoint->row == row) && (temp->npoint->col == col))
{
return temp->npoint;
}
else
{
temp = temp->next;
}
}
return NULL;
}
/*检查坐标是否在closed列表里*/
short isInClose(int row, int col)
{
PStack temp = Closed->next;
if(temp == NULL)
return 0;
while(temp != NULL)
{
if((temp->npoint->row == row) && (temp->npoint->col == col))
{
return 1;
}
else
{
temp = temp->next;
}
}
return 0;
}
/*一步处理逻辑*/
void creatSeccessionNode(PNode bestNode, int row, int col)
{
int g = bestNode->g + G_OFFSET;
if(!isInClose(row, col))
{
PNode oldNode = NULL;
//如果在open列表里则更新, 如果不在则添加至open列表
if((oldNode=checkOpen(row, col)) != NULL)
{
if(oldNode->g < g)
{
oldNode->parent = bestNode;
oldNode->g = g;
oldNode->f = g + oldNode->h;
}
}
else
{
PNode node = (PNode)malloc(sizeof(Node));
node->parent = bestNode;
node->g = g;
node->h = getH(row, col);
node->f = node->g + node->h;
node->row = row;
node->col = col;
directionIndex++;
node->direction = directionIndex;
PutintoOpen(node);
}
}
}
/**
* 根据传入的节点生成子节点
* @param bestNode
* @param destinationRow
* @param destinationCol
*/
void seachSeccessionNode(PNode bestNode)
{
int row, col;
//上部节点
if(isCanMove(row = bestNode->row - 1, col = bestNode->col))
{
creatSeccessionNode(bestNode, row, col);
}
//下部节点
if(isCanMove(row = bestNode->row + 1, col = bestNode->col))
{
creatSeccessionNode(bestNode, row, col);
}
//左部节点
if(isCanMove(row = bestNode->row, col = bestNode->col - 1))
{
creatSeccessionNode(bestNode, row, col);
}
//右部节点
if(isCanMove(row = bestNode->row, col = bestNode->col + 1))
{
creatSeccessionNode(bestNode, row, col);
}
//传入节点放入close列表
PutintoClosed(bestNode);
}
//遍历open链表
void printfOpenData()
{
PStack temp = Open->next;
PNode p_node;
if(temp == NULL)
return;
while(temp != NULL)
{
PStack head = temp;
temp = temp->next;
p_node = head->npoint;
printf("Open库数据! [%d,%d]\n", p_node->col, p_node->row);
free(p_node);
free(head);
Open->next = temp;
}
printf("\nOpen库数据 数据全部清除\n");
return;
}
//遍历close链表
void printfClosedData()
{
PStack temp = Closed->next;
PNode p_node;
if(temp == NULL)
return;
while(temp != NULL)
{
PStack head = temp;
temp = temp->next;
p_node = head->npoint;
printf("Closed库数据! [%d,%d]\n", p_node->col, p_node->row);
free(p_node);
free(head);
Closed->next = temp;
}
printf("\nClosed库数据 数据全部清除\n");
return;
}
void getPath(int startX, int StartY, int destinationX, int destinationY)
{
PNode startNode = (PNode)malloc(sizeof(Node));
PNode bestNode = NULL;
int index = 0;
destinationRow = getRowPosition(destinationY);
destinationCol = getColPosition(destinationX);
startNode->parent= NULL;
startNode->row = getRowPosition(StartY);
startNode->col = getColPosition(startX);
startNode->g = 0;
startNode->h = getH(startNode->row, startNode->col);
startNode->f = startNode->g + startNode->h;
startNode->direction = 0;
PutintoOpen(startNode);
while(1)
{
//从OPEN表中取出f值最小的节点
bestNode = getNodeFromOpen();
if(bestNode == NULL)
{
printf("未找到路径\n");
return;
}
else if(bestNode->row == destinationRow && bestNode->col == destinationCol)
{
//寻路结束，打印相关信息
PNode _Node = bestNode;
int nodeSum = 0;
int nodeIndex =0;
printf("程序运行次数=%d\n", index);
while(_Node->parent != NULL)
{
printf("x:%d y:%d direction = %d \n", _Node->col, _Node->row, _Node->direction);
_Node = _Node->parent;
nodeSum += 1;
}
printf("节点数量=%d\n", nodeSum);
_Node = bestNode;
nodeIndex = nodeSum-1;
while(_Node->parent != NULL && nodeIndex>=0)
{
PNode _NodeParent = _Node->parent;
printf("x:%d y:%d direction = %d \n", _Node->col, _Node->row, _Node->direction);
if(_NodeParent->col - _Node->col == 0 && _NodeParent->row - _Node->row == +1)
{
direction[nodeIndex] = 1;
}
else if(_NodeParent->col - _Node->col == 0 && _NodeParent->row - _Node->row == -1)
{
direction[nodeIndex] = 2;
}
else if(_NodeParent->col - _Node->col == +1 && _NodeParent->row - _Node->row == 0)
{
direction[nodeIndex] = 3;
}
else if(_NodeParent->col - _Node->col == -1 && _NodeParent->row - _Node->row == 0)
{
direction[nodeIndex] = 4;
}
else
{
direction[nodeIndex] = 0;
}
nodeIndex -= 1;
_Node = _Node->parent;
}
for(nodeIndex=0; nodeIndex<nodeSum; nodeIndex++)
{
printf("direction[%d]=%d\n",nodeIndex,direction[nodeIndex]);
}
return;
}
index++;
seachSeccessionNode(bestNode);
}
}
//主函数
void main()
{
//初始操作，建立open和closed表
Open = (PStack)malloc(sizeof(Stack));
Open->next = NULL;
Closed = (PStack)malloc(sizeof(Stack));
Closed->next = NULL;
getPath(0, 0, 19, 19);
printfOpenData();
printfClosedData();
free(Open);
free(Closed);
}
3.编译源码
$ gcc -o example example.c
4.运行及结果
$ ./example
程序运行次数=53
x:19 y:19 direction = 84
x:18 y:19 direction = 82
x:17 y:19 direction = 81
x:16 y:19 direction = 80
x:15 y:19 direction = 79
x:14 y:19 direction = 76
x:14 y:18 direction = 75
x:14 y:17 direction = 69
x:14 y:16 direction = 68
x:13 y:16 direction = 66
x:12 y:16 direction = 64
x:11 y:16 direction = 62
x:10 y:16 direction = 60
x:9 y:16 direction = 58
x:8 y:16 direction = 56
x:7 y:16 direction = 54
x:6 y:16 direction = 52
x:5 y:16 direction = 50
x:4 y:16 direction = 48
x:3 y:16 direction = 46
x:2 y:16 direction = 43
x:2 y:15 direction = 28
x:2 y:14 direction = 26
x:2 y:13 direction = 24
x:2 y:12 direction = 22
x:2 y:11 direction = 20
x:2 y:10 direction = 18
x:2 y:9 direction = 16
x:2 y:8 direction = 14
x:2 y:7 direction = 12
x:2 y:6 direction = 10
x:2 y:5 direction = 8
x:2 y:4 direction = 7
x:2 y:3 direction = 6
x:2 y:2 direction = 5
x:2 y:1 direction = 4
x:1 y:1 direction = 3
x:1 y:0 direction = 2
节点数量=38
x:19 y:19 direction = 84
x:18 y:19 direction = 82
x:17 y:19 direction = 81
x:16 y:19 direction = 80
x:15 y:19 direction = 79
x:14 y:19 direction = 76
x:14 y:18 direction = 75
x:14 y:17 direction = 69
x:14 y:16 direction = 68
x:13 y:16 direction = 66
x:12 y:16 direction = 64
x:11 y:16 direction = 62
x:10 y:16 direction = 60
x:9 y:16 direction = 58
x:8 y:16 direction = 56
x:7 y:16 direction = 54
x:6 y:16 direction = 52
x:5 y:16 direction = 50
x:4 y:16 direction = 48
x:3 y:16 direction = 46
x:2 y:16 direction = 43
x:2 y:15 direction = 28
x:2 y:14 direction = 26
x:2 y:13 direction = 24
x:2 y:12 direction = 22
x:2 y:11 direction = 20
x:2 y:10 direction = 18
x:2 y:9 direction = 16
x:2 y:8 direction = 14
x:2 y:7 direction = 12
x:2 y:6 direction = 10
x:2 y:5 direction = 8
x:2 y:4 direction = 7
x:2 y:3 direction = 6
x:2 y:2 direction = 5
x:2 y:1 direction = 4
x:1 y:1 direction = 3
x:1 y:0 direction = 2
direction[0]=4
direction[1]=2
direction[2]=4
direction[3]=2
direction[4]=2
direction[5]=2
direction[6]=2
direction[7]=2
direction[8]=2
direction[9]=2
direction[10]=2
direction[11]=2
direction[12]=2
direction[13]=2
direction[14]=2
direction[15]=2
direction[16]=2
direction[17]=2
direction[18]=4
direction[19]=4
direction[20]=4
direction[21]=4
direction[22]=4
direction[23]=4
direction[24]=4
direction[25]=4
direction[26]=4
direction[27]=4
direction[28]=4
direction[29]=4
direction[30]=2
direction[31]=2
direction[32]=2
direction[33]=4
direction[34]=4
direction[35]=4
direction[36]=4
direction[37]=4
Open库数据! [18,18]
Open库数据! [13,19]
Open库数据! [13,18]
Open库数据! [16,15]
Open库数据! [15,17]
Open库数据! [13,17]
Open库数据! [12,17]
Open库数据! [11,17]
Open库数据! [10,17]
Open库数据! [9,17]
Open库数据! [8,17]
Open库数据! [7,17]
Open库数据! [6,17]
Open库数据! [5,17]
Open库数据! [4,17]
Open库数据! [3,17]
Open库数据! [1,16]
Open库数据! [2,17]
Open库数据! [14,13]
Open库数据! [1,14]
Open库数据! [1,13]
Open库数据! [1,12]
Open库数据! [1,11]
Open库数据! [1,10]
Open库数据! [1,9]
Open库数据! [1,8]
Open库数据! [1,7]
Open库数据! [1,6]
Open库数据! [1,5]
Open库数据! [1,4]
Open库数据! [0,1]
Open库数据 数据全部清除
Closed库数据! [18,19]
Closed库数据! [17,19]
Closed库数据! [16,19]
Closed库数据! [15,19]
Closed库数据! [14,19]
Closed库数据! [14,18]
Closed库数据! [14,17]
Closed库数据! [16,17]
Closed库数据! [16,16]
Closed库数据! [15,16]
Closed库数据! [14,16]
Closed库数据! [13,16]
Closed库数据! [12,16]
Closed库数据! [11,16]
Closed库数据! [10,16]
Closed库数据! [9,16]
Closed库数据! [8,16]
Closed库数据! [7,16]
Closed库数据! [6,16]
Closed库数据! [5,16]
Closed库数据! [4,16]
Closed库数据! [3,16]
Closed库数据! [2,16]
Closed库数据! [2,15]
Closed库数据! [14,14]
Closed库数据! [13,14]
Closed库数据! [12,14]
Closed库数据! [11,14]
Closed库数据! [10,14]
Closed库数据! [9,14]
Closed库数据! [8,14]
Closed库数据! [7,14]
Closed库数据! [6,14]
Closed库数据! [5,14]
Closed库数据! [4,14]
Closed库数据! [3,14]
Closed库数据! [2,14]
Closed库数据! [2,13]
Closed库数据! [2,12]
Closed库数据! [2,11]
Closed库数据! [2,10]
Closed库数据! [2,9]
Closed库数据! [2,8]
Closed库数据! [2,7]
Closed库数据! [2,6]
Closed库数据! [2,5]
Closed库数据! [2,4]
Closed库数据! [2,3]
Closed库数据! [2,2]
Closed库数据! [2,1]
Closed库数据! [1,1]
Closed库数据! [1,0]
Closed库数据! [0,0]
Closed库数据 数据全部清除
