A*演算法（C++實現）
阿新 • • 發佈：2019-01-22
簡易地圖
如圖所示簡易地圖, 其中綠色方塊的是起點 (用 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 右邊的方塊), 然後對它進行如下處理:
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++實現程式碼：
版本1：
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;
}
// 清空臨時開閉列表，防止重複執行GetPath導致結果異常
openList.clear();
closeList.clear();
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;
}
版本2：
Astar.h
#ifndef ASTAR_H
#define ASTAR_H
#include <iostream>
#include <queue>
#include <vector>
#include <stack>
#include<algorithm>
using namespace std;
typedef struct Node
{
int x, y;
int g; //起始點到當前點實際代價
int h;//當前節點到目標節點最佳路徑的估計代價
int f;//估計值
Node* father;
Node(int x, int y)
{
this->x = x;
this->y = y;
this->g = 0;
this->h = 0;
this->f = 0;
this->father = NULL;
}
Node(int x, int y, Node* father)
{
this->x = x;
this->y = y;
this->g = 0;
this->h = 0;
this->f = 0;
this->father = father;
}
}Node;
class Astar{
public:
Astar();
~Astar();
void search(Node* startPos, Node* endPos);
void checkPoit(int x, int y, Node* father, int g);
void NextStep(Node* currentPoint);
int isContains(vector<Node*>* Nodelist, int x, int y);
void countGHF(Node* sNode, Node* eNode, int g);
static bool compare(Node* n1, Node* n2);
bool unWalk(int x, int y);
void printPath(Node* current);
void printMap();
vector<Node*> openList;
vector<Node*> closeList;
Node *startPos;
Node *endPos;
static const int WeightW = 10;// 正方向消耗
static const int WeightWH = 14;//打斜方向的消耗
static const int row = 6;
static const int col = 8;
};
#endifAstar.cpp
#include "Astar.h"
int map[101][101] =
{
{ 0, 0, 0, 1, 0, 1, 0, 0, 0 },
{ 0, 0, 0, 1, 0, 1, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 1, 0, 0, 0 },
{ 0, 0, 0, 1, 0, 1, 0, 1, 0 },
{ 0, 0, 0, 1, 0, 1, 0, 1, 0 },
{ 0, 0, 0, 1, 0, 0, 0, 1, 0 },
{ 0, 0, 0, 1, 0, 0, 0, 1, 0 }
};
Astar::Astar()
{
}
Astar::~Astar()
{
}
void Astar::search(Node* startPos, Node* endPos)
{
if (startPos->x < 0 || startPos->x > row || startPos->y < 0 || startPos->y >col ||
endPos->x < 0 || endPos->x > row || endPos->y < 0 || endPos->y > col)
return;
Node* current;
this->startPos = startPos;
this->endPos = endPos;
openList.push_back(startPos);
//主要是這塊，把開始的節點放入openlist後開始查詢旁邊的8個節點，如果座標超長範圍或在closelist就return 如果已經存在openlist就對比當前節點到遍歷到的那個節點的G值和當前節點到原來父節點的G值 如果原來的G值比較大 不用管 否則重新賦值G值 父節點 和f 如果是新節點 加入到openlist 直到opellist為空或找到終點
while (openList.size() > 0)
{
current = openList[0];
if (current->x == endPos->x && current->y == endPos->y)
{
cout << "find the path" << endl;
printMap();
printPath(current);
openList.clear();
closeList.clear();
break;
}
NextStep(current);
closeList.push_back(current);
openList.erase(openList.begin());
sort(openList.begin(), openList.end(), compare);
}
}
void Astar::checkPoit(int x, int y, Node* father, int g)
{
if (x < 0 || x > row || y < 0 || y > col)
return;
if (this->unWalk(x, y))
return;
if (isContains(&closeList, x, y) != -1)
return;
int index;
if ((index = isContains(&openList, x, y)) != -1)
{
Node *point = openList[index];
if (point->g > father->g + g)
{
point->father = father;
point->g = father->g + g;
point->f = point->g + point->h;
}
}
else
{
Node * point = new Node(x, y, father);
countGHF(point, endPos, g);
openList.push_back(point);
}
}
void Astar::NextStep(Node* current)
{
checkPoit(current->x - 1, current->y, current, WeightW);//左
checkPoit(current->x + 1, current->y, current, WeightW);//右
checkPoit(current->x, current->y + 1, current, WeightW);//上
checkPoit(current->x, current->y - 1, current, WeightW);//下
checkPoit(current->x - 1, current->y + 1, current, WeightWH);//左上
checkPoit(current->x - 1, current->y - 1, current, WeightWH);//左下
checkPoit(current->x + 1, current->y - 1, current, WeightWH);//右下
checkPoit(current->x + 1, current->y + 1, current, WeightWH);//右上
}
int Astar::isContains(vector<Node*>* Nodelist, int x, int y)
{
for (int i = 0; i < Nodelist->size(); i++)
{
if (Nodelist->at(i)->x == x && Nodelist->at(i)->y == y)
{
return i;
}
}
return -1;
}
void Astar::countGHF(Node* sNode, Node* eNode, int g)
{
int h = (abs(sNode->x - eNode->x) + abs(sNode->y - eNode->y)) * WeightW;
int currentg = sNode->father->g + g;
int f = currentg + h;
sNode->f = f;
sNode->h = h;
sNode->g = currentg;
}
bool Astar::compare(Node* n1, Node* n2)
{
//printf("%d,%d",n1->f,n2->f);
return n1->f < n2->f;
}
bool Astar::unWalk(int x, int y)
{
if (map[x][y] == 1)
return true;
return false;
}
void Astar::printPath(Node* current)
{
if (current->father != NULL)
printPath(current->father);
printf("(%d,%d)", current->x, current->y);
}
void Astar::printMap()
{
for (int i = 0; i <= row; i++){
for (int j = 0; j <= col; j++){
printf("%d ", map[i][j]);
}
printf("\n");
}
}main.cpp
#include "Astar.h"
int main(int argc, char* argv[])
{
Astar astar;
Node *startPos = new Node(5, 1);
Node *endPos = new Node(3, 8);
astar.search(startPos, endPos);
getchar();
return 0;
}