光速AStar尋路算法(C++)
一、最終效果
可以看到,我創建了500個小動物,也有110的FPS。雖然它們並不是每一幀都在計算尋路,但是平均下來不卡頓也不錯了。我是i7 6700 + GT 720,這個效果圖雖然不能直觀的說明效率問題,但我相信我的AStar比網上一般的例子快。
如果對繪圖的DND庫感興趣,可以移步我的其他文章。
二、使用方法
針對不同的物體,肯定有不同的尋路邏輯,飛禽走獸水怪可通行路徑和偏好都會不一致。假設我們要爲陸地的怪物設置尋路邏輯,則先要繼承基礎的AStar類:
class AStarFoo : public AStar
{
public:
//相鄰點之間的代價,可以選擇忽略第一個點,從而只返回第二點的消耗
virtual float GetCostNodeToNode(const PointU& source, const PointU& target)
{
//這裏通過target的座標訪問實際節點的信息
//如果目標節點可以通行,則返回代價值
//例如,普通地返回 1.0
//沼澤返回5(更難通行)
//草叢返回 0.85(更加偏好隱蔽的行徑)
auto* node = island->GetNode(target);
if(node->type == GRASS)
return 0.85f;
//……else if
//返回0代表不可通行
return 0;
}
AStarFoo() :AStar(true, 200 * 200) { }
};
繼承AStar類,重寫GetCostNodeToNode函數,定義了節點之間的自定義代價值,可以根據兩個節點的信息處理,也可以只處理將要去的節點,如上面代碼的例子。
而調用的基類構造函數中,true控制是否可以斜穿網格,200*200定義了最大訪問的節點數,防止尋路失敗時浪費大量計算時間,一旦尋路超過4萬,就會返回失敗。
接着如下代碼尋路:
//創建一個Foo實例
AstarFoo foo;
//用於存儲返回的路徑
list<PointU> list_path;
//尋找座標(100, 200) -> (300, 100)的路徑
if (foo.Search(
PointU(100, 200),
PointU(300, 100), list_path)
{
//處理返回的路徑list_path
}
還可以額外傳入一個鏈表指針,以返回相對最近的路徑(失敗尋路):
//用於存儲返回的路徑
list<PointU> list_path;
//失敗尋路的最近路徑
list<PointU> list_fail_path;
//尋找座標(100, 200) -> (300, 100)的路徑
if (foo.Search(
PointU(100, 200),
PointU(300, 100), list_path, &list_fail_path)
{
//處理返回的路徑list_path
}
else
{
//處理失敗返回的路徑list_fail_path
}
三、基本原理
(甲)圖的連接
網上有大把的關於A星尋路算法的原理,不過大部分都是基於2d網格的。實際上AStar算法,本質上是基於圖的,只要用戶定義了圖的連接,和節點之間的代價,AStar就可以進行尋路。
只不過2d網格是一種特殊的圖,並且一般來說是雙向,如果用圖來實現平鋪網格的信息,就會變得很浪費很慢。 例如從2d網格的(x, y)處出發,自然而然的就知道與它連接的節點是哪些:
右、下、左、上
如果可以斜穿,還包括 右下,左下,左上,右上。
而自定義代價值,附加到節點上即可,不需要保存到連接的信息上。例如,以圖實現,節點3到節點6的代價需要定義,節點6到節點3的代價也要定義,不過你也可以不定義,總之GetCostNodeToNode函數定義了兩個節點之間的代價值。Foo類我們重載的GetCostNodeToNode函數直接設置爲到任意節點,都是固定的代價值,只和目標節點有關。
(乙)代價值
F = G + H,即總代價 = 自定義代價值 + 估計代價值。前面我們反覆提到的代價值即自定義代價值G,表示節點之間的實際行走代價。而估計代價值H則是估計兩個節點的代價值……對於人類來說,一眼看到河對面,就知道走過去會不會太費力(取決於有沒有橋,或者船)。但是電腦不會直接知道,哪裏有沒有橋或者船(如果遍歷尋找,就失去了AStar的意義——快速的尋路)。其實這也不是人類的智慧多麼偉大,只是由於信息的獲知問題。假設人去一個從來沒去過的地方,然後前面是一座很大的山,人也無法估計是否路徑可行。
對於電腦(後面稱AI了)來說,最直觀的就是兩點之間的距離(隱含在了PointU裏面)。離目標點越近,則代價值越小,而AI就會先去往代價值小的點,但是AI並無法直接感知代價,只能先走着去,再實際判斷我們設定的代價值(爲0就不可通行了,無論估計值是多少)。
而總代價是兩者之和,決定了哪個節點最先被AI考察。在考察的路途中,還包含了經過的節點代價。例如尋路經過10個節點,則下一個節點的代價需要加上第10個節點的代價,當然第10個的代價已經加上了第9個的代價,依次類推,當前節點的代價爲整條路徑的代價和。當待考察的點有更小的代價值時,就會從那個節點開始往周圍尋路。更多細節問題,請見後面。
(丙)Open表和Closed表
open表表示應該被檢查的節點,每次循環,我們取出總代價值最小的的那一個。如果是目標節點(尋路的目標位置),則返回成功,並按_parent指針,返回整個路徑。如果不是,則移動到closed表,然後將與其相連的節點做以下處理:
1.如果在closed表,說明再訪問這個節點沒有了意義,因爲只可能通過它相鄰的節點達到目標。所以不做其他處理。
2.如果不在open表,則添加到open表,當然需要設置它的總代價值、自定義代價值、父節點指針三個屬性(此操作保證表有序)。
3.如果在open表,且此節點新計算的總代價值比舊值小,說明新的路徑更優,則更新爲新值,且使用新的父節點(此操作改變了總代價值,也需要保證有序)。
四、具體實現
(甲)節點銀行
首先我們需要表示一個節點類AStarNode,如下:
class AStarNode
{
public:
PointU _xy;
AStarNode* _parent; //父節點(爲空代表起點)
float _cost; //前往這個節點 的代價值
float _total; //總代價 (cost + 估計代價)
bool _open; //是否在 Open表
bool _closed; //是否在 Close表
};
其中成員表示的含義如下:
_xy | 位置 |
_parent | 父節點,當尋路成功時,按這個返回整條路徑的鏈表 |
_cost | 自定義代價 |
_total | 總代價 |
_open | 是否在open表 |
_closed | 是否在close表 |
由於需要大量使用AStarNode,所以我們直接用數組(節點銀行,簡單的內存池)保存,反覆使用,防止new和delete的開銷,在AStart的構造函數就分配了內存:
//AStar成員
AStarNode* _arrNodeBank;//節點銀行
//AStar構造函數
AStar(bool sideling = true, UINT32 max_node = DND_ASTAR_SEARCH_MAX)
{
_arrNodeBank = new AStarNode[max_node];
//other……
}
//析構函數
virtual ~AStar()
{
delete[] _arrNodeBank;
}
(乙)優先隊列(二叉堆)
由於我們需要反覆從open表取出最小代價的節點,所以可以用優先隊列來保存open表,也僅僅是保存指針,所有內存都在節點銀行。其中又以二叉堆的速度最快,可以利用標準庫的vector數組和堆的算法來實現一系列操作,代碼如下:
inline bool fn_CompareAStarNode(AStarNode* n1, AStarNode* n2)
{
return n1->_total > n2->_total;
}
//優先隊列
class AStarPriorityQueue
{
public:
AStarNode* Pop()
{
AStarNode* node = this->_heap.front();
pop_heap(this->_heap.begin(), this->_heap.end(), fn_CompareAStarNode);
this->_heap.pop_back();
return node;
}
void Push(AStarNode* node)
{
this->_heap.push_back(node);
push_heap(this->_heap.begin(), this->_heap.end(), fn_CompareAStarNode);
}
void UpdateNode(AStarNode* node)
{
vector<AStarNode*>::iterator iter;
for (iter = this->_heap.begin(); iter != this->_heap.end(); iter++)
{
if ((*iter)->_xy == node->_xy)
{
push_heap(this->_heap.begin(), iter + 1, fn_CompareAStarNode);
return;
}
}
}
bool IsEmpty()
{
return this->_heap.empty();
}
void Clear()
{
_heap.clear();
}
private:
vector<AStarNode*> _heap;
};
原書是通過仿函數進行排序的,不過lambda表達式和全局函數都可以,至於仿函數和lambda表達式誰快,我感覺應該是全局函數,但沒有實際實驗。防止比較函數重定義,必須加上內聯標記。部分成員函數的功能如下:
Pop | 取出總代價值最小的節點 |
Push | 按序插入一個節點 |
UpdateNode | 修改節點總代價值後,再刷新順序位置 |
(丙)估計值H的計算
前面說了估計值是通過幾何距離(勾股定理)求得的,不過我這兒不一樣,因爲怪物只能斜45度和水平垂直移動,實際的路徑如下:
所以說,默認的實現如下,這樣計算量也比較低:
//根號2 減 1
const float DND_ASTAR_SQRT_2_SUB_1 = 0.4142135f;
//返回到目標點的 估計值
virtual inline float GetNodeHeuristic(const PointU& source, const PointU& target)
{
PointU d = target - source;
return (d.x > d.y ? (d.x + DND_ASTAR_SQRT_2_SUB_1 * d.y) : (d.y + DND_ASTAR_SQRT_2_SUB_1 * d.x));
}
需要注意的是,PointU是無符號整型,在進行減法時,實際是求得二者之差絕對值。可以參考後面PointU類的實現。另外,這是一個虛函數,所以你也可以自己定義估計代價函數GetNodeHeuristic返回H值。另外高估斜着走的價值,可以使最終路徑斜着走的更少(因爲斜着走的估計代價值更高了)。
(丁)起始節點與成功返回
下面進入開始尋路的前要操作,首先放入起始點到open表,如下操作:
//起點插入Open表
AStarNode* node_start = _get_node(source);
node_start->_open = true;
node_start->_closed = false;
node_start->_cost = 0;
node_start->_total = GetNodeHeuristic(source, target);//_total = _cost + h
node_start->_parent = NULL;
_queueOpen.Push(node_start);
接着是循環的操作,直到open表爲空(失敗),或者best_node就是要尋路到的位置(成功):
AStarNode* best_node = NULL;//代價值最小的節點
while (!_queueOpen.IsEmpty())
{
//取出 消耗值 最小的節點
best_node = _queueOpen.Pop();
//是目標節點
if (best_node->_xy == target)
{
//構造路徑(包含首尾節點)
while (best_node)
{
list_path.push_front(best_node->_xy);
best_node = best_node->_parent;
}
return true;
}
//TODO: 如果不是目標節點的操作
}
(戊)添加相鄰節點到open表
顯然,一開始只有起點一個節點在open表。如果當前節點不是終點,就加入與它相鄰的節點到open表。相鄰節點的判斷如上面所說的,可以自行修改代碼實現圖的連接,也可以用默認的2d網格:
//你可以修改這裏 實現圖 的連接,這裏就不再實現了
//相鄰點
_check_connecting(1, 0, DND_ASTAR_COST_MIN, best_node, target);
_check_connecting(-1, 0, DND_ASTAR_COST_MIN, best_node, target);
_check_connecting(0, 1, DND_ASTAR_COST_MIN, best_node, target);
_check_connecting(0, -1, DND_ASTAR_COST_MIN, best_node, target);
//是否斜穿節點
if (_bSideling)
{
_check_connecting(1, 1, DND_ASTAR_COST_MIN_SQRT_2, best_node, target);
_check_connecting(-1, 1, DND_ASTAR_COST_MIN_SQRT_2, best_node, target);
_check_connecting(-1, -1, DND_ASTAR_COST_MIN_SQRT_2, best_node, target);
_check_connecting(1, -1, DND_ASTAR_COST_MIN_SQRT_2, best_node, target);
}
其中水平垂直的相鄰節點,代價傳入了1.0作爲單位距離的代價值係數(斜切的傳入了根號2,由於斜着穿格子路程更長,所以自定義代價值也應該更大):
//單位距離的代價值
const float DND_ASTAR_COST_MIN = 1.0f;
//根號2 倍單位距離 消耗
const float DND_ASTAR_COST_MIN_SQRT_2 = sqrt(2);
當加入相鄰的節點後,此節點就可以加入close表了,此次尋路不會再訪問到它:
best_node->_closed = true;
(己)相鄰節點的處理
前面已經說到了相鄰節點的處理,即_check_connecting函數的實現,首先判斷自定義代價值G,如果是不存在的遊戲節點(非AStarNode),或者不可通行的節點,則直接不處理:
//代價,new_xy是相鄰節點的位置
float g = GetCostNodeToNode(best_node->_xy, new_xy);
//如果代價 等於0,說明此相鄰節點不可通行
if (g == 0)
return;
接着需要返回此位置的AStarNode數據,當然這個AStarNode第一次返回時,會從節點銀行取得對象並進行初始化。之後再訪問,則通過hash_map直接返回,當然它存的還是指針,並不實際擁有內存的釋放權利。
AStarNode* _get_node(PointU xy)
{
//如果在 主列表 直接返回,如果沒有則從節點銀行構造一個
UINT64 index = xy.ToIndex2();//返回64位整型防止計算的索引越界
AStarNode*& node = _hashMasterNode[index];
if (node)
{
return node;
}
else
{
node = &_arrNodeBank[_bankCur++];//用一個,則加一
node->_xy = xy;
node->_open = false;//初始的節點,不在closed表,也不在open表
node->_closed = false;
return node;
}
}
返回位置的節點後,則按第三節(丙)的邏輯操作:
//在close表,則不作任何處理
if(actual_node->_closed == false)
{
if (actual_node->_open)
{//在open表,則判斷 總代價是否更小
//上一個點代價 + 移到自己的代價
float new_cost = best_node->_cost + g*cost;
//總代價,自己代價 + 估計代價
float new_total = new_cost + GetNodeHeuristic(new_xy, target);
//新的 代價更小 才刷新
if (new_total < actual_node->_total)
{
actual_node->_parent = best_node;
actual_node->_cost = new_cost;
actual_node->_total = new_total;
_queueOpen.UpdateNode(actual_node);
}
}
else
{//不在open表,則加入
actual_node->_parent = best_node;
actual_node->_cost = best_node->_cost + g*cost;
actual_node->_total = actual_node->_cost + GetNodeHeuristic(new_xy, target);
_queueOpen.Push(actual_node);
actual_node->_open = true;
}
}
五、完整代碼
其中報錯代碼,越界檢測代碼可自行修改或刪除。
//////////////////////////////////////////////////////////////////////////
//name: AStar
//author: Lveyou
//date: 18-08-12
//other:
//18-08-12: 網格二維地圖 快速尋找最短路徑 - Lveyou
//18-08-12: 本代碼 借鑑、改寫於 《遊戲編程精粹1》 - Lveyou
//18-08-14: 如何使用:繼承 AStar類,實現 GetCostNodeToNode函數即可(返回0代表此節點代價無限大)
// 重載 GetNodeHeuristic以自定義估價函數
// 網格大小隻能是[0-65535],如果要更大請修改 PointU類
// _bSideling 控制是否可以斜穿,在AStar類的構造函數初始化
// 有 bug俺可不負責,但可以找上門錘我 - Lveyou
//20-04-15: 優化代碼,且網格大小可以是UINT32值範圍
//////////////////////////////////////////////////////////////////////////
#pragma once
//返回路徑信息用
#include <list>
//二叉堆 用於Open表優先隊列
#include <vector>
//哈希表 儲存主節點表,所有尋路訪問過的節點
#include <unordered_map>
//二叉堆相關操作
#include <algorithm>
using namespace std;
//PointU定義
//#define _DND_ASTAR_
#ifdef _DND_ASTAR_
#include "head.h"
#else
typedef unsigned int UINT32, *PUINT32;
typedef unsigned __int64 UINT64, *PUINT64;
class PointU
{
public:
UINT32 x, y;
PointU() :x(0), y(0) {}
PointU(UINT32 ix, UINT32 iy) :x(ix), y(iy) {}
UINT64 ToIndex2()
{
return (UINT64(x) + UINT64(y) * UINT32_MAX);
}
bool operator==(const PointU& b) const
{
return (x == b.x) && (y == b.y);
}
bool operator!=(const PointU& b) const
{
return (x != b.x) || (y != b.y);
}
//無符號減法
PointU operator-(const PointU& b) const
{
return PointU(x >= b.x ? x - b.x : b.x - x, y >= b.y ? y - b.y : b.y - y);
}
};
#endif // false
//主節點表 最大節點數
const UINT32 DND_ASTAR_SEARCH_MAX = 10000;
//////////////////以下常量不要修改//////////////////////////////////////////
//根號2 減 1
const float DND_ASTAR_SQRT_2_SUB_1 = 0.4142135f;
//根號2
const float DND_ASTAR_SQRT_2 = 1.4142135f;
//單位距離的代價值
const float DND_ASTAR_COST_MIN = 1.0f;
//根號2 倍單位距離 消耗
const float DND_ASTAR_COST_MIN_SQRT_2 = DND_ASTAR_COST_MIN * (DND_ASTAR_SQRT_2);
//節點
class AStarNode
{
public:
PointU _xy;
AStarNode* _parent; //父節點(爲空代表起點)
float _cost; //前往這個節點 的代價值
float _total; //總代價 (cost + 估計代價)
bool _open; //是否在 Open表
bool _closed; //是否在 Close表
};
inline bool fn_CompareAStarNode(AStarNode* n1, AStarNode* n2)
{
return n1->_total > n2->_total;
}
//優先隊列
class AStarPriorityQueue
{
public:
AStarNode* Pop()
{
AStarNode* node = this->_heap.front();
pop_heap(this->_heap.begin(), this->_heap.end(), fn_CompareAStarNode);
this->_heap.pop_back();
return node;
}
void Push(AStarNode* node)
{
this->_heap.push_back(node);
push_heap(this->_heap.begin(), this->_heap.end(), fn_CompareAStarNode);
}
void UpdateNode(AStarNode* node)
{
vector<AStarNode*>::iterator iter;
for (iter = this->_heap.begin(); iter != this->_heap.end(); iter++)
{
if ((*iter)->_xy == node->_xy)
{
push_heap(this->_heap.begin(), iter + 1, fn_CompareAStarNode);
return;
}
}
}
bool IsEmpty()
{
return this->_heap.empty();
}
void Clear()
{
_heap.clear();
}
private:
vector<AStarNode*> _heap;
};
class AStar
{
public:
//返回到目標點的 估計值
virtual inline float GetNodeHeuristic(const PointU& source, const PointU& target)
{
PointU d = target - source;
return (d.x > d.y ? (d.x + DND_ASTAR_SQRT_2_SUB_1 * d.y) : (d.y + DND_ASTAR_SQRT_2_SUB_1 * d.x));
}
//相鄰點之間的代價,可以選擇忽略第一個參數,從而只返回第二個點的代價
virtual float GetCostNodeToNode(const PointU& source, const PointU& target) = 0;
//搜索(傳入失敗路徑,會返回相對最佳的路徑)
bool Search(const PointU& source, const PointU& target, list<PointU>& list_path, list<PointU>* fail_path = NULL)
{
if (target.x > 100000
|| target.y > 100000
|| source.x > 100000
|| source.y > 100000)
{
debug_err(String::Format(256, L"DND: AStar傳入的座標值過大,請檢查參數: [%u, %u] -> [%u, %u]",
source.x, source.y,
target.x, target.y).GetWcs());
return false;
}
/*else
debug_info(String::Format(256, L"DND: AStar傳入的座標爲: [%u, %u] -> [%u, %u]",
source.x, source.y,
target.x, target.y).GetWcs());*/
//清空
_bankCur = 0;
_hashMasterNode.clear();
_queueOpen.Clear();
//起點插入Open表
AStarNode* node_start = _get_node(source);
node_start->_open = true;
node_start->_closed = false;
node_start->_cost = 0;
node_start->_total = GetNodeHeuristic(source, target);
node_start->_parent = NULL;
_queueOpen.Push(node_start);
AStarNode* best_node = NULL;
while (!_queueOpen.IsEmpty())
{
//取出 消耗值 最小的節點
best_node = _queueOpen.Pop();
//是目標節點
if (best_node->_xy == target)
{
//構造路徑(包含首尾節點)
while (best_node)
{
list_path.push_front(best_node->_xy);
best_node = best_node->_parent;
}
return true;
}
/////////////////////////////////////////////////////////////////////////
//你可以修改這裏 實現圖 的連接,這裏就不再實現了
//相鄰點
_check_connecting(1, 0, DND_ASTAR_COST_MIN, best_node, target);
_check_connecting(-1, 0, DND_ASTAR_COST_MIN, best_node, target);
_check_connecting(0, 1, DND_ASTAR_COST_MIN, best_node, target);
_check_connecting(0, -1, DND_ASTAR_COST_MIN, best_node, target);
//是否斜穿節點
if (_bSideling)
{
_check_connecting(1, 1, DND_ASTAR_COST_MIN_SQRT_2, best_node, target);
_check_connecting(-1, 1, DND_ASTAR_COST_MIN_SQRT_2, best_node, target);
_check_connecting(-1, -1, DND_ASTAR_COST_MIN_SQRT_2, best_node, target);
_check_connecting(1, -1, DND_ASTAR_COST_MIN_SQRT_2, best_node, target);
}
////////////////////////////////////////////////////////////////////////
best_node->_closed = true;
if (_hashMasterNode.size() > _maxNode)
{
if (fail_path)
{
//構造路徑(包含首尾節點)
while (best_node)
{
fail_path->push_front(best_node->_xy);
best_node = best_node->_parent;
}
}
return false;
}
}
if (fail_path)
{
//構造路徑(包含首尾節點)
while (best_node)
{
fail_path->push_front(best_node->_xy);
best_node = best_node->_parent;
}
}
return false;
}
AStar(bool sideling = true, UINT32 max_node = DND_ASTAR_SEARCH_MAX)
{
_arrNodeBank = new AStarNode[max_node];
_bSideling = sideling;
_maxNode = max_node;
}
virtual ~AStar()
{
delete[] _arrNodeBank;
}
private:
AStarNode* _get_node(PointU xy)
{
//如果在 主列表 直接返回,如果沒有則從節點銀行構造一個
UINT64 index = xy.ToIndex2();
AStarNode*& node = _hashMasterNode[index];
if (node)
{
return node;
}
else
{
node = &_arrNodeBank[_bankCur++];
node->_xy = xy;
node->_open = false;
node->_closed = false;
return node;
}
}
void _check_connecting(int dx, int dy, float cost, AStarNode* best_node, const PointU& target)
{
PointU new_xy;
new_xy.x = int(best_node->_xy.x) + dx;
new_xy.y = int(best_node->_xy.y) + dy;
//防止越界
if (new_xy.x > 100000 || new_xy.y > 100000)
return;
//
if (best_node->_parent == NULL ||
best_node->_parent->_xy != new_xy)
{
//代價
float g = GetCostNodeToNode(best_node->_xy, new_xy);
//如果代價 等於0,說明此節點不可通行
if (g == 0)
return;
AStarNode* actual_node = _get_node(new_xy);
//不在close表
if(actual_node->_closed == false)
{
if (actual_node->_open)
{
//上一個點代價 + 移到自己的代價
float new_cost = best_node->_cost + g*cost;
//總代價,自己代價 + 估計代價
float new_total = new_cost + GetNodeHeuristic(new_xy, target);
//新的 代價更小 才刷新
if (new_total < actual_node->_total)
{
actual_node->_parent = best_node;
actual_node->_cost = new_cost;
actual_node->_total = new_total;
_queueOpen.UpdateNode(actual_node);
}
}
else
{//不在open表,則加入
actual_node->_parent = best_node;
actual_node->_cost = best_node->_cost + g*cost;
actual_node->_total = actual_node->_cost + GetNodeHeuristic(new_xy, target);
_queueOpen.Push(actual_node);
actual_node->_open = true;
}
}
}
}
//主列表
unordered_map<UINT64, AStarNode*> _hashMasterNode;
//Open表
AStarPriorityQueue _queueOpen;
//節點銀行
AStarNode* _arrNodeBank;
//每次搜索前置0
UINT32 _bankCur;
//能否斜穿
bool _bSideling;
//遍歷節點上限
UINT32 _maxNode;
};