A*启发式算法模拟实现

A*算法的核心是估值函数，通过估值函数来确定代价，永远只保留经过同一个点时代价最小的那条路径。

如 ：

A B C

E FG 六个点 我们要由A到达 C，可有的路径是 A-B-C、 A-E-F-B-C、A-E-B-C等 这几个路径都经过B，暂且把，每相邻两点的直接的代价都定为1，对角线的定为2，那么上面三个路径经过B的代价 分别是 1、3、3，由此我们可以只需存一个较小值就可以了，因为这个代价无疑是最小的。我们途径每一个的节点时都预判下这个代价是不是当前最小的，是最小的记下来，不是就不管它，这就是A*算法的精髓！ 下面是代码模拟实现，就不解释了

//写一个自己实现的A*搜索算法 // #include "stdafx.h" #include <iostream> #include <vector> #include <map> #include <algorithm> #include <assert.h> using namespace std; const int nMapWidth = 8; const int nMapHeight = 8; struct Node { int nEnable; int nNodeMark; int nValue; int x; int y; Node():nEnable(0),nNodeMark(0),nValue(0),x(0),y(0){}; }; std::map<int ,int > m_OpenList; std::map<int ,int > m_CloseList; std::vector<int> m_KeyList; Node m_MapNode[nMapWidth][nMapHeight]; //计算openlist中靠前节点周围的节点 void ComputerRound(int curx,int cury); //将一个新的节点加入到OPenList中 void AddNodeToOpenList(Node* pNode,int nNum); //打印地图 void Print(Node pNode[][nMapHeight]); void Print(Node pNode[][nMapHeight]) { for (int n = 0; n < nMapWidth; ++n) { for(int m = 0; m < nMapHeight; ++m) { if (m == 0) cout<<endl; if (n == 0) cout<<pNode[n][m].nEnable/*<<"("<<" "<<pNode[n][m].nNodeMark<<")"*/<<" "; else cout<<pNode[n][m].nEnable/*<<"("<<pNode[n][m].nNodeMark<<")"*/<<" "; } } } //将一个新的节点加入到OPenList中 void AddNodeToOpenList(Node* pNode,int nNum) { if(!pNode || !(pNode->nEnable)) return; if (m_OpenList.empty()) { m_OpenList[pNode->nNodeMark] = nNum; m_KeyList.push_back(pNode->nNodeMark); } else { std::map<int,int>::iterator itr = m_OpenList.find(pNode->nNodeMark); if (itr == m_OpenList.end()) { std::map<int,int>::iterator itrQ = m_CloseList.find(pNode->nNodeMark); if (itrQ != m_CloseList.end()) { if (nNum < (*itrQ).second) { m_CloseList.erase(itrQ); } else return; } else { m_OpenList[pNode->nNodeMark] =nNum; m_KeyList.push_back(pNode->nNodeMark); } } else { if ((*itr).second > nNum) { m_OpenList[pNode->nNodeMark] =nNum; } } } } //将openlist中的一个靠前的节点展开到CloseList中 void AddNodeToCloseList(Node* pNode,int nNum) { if(!pNode) return; if (m_CloseList.empty()) { m_CloseList[pNode->nNodeMark] = nNum; ComputerRound(pNode->x,pNode->y); } else { std::map<int,int>::iterator itrB = m_CloseList.find(pNode->nNodeMark); if(itrB == m_CloseList.end()) { std::map<int,int>::iterator itrO = m_OpenList.find(pNode->nNodeMark); if (itrO != m_OpenList.end()) { if ((*itrO).second < nNum) { return; } else { std::vector<int>::iterator itrK = std::find(m_KeyList.begin(),m_KeyList.end(),(*itrO).first); if (itrK != m_KeyList.end()) m_KeyList.erase(itrK); m_OpenList.erase(itrO); } } else { m_CloseList[pNode->nNodeMark] += nNum; ComputerRound(pNode->x,pNode->y); } } else { if (nNum < m_CloseList[pNode->nNodeMark]) m_CloseList[pNode->nNodeMark] = nNum; } } } //探索是否该节点可行 void TryNode(int nx,int ny,int curx, int cury) { if (nx < 0 || ny < 0 || nx >= nMapWidth || ny >= nMapWidth) return; if (m_MapNode[nx][ny].nEnable == 0) return; int nNum = abs(nx - curx) + abs(ny - cury); std::map<int,int>::iterator itr = m_CloseList.find(m_MapNode[curx][cury].nNodeMark); if(itr != m_CloseList.end()) nNum += (*itr).second; AddNodeToOpenList(&(m_MapNode[nx][ny]),nNum); } #define DesX 3 #define DesY 4 void ComputerRound(int curx,int cury) {//对每一个当前节点执行以下操作 TryNode(curx,cury+1,curx,cury); TryNode(curx+1,cury,curx,cury); TryNode(curx+1,cury+1,curx,cury); TryNode(curx-1,cury,curx,cury); TryNode(curx-1,cury-1,curx,cury); TryNode(curx-1,cury+1,curx,cury); TryNode(curx,cury-1,curx,cury); TryNode(curx+1,cury-1,curx,cury); } void main() { int nMark = 0; for (int n = 0; n < nMapWidth; ++n) { for(int m = 0; m < nMapHeight; ++m) { if (n != 2 || m == 3 || m == 1) m_MapNode[n][m].nEnable = 1; if (n == 4 && (m != 6)) m_MapNode[n][m].nEnable = 0; m_MapNode[n][m].nNodeMark = ++nMark; m_MapNode[n][m].x = n; m_MapNode[n][m].y = m; } } Print(m_MapNode); AddNodeToCloseList(&(m_MapNode[1][1]),0); std::map<int,int>::iterator itr; while(!m_KeyList.empty()) { itr = m_OpenList.find(*(m_KeyList.begin())); int nV = (*itr).first; int nNum =(*itr).second; std::vector<int>::iterator itrK = std::find(m_KeyList.begin(),m_KeyList.end(),(*itr).first); if (itrK != m_KeyList.end()) m_KeyList.erase(itrK); itr = m_OpenList.erase(itr); AddNodeToCloseList(&(m_MapNode[(nV-1)/nMapWidth][(nV-1)%nMapWidth]),nNum); } cout<<endl; cout<<endl; std::map<int,int>::iterator itrC; for (int n = 0; n < nMapWidth; ++n) { for(int m = 0; m < nMapHeight; ++m) { if (m == 0) cout<<endl; if (m == 1 && n == 1) { cout<<"ST"<<" "; continue; } itrC = m_CloseList.find(m_MapNode[n][m].nNodeMark); if (itrC != m_CloseList.end()) { cout<<(*itrC).second<<" "; } else cout<<"0"<<" "; } } getchar(); }