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