[Turn] A * pathfinding algorithm C + + simple implementation

Reference article:

Http://www.policyalmanac.org/games/aStarTutorial.htm This is the English original "A * primer", the most classic explanation, with demo demo

Http://www.cnblogs.com/technology/archive/2011/05/26/2058842.html This is the Chinese translation after the simple version, there is a simplified code demo, but some errors, speak very clearly, this picture from this article

http://blog.csdn.net/b2b160/article/details/4057781 A survey of pathfinding algorithm

A * pathfinding algorithm is a commonly used AI algorithm in the game, which is easy to understand by using C + + simple implementation of the algorithm.

Search areaA simple map, where the green block is the starting point (denoted by A), the middle blue is an obstacle, the red block (denoted by B) is the destination. In order to use a two-dimensional array to represent the map, we divide the map into small squares. Start finding a way

• 1. Starting from start a, put it in an "open list" as a pending box, and the open list is a list of squares waiting to be checked.
  
• 2. Look for the squares around the starting point A, put them in the "open list" and set their "parent square" to a.
  
• 3. Remove the start a from the "open list" and add the starting point A to the "Close list", "Close list" is a box that does not need to be checked again

　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　 a light green stroke block in the figure indicates that the "open list" has been added for inspection. The start of a light blue stroke a means that it has been placed in the close list, and it does not need to be checked again.find the most appropriate block from the "Open list" and calculate it by the formula F=g+h.F = G + HG means the movement cost of moving from start A to the specified squares on the grid (which can be moved in A diagonal direction).
h represents the estimated cost of moving from the specified square to the end B (H has many calculations, the code of this article uses a simple Euclidean distance calculation method). 　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　 we assume that the cost of moving a lattice horizontally is 10, and for ease of calculation, it is 14 to move a lattice in a diagonal direction. In order to show more intuitively how to operate FGH, the upper left corner of the block shows F, the lower left corner represents G, and the lower right corner represents H. See if it's the same as what you think?from the "Open list", select the lowest F-value square C (the square to the right of the Green start square), and then proceed as follows:(if there are obstacles above and below C, will it go into a dead end?) No, according to the algorithm, C will be placed directly into the close list, without any node's F Update and parent node update)

• 4. Remove it from the "open list" and place it in the "Close list".
• 5. Check all squares that are adjacent and can be reached (the blocks are not considered in the "closed list"). If these squares are not in the "open list", add them to the "open list", calculate the G, H, and F values of the squares, and set their "parent square" to "C".
• 6. If an adjacent square D is already in the "Open list", check if the new path (that is, through the path of C) to reach it, the G value will be lower, if the new G is low, then the "Parent square" to the currently selected grid C, and then recalculate its F-value and G-value ( The H value does not need to be recalculated, because the H value is constant for each block. If the new G-value is higher, then it is not a wise choice to go through C and then D, because it takes a long way, and we do nothing.

　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　 , we selected C because its F value is the smallest, we remove it from the "open list" and add it to the "close list". It's right up and down three walls, so don't think about them. It has a start block on the left and is added to the "close list" and is not considered. So the candidate blocks around it are There are only 4 left. Let's take a look at the grid below C, where the current G is 14, if you reach it by C, G will be 10 + 10, which is bigger than 14, so we do nothing.then we continue to find the lowest F value from the "open list", but we find that the top and bottom of C are 54 at the same time, so what do we do? Take whichever one you want, for example, we chose the block D below C. 　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　 D right side of the upper right is the wall, so do not consider, but why the lower right corner is not added to the "open list" it? Because if the block under C is not allowed to go, you need to go from the "corner of the Block" to the box in the lower right corner of C, and set whether this is allowed in the program. (the example in the figure does not allow this to go) 　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　 In this way, we find the lowest F value from the "Open list", remove it from the "open list" and add it to the "close list". And then continue to find the blocks around it, so loop it down ...So when does it stop?--when we find the destination end block in the "Start list", the path has been found. Output pathas shown, in addition to the starting block, every block that has been or is still in the "open list", it has a "parent box", through the "parent block" can be indexed to the original "Start Block", this is the path.Algorithm pseudo Code

Add the start grid to the open list do   {          look for the lowest F value in the Open list, we call it the current lattice.          Switch it to the close list.          Each of the 8 cells that are adjacent to the current grid             (it cannot be passed | | Already in the "Close list")             {                   do nothing.              }             if (it is not in the open list)             {                   add it to the "open list" and use the current grid as the parent of this grid, calculate the FGH             if (it is already in the open list)             {                   if ( Check to see if the new path is better with a G value, a lower g value means a better path)                       {                               Change the parent node of this grid to the current grid, and recalculate the GF value of this lattice.                       }   } while (the target is already in the "open list", The path is found)   if the open list is empty, the path does not exist.    

　　

C + + Implementation codeAstar.h

#pragma once/*//a* algorithm object class */#include <vector> #include <list> const int kcost1=10; Shift one grid to consume the const int kcost2=14; Skew one grid consumes struct point {int x, y,//DOT coordinates, here to facilitate the calculation by C + + array, x for the horizontal, y for vertical column int f,g,h;//f=g+h points *parent;      The coordinates of the parent, where there is no pointer, simplifies the code point (int _x,int _y): X (_x), Y (_y), F (0), G (0), H (0), parent (NULL)//variable initialization {}};      Class Astar {public:void Initastar (std::vector<std::vector<int>> &_maze);    Std::list<point *> GetPath (Point &startpoint,point &endpoint,bool isignorecorner);      Private:point *findpath (Point &startpoint,point &endpoint,bool isignorecorner);      Std::vector<point *> getsurroundpoints (const point *point,bool isignorecorner) const; BOOL Iscanreach (const point *point,const, point *target,bool isignorecorner) const; Determines whether a point can be used for the next step in judging the const std::list<point *>, the *isinlist (const-&AMP;LIST,CONST-dot *point); Determine if a point in the open/Close list is included*getleastfpoint ();      Returns the node with the lowest F-value from the Open list//calculates the FGH value int CALCG (point *temp_start,point *point);      int Calch (point *point,point *end);  int CALCF (point *point);      Private:std::vector<std::vector<int>> Maze;  Std::list<point *> openlist; Open list Std::list<point *> closelist;  Close List};

Astar.cpp

#include <math.h> #include "Astar.h" void Astar::initastar (std::vector<std::vector<int>> &_  Maze) {Maze=_maze; } int ASTAR::CALCG (point *temp_start,point *point) {int extrag= (ABS (POINT-&GT;X-TEMP_START-&GT;X) +abs (point->      y-temp_start->y)) ==1?kcost1:kcost2; int parentg=point->parent==null?0:point->parent->g;  If it is the initial node, its parent node is empty return Parentg+extrag; } int Astar::calch (point *point,point *end) {//h is calculated with a simple Euclidean distance, the calculation of H is the key, there are many algorithms, no in-depth study ^_^ return sqrt (double) (end->x-point->x) * (double) (end->x-point->x) + (double) (end->y-point->y) * (double) (end->  y-point->y)) *kcost1;  } int ASTAR::CALCF (point *point) {return point->g+point->h;          } point *astar::getleastfpoint () {if (!openlist.empty ()) {Auto Respoint=openlist.front ();          for (auto &point:openlist) if (point->f<respoint->f) Respoint=point; Return ResPoint;  } return NULL; } point *astar::findpath (Point &startpoint,point &endpoint,bool isignorecorner) {openlist.push_back (new Point (STARTPOINT.X,STARTPOINT.Y)); Place the starting point, copy open a node, internal and external isolation while (!openlist.empty ()) {Auto Curpoint=getleastfpoint ();//Find the point with the lowest F value ope Nlist.remove (Curpoint); Remove Closelist.push_back (curpoint) from the open list;          Put in the close list//1, find the current surrounding eight grid can pass the lattice auto surroundpoints=getsurroundpoints (Curpoint,isignorecorner);              for (auto &target:surroundpoints) {//2, on a certain lattice, if it is not in the open list, add to the open list, set the current grid to its parent node, calculate F G H                    if (!isinlist (Openlist,target)) {target->parent=curpoint;                  TARGET-&GT;G=CALCG (Curpoint,target);                  Target->h=calch (Target,&endpoint);                    TARGET-&GT;F=CALCF (target);              Openlist.push_back (target); }//3, for a certain lattice, it in the open list, calculates the G value, if bigger than the original, do nothing, noThe parent node is set to the current point, and the G and f else {int TEMPG=CALCG (curpoint,target) are updated;                        if (tempg<target->g) {target->parent=curpoint;                      target->g=tempg;                  TARGET-&GT;F=CALCF (target);              }} point *respoint=isinlist (Openlist,&endpoint); if (respoint) return respoint;  Returns the node pointer in the list, not with the original endpoint pointer, because a deep copy has occurred}} return NULL; } std::list<point *> Astar::getpath (Point &startpoint,point &endpoint,bool isIgnoreCorner) {point      *result=findpath (Startpoint,endpoint,isignorecorner);      Std::list<point *> Path;          Returns the path, if no path is found, returns an empty list while (result) {Path.push_front (result);      result=result->parent;  } return path; } point *astar::isinlist (const std::list<point *> &list,const point *point) const {      To determine if a node is in the list, it is not possible to compare pointers here, because each join list is a newly opened node and can only compare coordinates for (auto p:list) if (p->x==point->x&&p-&gt      ; y==point->y) return p;  return NULL; } bool Astar::iscanreach (const point *point,const point *target,bool isignorecorner) const {if (target->x<0 || Target->x>maze.size ()-1 | | Target->y<0&&target->y>maze[0].size ()-1 | | Maze[target->x][target->y]==1 | | Target->x==point->x&&target->y==point->y | |      Isinlist (Closelist,target))//returns False if the point is coincident with the current node, is out of the map, is an obstacle, or is in the closed list;          else {if (ABS (POINT-&GT;X-TARGET-&GT;X) +abs (point->y-target->y) ==1)//non-bevel can return true; else {//diagonal to determine if (maze[point->x][target->y]==0&&maze[target              ->x][point->y]==0) return true;          else return isignorecorner; }     }} std::vector<point *> astar::getsurroundpoints (const point *point,bool isignorecorner) const {STD        :: Vector<point *> surroundpoints; for (int x=point->x-1;x<=point->x+1;x++) for (int y=point->y-1;y<=point->y+1;y++) if            (Iscanreach (Point,new point (x, y), Isignorecorner)) Surroundpoints.push_back (new Point (x, y));  return surroundpoints;   }

Main.cpp

#include <iostream>  #include "Astar.h"  using namespace std;    int main ()  {      //initializes the map, represents the map with a two-dimensional matrix, 1 represents an obstacle, and 0 indicates a      vector<vector<int>> maze={          { 1,1,1,1,1,1,1,1,1,1,1,1},          {1,0,0,1,1,0,1,0,0,0,0,1}, {          1,0,0,1,1,0,0,0,0,0,0,1},          { 1,0,0,0,0,0,1,0,0,1,1,1},          {1,1,1,0,0,0,0,0,1,1,0,1}, {          1,1,0,1,0,0,0,0,0,0,0,1},          { 1,0,1,0,0,0,0,1,0,0,0,1},          {1,1,1,1,1,1,1,1,1,1,1,1}      };      Astar Astar;      Astar. Initastar (maze);        Set start and end points point      start (n);      Point End (6,10);      A * algorithm seeks the path      list<point *> Path=astar. GetPath (start,end,false);      Print      for (auto &p:path)          cout<< ' (' <<p->x<< ', ' <<p->y<< ') ' < <endl;        System ("pause");      return 0;  }  

Run Results

[Turn] A * pathfinding algorithm C + + simple implementation