A星寻路

#ifndef KASTAR_H
#define KASTAR_H
#include <iostream>
#include <map>
#include <vector>
using namespace std;
enum eAStarBarrier
{
eASB_None,
eASB_Yes,
eASB_Start,//起始点
eASB_End,//结束点
};
enum enum_FindRes
{
enum_FindPathFailed,
enum_FindPathSuccess,
enum_FindPathContinue,
};
struct PosInfo
{
int x;
int y;
int g;//g值
int f;//f值
int h;//h值
PosInfo *parent;//父结点
PosInfo(){
x = 0;
y = 0;
g = 0;
f = 0;
h = 0;
parent = NULL;
}
};
struct PosValue
{
PosInfo *pos;
bool operator <(const PosValue &o)const
{
if (pos->f == o.pos->f)
{
return (long)pos < (long)o.pos;
}
return pos->f  < o.pos->f;
}
PosValue(PosInfo *pos)
{
this->pos = pos;
}
};
typedef pair<int,int> PosPair;
typedef map<PosPair,PosInfo *> Map_PosInfo;
typedef vector<PosInfo*> Vec_PosInfo;
typedef map<PosValue,PosInfo*> Map_PosValue;
typedef pair<Map_PosValue::iterator,bool > Map_PosValue_Ret;
class AStar
{
public:
PosInfo *m_startPos;
PosInfo *m_endPos;
int         m_srcX;
int         m_srcY;
int         m_destX;
int         m_destY;
int         m_minX;
int         m_maxX;
int         m_minY;
int         m_maxY;
int         m_yLimit;
int         m_xLimit;
int         **m_pointInfo;
PosInfo *m_optimumPos;//最优点
Map_PosValue m_mvOpenList;//为了找到最优点的开放列表
Map_PosInfo m_openList;//开放列表
Map_PosInfo m_closeList;//关闭列表
vector<PosInfo *> m_trackALiveList;//寻路跟踪列表,不包括死路
vector<PosInfo *> m_trackAllList;//寻路跟踪列表,包括死路点
public:
int AddOpenPos(PosInfo *pos)
{
m_openList.insert(Map_PosInfo::value_type(PosPair(pos->x,pos->y),pos));
m_mvOpenList.insert(Map_PosValue::value_type(PosValue(pos),pos));
FindOptimumpos();
return 0;
}
PosInfo *FindOptimumpos()
{
m_optimumPos = NULL;
if (m_mvOpenList.size() > 0)
{
Map_PosValue::iterator iterV = m_mvOpenList.begin();
m_optimumPos = iterV->second;
}
return m_optimumPos;
}
int ChangeFValue(PosInfo *pos)
{
Map_PosValue::iterator iterv = m_mvOpenList.find(PosValue(pos));
if (iterv != m_mvOpenList.end())
{
m_mvOpenList.erase(iterv);
fFunction(pos);
m_mvOpenList.insert(Map_PosValue::value_type(PosValue(pos),pos));
FindOptimumpos();
return 0;
}
return 1;
}
int DelOpenPos(PosInfo *pos)
{
m_openList.erase(PosPair(pos->x,pos->y));
m_closeList.insert(Map_PosInfo::value_type(PosPair(pos->x,pos->y),pos));
Map_PosValue::iterator iterV = m_mvOpenList.find(PosValue(pos));
if (iterV != m_mvOpenList.end())
{
m_mvOpenList.erase(iterV);
}
if (pos == m_optimumPos)
{
FindOptimumpos();
}
return 0;
}
AStar();
~AStar();
int printInit(PosInfo *pos);
int printTrack(PosInfo *pos);
int hFuncion(PosInfo *pos)
{
return pos->h = 10*(abs(pos->x-m_destX) + abs(pos->y-m_destY));
}
int hasFindFunc(PosInfo *pos)
{
return pos->x == m_destX  && pos->y == m_destY;
}
int fFunction(PosInfo *pos)
{
return pos->f = pos->h + pos->g;
}
void hfFunction(PosInfo *pos)
{
hFuncion(pos);
fFunction(pos);
}
int gfunction(int i,int j)
{
i = abs(i);
j = abs(j);
if(i>1 || j>1 ) return 10000;
switch(i+j)
{
case 0:
return 0;
case 1:
return 10;
case 2:
return 14;
}
return 10000;
}
int init();
int InitByTable();
int FindPath();//A星寻路
inline bool IsValid(int x,int nMin,int nMax)
{
return x >= nMin && x < nMax;
}
inline bool IsValidX(int x)
{
return IsValid(x,m_minX,m_maxX);
}
inline bool IsValidY(int y)
{
return IsValid(y,m_minY,m_maxY);
}
inline bool IsValidPos(int x,int y)
{
return IsValidX(x) && IsValidY(y) && m_pointInfo[x][y] != eASB_Yes;
}
int clear();
};

#endif

#include "AStar.h"
#include <stdio.h>
#include <algorithm>

AStar::AStar()
{
m_srcX = 0;
m_srcY = 0;
m_destX = 0;
m_destY = 0;
m_minX = 0;
m_minY = 0;
m_maxX = 0;
m_maxY = 0;
m_pointInfo = NULL;
m_optimumPos = NULL;
}
AStar::~AStar()
{
clear();
}
const int MaxValue = 40;
int AStar::init()
{
m_yLimit = MaxValue;
m_xLimit = MaxValue;
m_pointInfo = new int *[m_xLimit];
for (int i = 0;i<m_xLimit;i++)
{
m_pointInfo[i] = new int[m_yLimit];
}
for (int i = 0;i<m_xLimit;i++)
{
for (int j = 0;j<m_yLimit;j++)
{
m_pointInfo[i][j] = 0;
}
}
int d = MaxValue/10;
int middle = MaxValue/2;
for(int i = middle-d; i<= middle+d; i++)
m_pointInfo[i][middle+2*d] = 1;
for(int i = middle; i< middle+2*d; i++)
m_pointInfo[middle-d][i] = 1;
for(int i = middle; i< middle+2*d; i++)
m_pointInfo[middle+d][i] = 1;
m_srcX = middle;
m_srcY = middle+d;
m_destX = middle;
m_destY = middle+3*d;
m_minX = 0;
m_minY = 0;
m_maxX = m_xLimit;
m_maxY = m_yLimit;
return 0;
}

int AStar::clear()
{
Map_PosInfo::iterator it;
for(it=m_closeList.begin();it!=m_closeList.end();++it)
delete it->second;
m_closeList.clear();
for(it=m_openList.begin();it!=m_openList.end();++it)
delete it->second;
m_openList.clear();
m_trackAllList.clear();
m_trackALiveList.clear();
return 0;
}
int AStar::printInit(PosInfo *endPos)
{
PosInfo *pos = endPos->parent;
map<PosPair,int> path;
while(pos && pos->parent)
{
path[PosPair(pos->x,pos->y)] = 1;
pos = pos->parent;
}
for (int j = 0;j<m_yLimit;j++)
{
for (int i = 0;i<m_xLimit;i++)
{
if (i==m_srcX && j == m_srcY)
{
printf("S");
}
else if(i==endPos->x && j == endPos->y)
{
printf("E");
}
else if(path[PosPair(i,j)] == 1)
{
printf("+");
}
else if(m_pointInfo[i][j] == 1)
{
printf("*");
}
else
{
printf(" ");
}
printf("  ");
}
printf("\n");
}
return 0;
}

int AStar::printTrack(PosInfo *endPos)
{
map<PosPair,int> path;
int j = 0;
for (vector<PosInfo *>::iterator it = m_trackAllList.begin();it!=m_trackAllList.end();it++)
{
PosInfo *pos = *it;
path[PosPair(pos->x,pos->y)] = j%26+1;
j++;
}
for (int j = 0;j<m_yLimit;j++)
{
for (int i = 0;i<m_xLimit;i++)
{
if (i==m_srcX && j == m_srcY)
{
printf("S");
}
else if(i==endPos->x && j == endPos->y)
{
printf("E");
}
else if(path[PosPair(i,j)])
{
printf("%c",path[PosPair(i,j)]+'a'-1);
}
else if(m_pointInfo[i][j] == 1)
{
printf("*");
}
else
{
printf(" ");
}
printf("  ");
}
printf("\n");
}
return 0;
}

int AStar::FindPath()//循环实现
{
m_startPos = new PosInfo();
m_startPos->x = m_srcX;
m_startPos->y = m_srcY;
hfFunction(m_startPos);
PosInfo *currentPos = m_startPos;
DelOpenPos(currentPos);
m_trackALiveList.push_back(currentPos);
m_trackAllList.push_back(currentPos);
do
{
for (int i = -1;i<2;i++)
{
for (int j = -1;j<2;j++)
{
if(i==0 && j==0)
{
continue;
}
if(!IsValidPos(currentPos->x+i,currentPos->y+j))
continue;
int g = gfunction(i,j);
PosInfo * openPos = NULL;
PosInfo * closePos = NULL;
Map_PosInfo::iterator openPosIter = m_openList.find(PosPair(currentPos->x+i,currentPos->y+j));
Map_PosInfo::iterator closePosIter = m_closeList.find(PosPair(currentPos->x+i,currentPos->y+j));

if(openPosIter != m_openList.end())
{
openPos = openPosIter->second;
}
if(closePosIter != m_closeList.end())
{
closePos = closePosIter->second;
continue;
}
else if (openPosIter == m_openList.end())
{
openPos = new PosInfo();
openPos->g = g;
openPos->g += currentPos->g;
openPos->x = currentPos->x + i;
openPos->y = currentPos->y + j;
openPos->parent = currentPos;
hfFunction(openPos);
AddOpenPos(openPos);
}
else if(currentPos->g + g < openPos->g)
{
openPos->g = currentPos->g + g;
openPos->parent = currentPos;
ChangeFValue(openPos);
}
}
}
int size = m_trackAllList.size();
if(m_optimumPos)
{
currentPos = m_optimumPos;
DelOpenPos(currentPos);
int m_openListSize = m_openList.size();
int  m_closeListSize = m_closeList.size();
m_trackALiveList.push_back(currentPos);
m_trackAllList.push_back(currentPos);
if(hasFindFunc(currentPos))
{
m_endPos = currentPos;
printInit(currentPos);
printf("\n--------------------------------------------------------------\n");
printTrack(currentPos);
return currentPos->f;
}
if(m_openList.size() == 0)
{
break;
}
}
else
{
break;
}
} while(true);
printInit(currentPos);
printTrack(currentPos);
return -1;
}

bool sortPosInfo(PosInfo *&pos1,PosInfo *&pos2)
{
return pos1->f < pos2->f;
}

#endif

#include <iostream>
using namespace std;
#include "AStar.h"

int main()
{
{

AStar p;
p.init();
p.FindPath();
cout << "over" << endl;
}
while(1);
}