A*寻路C++ 代码
|
#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; } （编辑：李大同） 【声明】本站内容均来自网络，其相关言论仅代表作者个人观点，不代表本站立场。若无意侵犯到您的权利，请及时与联系站长删除相关内容! |