#include "AStar.h"
//准备一张 开标 和 闭表
std::vector
std::vector
#define _ABS(v) ((v) < 0 ? -(v):(v))
#define _H_V(c,e) (_ABS((c)%w-(e)%w)+_ABS((c)/w - (e)/w))
//找到最小的F值的下标
unsigned int CAStar::GetMinF()
{
unsigned int i = 0;
for(unsigned int j = 1; j < open.size(); ++j)
{
if(open[i]._g + open[i]._h > open[j]._g + open[j]._h)
{
i = j;
}
}
return i;
}
//看下标的节点 是否在 开表中
int CAStar::Inopen(int index)
{
for(unsigned int i = 0; i < open.size(); ++i)
{
if(index == open[i]._cur)
return i;
}
return -1;
}
int CAStar::InClose(int index)
{
for(unsigned int i = 0; i < close.size(); ++i)
{
if(index == close[i]._cur)
return i;
}
return -1;
}
int CAStar::Findpath(const char* map,//传进来的地图
int w,int h,//地图宽高
int b,int e,//起始点 和 终点
int* path) //寻路路径
{
if(!map || w < 1 || h < 1 || b < 0 || b >= w * h || e < 0 || e >= w * h || !path)
{
return -1;
}
//八方向 判断
const int offset_x[] = {0,1,-1,-1};
const int offset_y[] = {-1,-1};
//清空表
open.clear();
close.clear();
//把起点放入开表
_NODE begin = {b,_H_V(b,e)};
open.push_back(begin);
//循环找路
while(!open.empty())
{
//得到开表中 F值 最小的节点下标
unsigned int min_f = GetMinF();
//得到开表中F值最小的节点
_NODE min_f_n = open[min_f];
//将其中开表中删除
open.erase(open.begin() + min_f);
//放入闭表中
close.push_back(min_f_n);
//获取坐标
int cx = min_f_n._cur % w;
int cy = min_f_n._cur / w;
//循环当前节点的 八方向 判断是否有通路
for(int i = 0; i < 8; ++i)
{
int dx = cx + offset_x[i];
int dy = cy + offset_y[i];
//地图中判断
if(dx >= 0 && dx < w && dy >= 0 && dy < h)
{
//找到终点了
int di = dx + dy * w;
if(di == e)
{
//终点入表
_NODE node = {di,min_f_n._cur,0};
close.push_back(node);
//最后放入的节点下标
int index = close.size() - 1;
//循环得到路径
int pathlen = 0;
while (index != -1)
{
path[pathlen++] = close[index]._cur;
int j = index;
int k = close[index]._b;
if(k == -1)
break;
//效率优化点
while(1)
{
if(close[j]._cur == k)
break;
else
--j;
}
index = j;
}
return pathlen;
}
else
{
//不是障碍
if(map[di] > 0)
{
//不在闭表中
if(!InClose(di))
{
//不在开表中
int r = Inopen(di);
int curG = min_f_n._g + (i % 2 == 1 ? 7 : 5);
if(-1 == r)
{
_NODE node = {di,curG,_H_V(di,e)};
open.push_back(node);
}
else
{
//如果G值 小于 开表中的G 就替换
if(open[r]._g < curG)
{
open[r]._b = min_f_n._cur;
open[r]._g = curG;
}
}
}
}
}
}
}
}
return 0;
}
