美文网首页
c语言实现A*寻路算法

c语言实现A*寻路算法

作者: 一路向后 | 来源:发表于2021-01-31 21:17 被阅读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库数据 数据全部清除

    相关文章

      网友评论

          本文标题：c语言实现A*寻路算法

          本文链接：https://www.haomeiwen.com/subject/orfdtltx.html

          延伸阅读

          深度阅读

          • 您也可以注册成为美文阅读网的作者，发表您的原创作品、分享您的心情！

          栏目导航

          热点阅读

          关于我们|服务条款|联系我们|c语言实现A*寻路算法|投稿指南|网站地图|RSS订阅|排版工具|手机版

          提供经典美文摘抄,优美散文欣赏,现代诗歌精选,短篇小说,心情随笔,表白情书范文,故事会在线阅读欣赏

          Copyright © 2014-2023 Haomeiwen.com All Rights Reserved. 好美文阅读网 版权所有

          备案信息：桂公网安备 45052102000051号 · 桂ICP备13007215号-3

          本站所收录作品、热点评论等信息部分来源互联网，目的只是为了系统归纳学习和传递资讯

          所有作品版权归原创作者所有，与本站立场无关，如不慎侵犯了你的权益，请联系我们告知，我们将做删除处理！