[轉] A*尋路演算法C++簡單實現

標籤：輸出   post   紅色   nts   base   astar   切換   gets   刪除   

參考文章：

http://www.policyalmanac.org/games/aStarTutorial.htm   這是英文原文《A*入門》，最經典的講解，有demo示範

http://www.cnblogs.com/technology/archive/2011/05/26/2058842.html  這是國人翻譯後整理的簡版，有簡單代碼demo，不過有些錯誤，講得很清晰，本文圖片來自這篇

http://blog.csdn.net/b2b160/article/details/4057781  一片關於尋路演算法的綜述

 

A*尋路演算法是遊戲中常用的AI演算法，這裡用C++簡單實現了一下演算法，便於理解。

搜尋地區  　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　簡易地圖, 其中綠色方塊的是起點 (用 A 表示), 中間藍色的是障礙物, 紅色的方塊 (用 B 表示) 是目的地. 為了可以用一個二維數組來表示地圖, 我們將地圖劃分成一個個的小方塊。 開始尋路

• 1.從起點A開始, 把它作為待處理的方格存入一個"開啟列表", 開啟列表就是一個等待檢查方格的列表.
  
• 2.尋找起點A周圍可以到達的方格, 將它們放入"開啟列表", 並設定它們的"父方格"為A.
  
• 3.從"開啟列表"中刪除起點 A, 並將起點 A 加入"關閉列表", "關閉列表"中存放的都是不需要再次檢查的方格

　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　圖中淺綠色描邊的方塊表示已經加入 "開啟列表" 等待檢查. 淡藍色描邊的起點 A 表示已經放入 "關閉列表" , 它不需要再執行檢查.從 "開啟列表" 中找出相對最適宜的方塊, 通過公式 F=G+H 來計算. F = G + H G 表示從起點 A 移動到網格上指定方格的移動耗費 (可沿斜方向移動).
 H 表示從指定的方格移動到終點 B 的預計耗費 (H 有很多計算方法, 本文代碼使用簡單的歐幾裡得距離計算方法).　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　我們假設橫向移動一個格子的耗費為10, 為了便於計算, 沿斜方向移動一個格子耗費是14. 為了更直觀的展示如何運算 FGH, 圖中方塊的左上方數字表示 F, 左下角表示 G, 右下角表示 H. 看看是否跟你心裡想的結果一樣?從 "開啟列表" 中選擇 F 度量最低的方格 C (綠色起始方塊 A 右邊的方塊), 然後對它進行如下處理:（如果C上方和下方都是障礙物的話會走入死胡同嗎？不會，根據演算法，這時候C會被直接放到關閉列表，沒有發生任何節點的F更新和父節點更新）

• 4.把它從 "開啟列表" 中刪除, 並放到 "關閉列表" 中.
• 5.檢查它所有相鄰並且可以到達 (障礙物和 "關閉列表" 的方格都不考慮) 的方格. 如果這些方格還不在 "開啟列表" 裡的話, 將它們加入 "開啟列表", 計算這些方格的 G, H 和 F 度量各是多少, 並設定它們的 "父方格" 為 C.
• 6.如果某個相鄰方格 D 已經在 "開啟列表" 裡了, 檢查如果用新的路徑 (就是經過C 的路徑) 到達它的話, G值是否會更低一些, 如果新的G值更低, 那就把它的 "父方格" 改為目前選中的方格 C, 然後重新計算它的 F 度量和 G 值 (H 值不需要重新計算, 因為對於每個方塊, H 值是不變的). 如果新的 G 值比較高, 就說明經過 C 再到達 D 不是一個明智的選擇, 因為它需要更遠的路, 這時我們什麼也不做.

　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　, 我們選中了 C 因為它的 F 度量最小, 我們把它從 "開啟列表" 中刪除, 並把它加入 "關閉列表". 它右邊上下三個都是牆, 所以不考慮它們. 它左邊是起始方塊, 已經加入到 "關閉列表" 了, 也不考慮. 所以它周圍的候選方塊就只剩下 4 個. 讓我們來看看 C 下面的那個格子, 它目前的 G 是14, 如果通過 C 到達它的話, G將會是 10 + 10, 這比 14 要大, 因此我們什麼也不做.然後我們繼續從 "開啟列表" 中找出 F 度量最小的, 但我們發現 C 上面的和下面的同時為 54, 這時怎麼辦呢? 這時隨便取哪一個都行, 比如我們選擇了 C 下面的那個方塊 D.　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　D 右邊已經右上方的都是牆, 所以不考慮, 但為什麼右下角的沒有被加進 "開啟列表" 呢? 因為如果 C 下面的那塊也不可以走, 想要到達 C 右下角的方塊就需要從 "方塊的角" 走了, 在程式中設定是否允許這樣走. (圖中的樣本不允許這樣走)　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　就這樣, 我們從 "開啟列表" 找出 F 度量最小的, 將它從 "開啟列表" 中移掉, 添加到 "關閉列表". 再繼續找出它周圍可以到達的方塊, 如此迴圈下去...那麼什麼時候停止呢? —— 當我們發現 "開始列表" 裡出現了目標終點方塊的時候, 說明路徑已經被找到. 輸出路徑　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　如所示, 除了起始方塊, 每一個曾經或者現在還在 "開啟列表" 裡的方塊, 它都有一個 "父方塊", 通過 "父方塊" 可以索引到最初的 "起始方塊", 這就是路徑. 演算法偽碼

把起始格添加到 "開啟列表"   do   {          尋找開啟列表中F值最低的格子, 我們稱它為當前格.          把它切換到關閉列表.          對當前格相鄰的8格中的每一個             if (它不可通過 || 已經在 "關閉列表" 中)             {                   什麼也不做.              }             if (它不在開啟列表中)             {                   把它添加進 "開啟列表", 把當前格作為這一格的父節點, 計算這一格的 FGH             if (它已經在開啟列表中)             {                   if (用G值為參考檢查新的路徑是否更好, 更低的G值意味著更好的路徑)                       {                               把這一格的父節點改成當前格, 並且重新計算這一格的 GF 度量.                       }   } while( 目標格已經在 "開啟列表", 這時候路徑被找到)   如果開啟列表已經空了, 說明路徑不存在.    最後從目標格開始, 沿著每一格的父節點移動直到回到起始格, 這就是路徑. 

　　

C++實現代碼 Astar.h

#pragma once  /* //A*演算法對象類 */  #include <vector>  #include <list>    const int kCost1=10; //直移一格消耗  const int kCost2=14; //斜移一格消耗    struct Point  {      int x,y; //點座標，這裡為了方便按照C++的數組來計算，x代表橫排，y代表豎列      int F,G,H; //F=G+H      Point *parent; //parent的座標，這裡沒有用指標，從而簡化代碼      Point(int _x,int _y):x(_x),y(_y),F(0),G(0),H(0),parent(NULL)  //變數初始化      {      }  };      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; //判斷某點是否可以用於下一步判斷      Point *isInList(const std::list<Point *> &list,const Point *point) const; //判斷開啟/關閉列表中是否包含某點      Point *getLeastFpoint(); //從開啟列表中返回F值最小的節點      //計算FGH值      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;  //開啟列表      std::list<Point *> closeList; //關閉列表  }; 

 

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->x-temp_start->x)+abs(point->y-temp_start->y))==1?kCost1:kCost2;      int parentG=point->parent==NULL?0:point->parent->G; //如果是初始節點，則其父節點是空      return parentG+extraG;  }    int Astar::calcH(Point *point,Point *end)  {      //用簡單的歐幾裡得距離計算H，這個H的計算是關鍵，還有很多演算法，沒深入研究^_^      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)); //置入起點,拷貝開闢一個節點，內外隔離      while(!openList.empty())      {          auto curPoint=getLeastFpoint(); //找到F值最小的點          openList.remove(curPoint); //從開啟列表中刪除          closeList.push_back(curPoint); //放到關閉列表          //1,找到當前周圍八個格中可以通過的格子          auto surroundPoints=getSurroundPoints(curPoint,isIgnoreCorner);          for(auto &target:surroundPoints)          {              //2,對某一個格子，如果它不在開啟列表中，加入到開啟列表，設定當前格為其父節點，計算F G H              if(!isInList(openList,target))              {                  target->parent=curPoint;                    target->G=calcG(curPoint,target);                  target->H=calcH(target,&endPoint);                  target->F=calcF(target);                    openList.push_back(target);              }              //3，對某一個格子，它在開啟列表中，計算G值, 如果比原來的大, 就什麼都不做, 否則設定它的父節點為當前點,並更新G和F              else              {                  int tempG=calcG(curPoint,target);                  if(tempG<target->G)                  {                      target->parent=curPoint;                        target->G=tempG;                      target->F=calcF(target);                  }              }              Point *resPoint=isInList(openList,&endPoint);              if(resPoint)                  return resPoint; //返回列表裡的節點指標，不要用原來傳入的endpoint指標，因為發生了深拷貝          }      }        return NULL;  }    std::list<Point *> Astar::GetPath(Point &startPoint,Point &endPoint,bool isIgnoreCorner)  {      Point *result=findPath(startPoint,endPoint,isIgnoreCorner);      std::list<Point *> path;      //返迴路徑，如果沒找到路徑，返回空鏈表      while(result)      {          path.push_front(result);          result=result->parent;      }      return path;  }    Point *Astar::isInList(const std::list<Point *> &list,const Point *point) const  {      //判斷某個節點是否在列表中，這裡不能比較指標，因為每次加入列表是新開闢的節點，只能比較座標      for(auto p:list)          if(p->x==point->x&&p->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)) //如果點與當前節點重合、超出地圖、是障礙物、或者在關閉列表中，返回false          return false;      else      {          if(abs(point->x-target->x)+abs(point->y-target->y)==1) //非斜角可以              return true;          else          {              //斜對角要判斷是否絆住              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()  {      //初始化地圖，用二維矩陣代表地圖，1表示障礙物，0表示可通      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);        //設定起始和結束點      Point start(1,1);      Point end(6,10);      //A*演算法找尋路徑      list<Point *> path=astar.GetPath(start,end,false);      //列印      for(auto &p:path)          cout<<‘(‘<<p->x<<‘,‘<<p->y<<‘)‘<<endl;        system("pause");      return 0;  }  

 

 

運行結果  

 

 

[轉] A*尋路演算法C++簡單實現