游戏寻路——A*算法

A*其实是一种思想，采用启发式遍历来得到起始点到目标点的路径。

A*的基本思想：f(n) = g(n) + h(n);

其中，g(n)是从起始点到当前点V的实际代价，h(n)表示当前点V到目标点的代价估计，一般用两点间x，y坐标差的平方和表示。

则f(n)值就表示该点的估价值了，f值越低表示该点越优。

最佳路径就是一系列最优节点的集合。

 

算法描述：

1.把起始点加入到开启列表，开启列表表示将要搜索的点的集合。

2.重复如下的工作：

a.寻找开启列表中F值最低的点，我们把它称为当前点V。

b.把它添加到关闭列表，并把它从开启列表删除。

c.对它周围相邻的8个点：

*如果它在关闭列表或者不可通过，略过它

*如果它既不在开启列表，也不在关闭列表，则把它添加进开启列表，并把当前点V作为其父节点，记录改点的F,G,H值。

*如果其已经在开启列表，则比较该点新的G值和其旧的G值，如果新的G值要小，则更新该点的父节点为节点V，从新计算F,G,H值。(以上往开启列表添加节点或是更新节点的数据都要从新对开启列表排序)

d.停止

*如果把目标点添加进了关闭列表，这时候路径找到

  或者*没有找到目标点，但是开启列表已经空了，这时候表示路径不存在。

3.保存路径。从目标点开始找其父节点，直到移动到起始点。这就是路径。

 

具体代码：

#ifndef _ASTAR_H#define _ASTAR_H#include <iostream>#include <vector>#include <set>#define SAFE_DELETE(p) {if(p) {delete (p);(p) = NULL;}}#define SAFE_DELETE_ARRAY(p) {if(p) {delete [] (p); (p) = NULL;}}struct Point{Point(){ x = 0;y = 0;} Point(int xx,int yy){x = xx;y = yy;}bool operator == (const Point &rh){ return (x == rh.x) && (y == rh.y);} int x;int y;};struct Node{Node(){pt = Point(0,0);parent = NULL;f = g = h = 0;}Point pt;int f;int g;int h;Node *parent;};struct NodeSort{bool operator()(Node *lh,Node *rh) const{return lh->f < rh->f;}};class Astar{public:Astar(){m_Map = NULL;#ifdef _DBUG_newcnt = 0;#endif}Astar(Point start,Point end): m_startPt(start),m_endPt(end){m_Map = NULL;#ifdef _DBUG_newcnt = 0;#endif}~Astar(){FreeNodes();SAFE_DELETE_ARRAY(m_Map);}void SetStartPt(Point pt){m_startPt = pt;}void SetEndPt(Point pt){m_endPt = pt;}void SetLenX(const int x) { m_lenx = x;}void SetLenY(const int y) { m_leny = y;}void SetLen(const int x,const int y){m_lenx = x;m_leny = y;}void SetMap(const char* a){int len = m_lenx * m_leny * sizeof(char);SAFE_DELETE_ARRAY(m_Map);m_Map = new char[len];memcpy(m_Map,a,len);}void SetMap(const char* a,int lenx,int leny){SetLen(lenx,leny);SetMap(a);}void ChildNodes(Node *bestnode);void StartSearch();void UpdateNode(Node *bestnode,int x,int y);bool IsDestNode(Node *node);Node* GetBestNode();bool IsInOpen(Node *node);bool IsInOpen(int x,int y);bool IsInClosed(Node *node);bool IsInClosed(int x,int y);Node* GetNodeInOpen(int x,int y);Node* GetNodeInClosed(int x,int y);void FreeNodes();std::vector<Point> GetPath(){return m_Path;}void PrintPath(){for(int i = 0;i != m_Path.size(); ++i){printf("%d : %d,%d/n",i,m_Path[i].x,m_Path[i].y);}}void PrintPath2(){memset(m_Map,' ',m_lenx * m_leny * sizeof(char));for(int i = 0;i != m_Path.size(); ++i){*(m_Map + m_Path[i].y * m_lenx + m_Path[i].x) = '*';}for(int i = 0;i != m_leny; ++i){for(int j = 0;j != m_lenx; ++j){printf("%c",*(m_Map + i * m_lenx + j));}printf("/n");}}private:Point m_startPt;Point m_endPt;int m_lenx;int m_leny;char *m_Map;//arraystd::vector<Point>m_Path;std::set<Node*,NodeSort> m_Open;typedef std::set<Node*,NodeSort>::iterator OpenItr;std::set<Node*,NodeSort> m_Closed;typedef std::set<Node*,NodeSort>::iterator ClosedItr;void SetPath(Node *node);bool CanCross(Point pt){if(pt.x >= m_lenx || pt.x < 0)return false;if(pt.y >= m_leny || pt.y < 0)return false;return *(m_Map + pt.y * m_lenx + pt.x) == 0;}bool CanCross(int x,int y){bool can = CanCross(Point(x,y));return can;}static const int TILESIZE = 1;#ifdef _DBUG_int newcnt;#endif};#endif 

 

#include "Astar.h"void Astar::StartSearch(){FreeNodes();Node *node = new Node;Node *bestnode;#ifdef _DBUG_newcnt += 1;#endifnode->g = 0;node->h = (m_endPt.x - m_startPt.x) * (m_endPt.x - m_startPt.x)+ (m_endPt.y - m_startPt.y) * (m_endPt.y - m_startPt.y);node->f = node->g + node->h;node->pt = m_startPt;m_Open.insert(node);while(!m_Open.empty()){bestnode = GetBestNode();m_Open.erase(m_Open.begin());m_Closed.insert(bestnode);if(IsDestNode(bestnode)){break;}ChildNodes(bestnode);}SetPath(bestnode);}void Astar::ChildNodes(Node *bestnode){int x,y;//l tx = bestnode->pt.x - TILESIZE;y = bestnode->pt.y - TILESIZE;UpdateNode(bestnode,x,y);//tx = bestnode->pt.x;y = bestnode->pt.y - TILESIZE;UpdateNode(bestnode,x,y);//r tx = bestnode->pt.x + TILESIZE;y = bestnode->pt.y - TILESIZE;UpdateNode(bestnode,x,y);//rx = bestnode->pt.x + TILESIZE;y = bestnode->pt.y;UpdateNode(bestnode,x,y);//r bx = bestnode->pt.x + TILESIZE;y = bestnode->pt.y + TILESIZE;UpdateNode(bestnode,x,y);//bx = bestnode->pt.x;y = bestnode->pt.y + TILESIZE;UpdateNode(bestnode,x,y);//l bx = bestnode->pt.x - TILESIZE;y = bestnode->pt.y + TILESIZE;UpdateNode(bestnode,x,y);//lx = bestnode->pt.x - TILESIZE;y = bestnode->pt.y;UpdateNode(bestnode,x,y);}void Astar::UpdateNode(Node *bestnode,int x,int y){Node *nodeOpen = GetNodeInOpen(x,y);Node *nodeClosed = GetNodeInClosed(x,y);bool inopen = (nodeOpen != NULL);bool inclosed = (nodeClosed != NULL);bool cross = CanCross(x,y);if(inclosed || !cross) {return;}else if(!inclosed && !inopen){Node *child = new Node;#ifdef _DBUG_newcnt += 1;#endifchild->h = (m_endPt.x - x) * (m_endPt.x - x)+ (m_endPt.y - y) * (m_endPt.y - y);child->g = bestnode->g + TILESIZE;child->f = child->g + child->h;child->pt.x = x;child->pt.y = y;child->parent = bestnode;m_Open.insert(child);}else if(inopen){Node *child = nodeOpen;if(bestnode->g + TILESIZE < child->g){child->g = bestnode->g + TILESIZE;child->parent = bestnode;}}}bool Astar::IsDestNode(Node *node){return node->h == 0;}Node* Astar::GetBestNode(){OpenItr it = m_Open.begin();if(it != m_Open.end()){return *it;}return NULL;}bool Astar::IsInOpen(Node *node){bool have = m_Open.count(node);return have;}bool Astar::IsInOpen(int x,int y){return GetNodeInOpen(x,y) != NULL;}bool Astar::IsInClosed(Node *node){bool have = m_Closed.count(node);return have;}bool Astar::IsInClosed(int x,int y){return GetNodeInClosed(x,y) != NULL;}Node* Astar::GetNodeInOpen(int x,int y){for(OpenItr it = m_Open.begin(); it != m_Open.end(); ++it){if((*it)->pt == Point(x,y)){return *it;}}return NULL;}Node* Astar::GetNodeInClosed(int x,int y){for(ClosedItr it = m_Closed.begin(); it != m_Closed.end(); ++it){if((*it)->pt == Point(x,y)){return *it;}}return NULL;}void Astar::SetPath(Node *node){Point pt;m_Path.clear();while((node->pt.x != m_startPt.x) || (node->pt.y != m_startPt.y )){pt.x = node->pt.x;pt.y = node->pt.y;m_Path.push_back(pt);node = node->parent;}}void Astar::FreeNodes(){for(OpenItr it = m_Open.begin(); it != m_Open.end(); ++it){Node *node = *it;delete node;}for(ClosedItr it = m_Closed.begin(); it != m_Closed.end(); ++it){Node *node = *it;delete node;}#ifdef _DBUG_std::cout << "new times :" << newcnt << "/tfree times :" << m_Open.size() + m_Closed.size() << std::endl;#endifm_Open.clear();m_Closed.clear();} 

 

测试代码：

#include "Astar.h"#define LEN 5char Block[LEN][LEN] ={{0,0,0,0,0},{0,0,1,0,0},{0,0,1,0,0},{0,0,1,0,0},{0,0,0,0,0}};int main(){Point start(1,2);Point end(3,2);Astar astar(start,end);astar.SetMap((char*)Block,LEN,LEN);astar.StartSearch();astar.PrintPath();astar.PrintPath2();return 0;}