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