Astar寻路算法C++实现

http://www.iqiyi.com/w_19rsb3lwit.html#vfrm=16-1-1-1这个视频讲的很好，四十分钟理解A*没问题。这里我做下总结：

分为开列表和关列表，

步骤如下：

1.开始搜索，将起始节点压入开队列中，队列不为空时，从开队列中取出一个节点，寻找开始列表里代价最小的一个作为当前节点。

判断该点是否是终点，如果是的话，结束，

2.将当前点压入关闭列表，同时移出开队列，

3.遍历当前节点的8领域内的节点，首先判断是否是墙或在关闭队列里面，

如果不在，重新计算代价G和H，判断是否在开列表，

如果在开队列中，且此次代价小于之前的，则更新代价值，并把父节点指向当前节点

如果不在，则新构造一个节点，包括父节点，代价值G，H，压入开列表。

下面看代码，

首先是数据的定义，点的定义：

struct Point
    {
        int x, y;
        Point(int x_ = 0, int y_ = 0) { x = x_; y = y_; };
    };

节点的定义：

struct Node
    {
        int G, H;
        Point coordinates_;
        Node *parent_;
        vector<Point> direction = {
            {-1,0},{1,0},{0,-1},{0,1},
            {-1,-1},{-1,1},{1,-1},{1,1}
        };

        Node(Point coordinates, Node *parent = nullptr)
        {
            coordinates_.x = coordinates.x;
            coordinates_.y = coordinates.y;

            parent_ = parent;
            G = 0;
            H = 0;
        };
        int Get_score() { return G + H; };

    };

图的定义：

    class Map
    {
    public:
    vector<Node*> NodeSet;
    vector<Point> walls;
    Point size_;
    int directions;

    void Set_diag_move(bool enable) { directions = (enable ? 8 : 4); };

    void Set_size(Point size) { size_ = size; };
    void addCollision(Point coordinates);
    void removeCollision(Point coordinates);
    void clearCollisions();

    void releaseNodes(vector<Node*> nodes);
    Node* findNodeOnList(vector<Node*> nodes_, Point coordinates_);
    Point getDelta(Point source, Point target);

    int manhattan(Point source, Point target);

    bool Detect_collision(Point coordinates);

    vector<A_star::Point> findPath(Point source, Point target);

    };

A_star.c如下：

#include "A_star.h"
#include <iostream>
#include <algorithm>
using namespace std;
void A_star::Map::addCollision(Point coordinates)//添加墙
{
    walls.push_back(coordinates);
}

void A_star::Map::removeCollision(Point coordinates)//删除墙
{
    vector<Point>::iterator it = walls.begin();
    while(it != walls.end())
    {
        if (coordinates.x == (*it).x && coordinates.y == (*it).y)
        {
            walls.erase(it);
        }
        it++;
    }
}

void A_star::Map::clearCollisions()//清除墙
{
    walls.clear();
}

A_star::Point A_star::Map::getDelta(Point source, Point target)
{
    return{ abs(source.x - target.x),  abs(source.y - target.y) };
}

int A_star::Map::manhattan(Point source, Point target)//曼哈顿距离
{
    //auto delta = std::move(getDelta(source, target));//以非常简单的方式将左值引用转换为右值引用，不懂为什么这么做
    Point delta = getDelta(source, target);
    return static_cast<int>(10 * (delta.x + delta.y));
}

bool A_star::Map::Detect_collision(Point coordinates)//是否是墙
{
    if (coordinates.x >= 0 && coordinates.x <= size_.x &&
        coordinates.y >= 0 && coordinates.y <= size_.y)
    {
        vector<Point>::iterator it = walls.begin();
        while (it != walls.end())
        {
            if (coordinates.x == (*it).x && coordinates.y == (*it).y)
            {
                return true;
            }
            it++;
        }
        return false;
    }
    else
    return true;
}

void A_star::Map::releaseNodes(vector<Node*> nodes)//移除节点
{
    for (auto it = nodes.begin(); it != nodes.end();) {
        delete *it;
        it = nodes.erase(it);
    }
}

A_star::Node* A_star::Map::findNodeOnList(vector<Node*> nodes, Point coordinates)//判断是否 在列表中
{
    for (auto node : nodes) {
        //Point s = node->coordinates_;
        if (node->coordinates_.x == coordinates.x && node->coordinates_.y == coordinates.y) {
            return node;
        }
    }
    return nullptr;
}

vector<A_star::Point> A_star::Map::findPath(Point source, Point target)//寻路
{
    vector<Point> path;
    if(Detect_collision(source) || Detect_collision(target))//判断起点和终点是否是墙
    {
        cout << "please input orgin and target again" << endl;
        return path;
    }
    Node *current = nullptr;
    vector<A_star::Node*> openSet, closedSet;
    openSet.push_back(new Node(source));

    while (!openSet.empty()) {
        current = *openSet.begin();
        for (auto node : openSet) {
            if (node->Get_score() <= current->Get_score()) {
                current = node;
            }
        }

        if (current->coordinates_.x == target.x && current->coordinates_.y == target.y) {
            break;
        }

        closedSet.push_back(current);
        openSet.erase(std::find(openSet.begin(), openSet.end(), current));

        for (int i = 0; i < directions; ++i) {
            Point newCoordinates((current->coordinates_.x + current->direction[i].x),(current->coordinates_.y + current->direction[i].y));
            if (Detect_collision(newCoordinates) ||
                findNodeOnList(closedSet, newCoordinates)) {//判断墙和是否在关闭队列里面
                continue;
            }

            int totalCost = current->G + ((i < 4) ? 10 : 14);

            Node *successor = findNodeOnList(openSet, newCoordinates);
            if (successor == nullptr) {//不在开队列里面
                successor = new Node(newCoordinates, current);
                successor->G = totalCost;
                successor->H = manhattan(successor->coordinates_, target);
                openSet.push_back(successor);
            }
            else if (totalCost < successor->G) {//已经在开队列里了
                successor->parent_ = current;
                successor->G = totalCost;
            }
        }
    }
    
    while (current != nullptr) {
        path.push_back(current->coordinates_);
        current = current->parent_;
    }

    releaseNodes(openSet);
    releaseNodes(closedSet);

    return path;
}

main.cpp

#include "A_star.h"
#include <iostream>
using namespace A_star;

#define Max_X 10
#define Max_Y 10

void printMap(char map[Max_X][Max_Y], int width, int height)
{
    for (int i = 0; i<width; i++)
    {
        for (int j = 0; j<height; j++)
        {
            printf("%c\t", map[i][j]);
        }
        printf("\n");
    }
}

int main()
{
    char mapdata[Max_X][Max_Y] =
    {
        { '1','0','0','1','0','1','1','1','1','1' },
        { '1','1','1','1','0','1','1','1','1','1' },
        { '0','0','0','1','0','1','1','1','1','1' },
        { '1','0','0','1','0','1','1','1','1','0' },
        { '1','1','1','1','0','1','1','1','1','1' },
        { '1','1','0','0','1','1','1','1','1','1' },
        { '1','1','1','1','1','1','1','1','1','1' },
        { '1','0','0','1','1','1','1','1','1','1' },
        { '1','1','0','0','1','1','1','1','1','1' },
        { '1','0','1','1','1','1','1','1','1','1' },
    };

    printMap(mapdata, Max_X, Max_Y);
    Point size(Max_X, Max_Y);
    Map Map_;
    Map_.Set_size(size);
    for (int i = 0; i<Max_X; i++)
    {

        for (int j = 0; j<Max_Y; j++)
        {
            Point point(i, j); 
            if (mapdata[i][j] == '0')
            {
                Map_.addCollision(point);
            }
        }
    }
    Map_.Set_diag_move(true);
    vector<Point> path;
    path = Map_.findPath({ 0, 0 }, { 4, 1});
    for (auto& coordinate : path) {
        std::cout << coordinate.x << " " << coordinate.y << "\n";
    }
    system("pause");
}