A* 寻路算法模拟实现 C++ 可运行

http://wenku.baidu.com/view/228633c45fbfc77da269b169.html

#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);    
}


int _tmain(int argc, _TCHAR* argv[])
{
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();
return 0;
}