[轉] A*尋路算法C++簡單實現
阿新 • • 發佈:2017-06-29
track pos endpoint 障礙 close math.h 不存在 rec 節點
如圖所示簡易地圖, 其中綠色方塊的是起點 (用 A 表示), 中間藍色的是障礙物, 紅色的方塊 (用 B 表示) 是目的地. 為了可以用一個二維數組來表示地圖, 我們將地圖劃分成一個個的小方塊。
圖中淺綠色描邊的方塊表示已經加入 "開啟列表" 等待檢查. 淡藍色描邊的起點 A 表示已經放入 "關閉列表" , 它不需要再執行檢查.
從 "開啟列表" 中找出相對最適宜的方塊, 通過公式 F=G+H 來計算.
F = G + H
G 表示從起點 A 移動到網格上指定方格的移動耗費 (可沿斜方向移動).
H 表示從指定的方格移動到終點 B 的預計耗費 (H 有很多計算方法, 本文代碼使用簡單的歐幾裏得距離計算方法). 我們假設橫向移動一個格子的耗費為10, 為了便於計算, 沿斜方向移動一個格子耗費是14. 為了更直觀的展示如何運算 FGH, 圖中方塊的左上角數字表示 F, 左下角表示 G, 右下角表示 H. 看看是否跟你心裏想的結果一樣?
從 "開啟列表" 中選擇 F 值最低的方格 C (綠色起始方塊 A 右邊的方塊), 然後對它進行如下處理:
(如果C上方和下方都是障礙物的話會走入死胡同嗎?不會,根據算法,這時候C會被直接放到關閉列表,沒有發生任何節點的F更新和父節點更新)
參考文章:
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++簡單實現了一下算法,便於理解。
搜索區域
開始尋路
- 1.從起點A開始, 把它作為待處理的方格存入一個"開啟列表", 開啟列表就是一個等待檢查方格的列表.
- 2.尋找起點A周圍可以到達的方格, 將它們放入"開啟列表", 並設置它們的"父方格"為A.
- 3.從"開啟列表"中刪除起點 A, 並將起點 A 加入"關閉列表", "關閉列表"中存放的都是不需要再次檢查的方格
H 表示從指定的方格移動到終點 B 的預計耗費 (H 有很多計算方法, 本文代碼使用簡單的歐幾裏得距離計算方法). 我們假設橫向移動一個格子的耗費為10, 為了便於計算, 沿斜方向移動一個格子耗費是14. 為了更直觀的展示如何運算 FGH, 圖中方塊的左上角數字表示 F, 左下角表示 G, 右下角表示 H. 看看是否跟你心裏想的結果一樣?
- 4.把它從 "開啟列表" 中刪除, 並放到 "關閉列表" 中.
- 5.檢查它所有相鄰並且可以到達 (障礙物和 "關閉列表" 的方格都不考慮) 的方格. 如果這些方格還不在 "開啟列表" 裏的話, 將它們加入 "開啟列表", 計算這些方格的 G, H 和 F 值各是多少, 並設置它們的 "父方格" 為 C.
- 6.如果某個相鄰方格 D 已經在 "開啟列表" 裏了, 檢查如果用新的路徑 (就是經過C 的路徑) 到達它的話, G值是否會更低一些, 如果新的G值更低, 那就把它的 "父方格" 改為目前選中的方格 C, 然後重新計算它的 F 值和 G 值 (H 值不需要重新計算, 因為對於每個方塊, H 值是不變的). 如果新的 G 值比較高, 就說明經過 C 再到達 D 不是一個明智的選擇, 因為它需要更遠的路, 這時我們什麽也不做.
輸出路徑
如上圖所示, 除了起始方塊, 每一個曾經或者現在還在 "開啟列表" 裏的方塊, 它都有一個 "父方塊", 通過 "父方塊" 可以索引到最初的 "起始方塊", 這就是路徑.算法偽碼
把起始格添加到 "開啟列表" 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++簡單實現