A星寻路算法简易版(代码实现)

绿色为起点，红色为终点，蓝色为墙不可走。启动只能上下左右不可以斜着走。

以下为A*寻路算法代码实现，核心思想就是只走F值(格子评估值)最小的格子，直至到达终点。

F值最小代表着从起点出发到达终点需要的最少的步数，只走最少的步数，那么路径就是最短的。

#include <iostream>
#include <vector>
#include <string>
#include <cassert>
#include <memory>
#include<stdio.h>
#include<string.h>
#include <cassert>
#include <cmath>
using namespace std;

typedef struct _node
{
    int x;//地图中的第几行
    int y;//地图中的第几列
    int F;//评估值F=G+H，G代表从起点到当前格子的成本，H代表从当前格无视障碍到目标的成本
    //尽量选择步数少的也就是F值小的
    int G;//G代表从起点到当前格子的成本，也就是走了多少步
    int H;//H代表从当前格无视障碍到目标的成本，也就是至少还需要多少步
    struct _node* preNode;//父节点
}node;



vector<node*> openList;//储存可到达或可走的格子
vector<node*> closeList;//储存已经到达或者已经走过的格子
int map[5][7]{
    //5行7列
    // 0  1  2  3  4  5  6
    0, 0, 0, 0, 0, 0, 0,
    0, 0, 0, 0, 1, 0, 0,
    0, 0, 0, 0, 1, 0, 0,
    0, 0, 0, 0, 1, 0, 0,
    0, 0, 0, 0, 0, 0, 0,
};
//node map[1][2]{{{0,0,0,0,0}},{{0,0,0,0,0}}};

//找出OpenList中F值最小的方格
int findMinNode()
{
    int index = 0;
    int size = openList.size();
    for(int i=1;i<size;++i)
    {
        if(openList[i]->F < openList[index]->F)
        {
            index = i;
        }
    }
    return index;
}

void findNeighbors(node* current,node* end)
{
    int openSize = openList.size();
    int closeSize = closeList.size();
    bool isInclude = false;
    //上
    if(current->x-1>=0 &&map[current->x-1][current->y]==0 )
    {
        for(int i =0;i<openSize;++i)
        {
            if(openList[i]->x == current->x-1 && openList[i]->y == current->y)
            {
                isInclude = true;
                break;
            }
        }
        for(int i =0;i<closeSize;++i)
        {
            if(closeList[i]->x == current->x-1 && closeList[i]->y == current->y)
            {
                isInclude = true;
                break;
            }
        }
        if(isInclude == false)
        {
            node *up = (node*)new node;
            up->x = current->x-1;
            up->y = current->y;
            cout<<"up  "<<up->x<<"  "<<up->y<<endl;
            up->G = current->G+1;
            up->H = abs(end->x-up->x)+abs(end->y-up->y);
            up->preNode = current;
            openList.push_back(up);
        }
    }
    //下
    isInclude = false;
    if(current->x+1<5 &&map[current->x+1][current->y]==0 )
    {
        for(int i =0;i<openSize;++i)
        {
            if(openList[i]->x == current->x+1 && openList[i]->y == current->y)
            {
                isInclude = true;
                break;
            }
        }
        for(int i =0;i<closeSize;++i)
        {
            if(closeList[i]->x == current->x+1 && closeList[i]->y == current->y)
            {
                isInclude = true;
                break;
            }
        }
        if(isInclude == false)
        {
            node* down=(node*)new node;
            down->x = current->x+1;
            down->y = current->y;
            cout<<"down  "<<down->x<<"  "<<down->y<<endl;
            down->G = current->G+1;
            down->H = abs(end->x-down->x)+abs(end->y-down->y);
            down->preNode = current;
            openList.push_back(down);
        }
    }
    //左
    isInclude = false;
    if(current->y-1>=0 &&map[current->x][current->y-1]==0)
    {
        for(int i =0;i<openSize;++i)
        {
            if(openList[i]->x == current->x && openList[i]->y == current->y-1)
            {
                isInclude = true;
                break;
            }
        }
        for(int i =0;i<closeSize;++i)
        {
            if(closeList[i]->x == current->x && closeList[i]->y == current->y-1)
            {
                isInclude = true;
                break;
            }
        }
        if(isInclude == false)
        {
            node* left=(node*)new node;
            left->x = current->x;
            left->y = current->y-1;
            cout<<"left  "<<left->x<<"  "<<left->y<<endl;
            left->G = current->G+1;
            left->H = abs(end->x-left->x)+abs(end->y-left->y);
            left->preNode = current;
            openList.push_back(left);
        }
    }
    //右
    isInclude = false;
    if(current->y+1<7 &&map[current->x][current->y+1]==0)
    {
        for(int i =0;i<openSize;++i)
        {
            if(openList[i]->x == current->x && openList[i]->y == current->y+1)
            {
                isInclude = true;
                break;
            }
        }
        for(int i =0;i<closeSize;++i)
        {
            if(closeList[i]->x == current->x && closeList[i]->y == current->y+1)
            {
                isInclude = true;
                break;
            }
        }
        if(isInclude == false)
        {
            node* right=(node*)new node;
            right->x = current->x;

            right->y = current->y+1;
            cout<<"right  "<<right->x<<"  "<<right->y<<endl;
            right->G = current->G+1;
            right->H = abs(end->x-right->x)+abs(end->y-right->y);
            right->preNode = current;
            openList.push_back(right);
        }
    }
}

void AstarSerch (node* start,node* end)
{
    openList.push_back(start);
    while(openList.size()>0)
    {
        int currentIndex = findMinNode();//找出OpenList中F值最小的方格，并把该格移出OpenList，放入CloseList。代表这个格子已到达并检查过了。
        node* current = openList[currentIndex];
        openList.erase(openList.begin()+currentIndex);
        closeList.push_back(current);
        findNeighbors(current,end);//找出当前格上下左右所有可到达的格子，看它们是否在OpenList当中。计算出相应的G、H、F值，并把当前格子作为它们的“父亲节点”。
        for(int i =0;i<openList.size();++i)
        {
            if(openList[i]->x == end->x && openList[i]->y == end->y)
            {
                cout<<"find it"<<endl;
                node* cur = openList[i];
                while(cur->preNode)
                {
                    cout<<""<<cur->x<<"   "<<cur->y<<endl;
                    cur=(cur->preNode);
                }
                return;
            }
        }
    }
}

int main()
{
    node* start=(node*)new node;
    start->x=2;
    start->y=1;
    start->F=0;
    start->G=0;
    start->H=0;
    start->preNode = nullptr;

    node* end=(node*)new node;
    end->x=2;
    end->y=5;
    end->F=0;
    end->G=0;
    end->H=0;

    AstarSerch(start,end);

    return 0;
}