Astar routing algorithm implemented in C ++

http://www.iqiyi.com/w_19rsb3lwit.html#vfrm=16-1-1-1 this video talking about the good forty minutes to understand A * no problem. Here I do the next summary:

The list is divided into open and closed lists,

Proceed as follows:

1. Start searching start node is pressed into the opening in the queue, when the queue is not empty, remove a node from the open queue, find the start of a list as the current minimum cost node.

The end point is to determine whether, and if so, end,

2. The current point is pressed into the closed list, queue removed simultaneously open,

3. traversed nodes in the current node field 8, first determines whether the queue is in a closed wall or inside,

If not, the cost recalculated G and H, the list is determined whether the opening,

If the open before the queue, and the cost of less than, the cost value is updated, and the parent node points to the current node

If not, then a new node configuration, including a parent node, a cost value G, H, press-open list.

 

Let's look at the code,

First, define the data definition points:

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

Node definition:

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; };

    };

Figure definition:

    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 as follows:

#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 the Map :: :: clearCollisions () // clear the wall 
{ 
    walls.clear (); 
} 

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

int A_star the Map :: :: Manhattan (Point Source, target Point) // Manhattan distance 
{
     // Auto Delta = STD the Move :: (getDelta (Source, target)); // in a very simple way lvalue reference value is converted into the right references, do not understand why 
    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 ); 

            the Node * by successor = findNodeOnList (openSet, newCoordinates);
             IF (by successor == nullptr a) { // do not queue inside the opening 
                by successor = new new the Node (newCoordinates, Current); 
                by successor -> G = the totalCost; 
                by successor -> H = Manhattan (successor-> coordinates_, target); 
                openSet.push_back (by successor); 
            } 
            the else  IF (the totalCost <successor-> G) { // has been opened in a queue 
                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");
}