A*启发式算法模拟实现

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

如 ：

A  B  C 

E  F  G  六个点  我们要由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<<")"*/<<"   ";elsecout<<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);}elsereturn;}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 4void   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<<"   ";}elsecout<<"0"<<"   ";}}getchar();}