C++二叉堆实现A*算法及方向优化

#include"my_map.h" #define HEAPSIZE 32768 //设置起点和终点 //x为从左往右数第几列，y为从上往下数第几行 short start_x = 0, start_y = 0;//起始坐标 short end_x = 499, end_y = 499;//终点坐标 //short end_x = 13, end_y = 5;//终点坐标 /*******************定义方格和二叉堆的数据结构*******************/ struct Grid//方格结构体 { short x, y;//方格坐标 short f, g, h; Grid *parent;//父节点 }; class OpenList//由若干方格对象组成的二叉堆 { public: OpenList(void); bool push_openlist(Grid *grid);//将一个方格放入堆 bool pop_openlist(Grid *grid);//从堆中取出f值最小的一个方格，即位置1 bool check_openlist(Grid *grid);//从堆中找出与给定方格的坐标相同的方格 bool decrease_g(Grid *grid);//将g值减小了的方格在堆中的位置重新调整 private: short size;////堆中的方格数量，第0个位置为空 Grid HeapGrid[HEAPSIZE];//堆的存储空间 short get_index(const Grid *grid);//根据给定方格的坐标找出它在堆中的位置，没找到则返回0 void percolate_up(short hole);//将给定方格在堆中的位置重新调整 }; OpenList::OpenList(void)//将起点信息事先写入第1个位置 { size = 1; HeapGrid[1].x = start_x; HeapGrid[1].y = start_y; HeapGrid[1].f = 0; HeapGrid[1].g = 0; HeapGrid[1].h = 0; HeapGrid[1].parent = NULL; } inline short OpenList::get_index(const Grid *grid) { for (short i = 1; i <= size; i++) if ((HeapGrid[i].x == grid->x) && (HeapGrid[i].y == grid->y)) return i; return 0; } inline void OpenList::percolate_up(short hole) { if (hole == 1) return; HeapGrid[0] = HeapGrid[hole]; for (; (HeapGrid[0].f <= HeapGrid[hole / 2].f) && (hole > 1); hole /= 2) HeapGrid[hole] = HeapGrid[hole / 2];//深度优先遍历，相同的几个数应服从先入后出规则 HeapGrid[hole] = HeapGrid[0]; } bool OpenList::push_openlist(Grid *grid) { if (size == HEAPSIZE)//堆已满，放入操作无效 return false; size++; HeapGrid[size] = *grid; percolate_up(size); return true; } bool OpenList::pop_openlist(Grid *grid) { if (size == 0)//堆已空，取出操作无效 return false; short hole = 1; HeapGrid[0] = HeapGrid[1]; for (short child = 2; child <= size; child = hole << 1) { if ((HeapGrid[child].f >= HeapGrid[child + 1].f) && (child < size))//优先取出右孩子 { HeapGrid[hole] = HeapGrid[child+1]; hole = child + 1; } else//若只有一个孩子则一定是左孩子 { HeapGrid[hole] = HeapGrid[child]; hole = child; } } HeapGrid[hole] = HeapGrid[size];//记得把空穴补上 size--; *grid = HeapGrid[0]; return true; } bool OpenList::check_openlist(Grid *grid) { short num = get_index(grid); if (num == 0) return false; else { *grid = HeapGrid[num]; return true; } } bool OpenList::decrease_g(Grid *grid) { short num = get_index(grid); if (num == 0) return false; HeapGrid[num] = *grid; percolate_up(num); return true; } /*******************A*算法后的路径优化*******************/ //检测两点之间是否有障碍物 //(x1,y1)为始端坐标，(x2,y2)为末端坐标 bool has_obstacle(short x1, short y1, short x2, short y2) { if (ABS(x1 - x2) <= 1 && ABS(y1 - y2) <= 1) return 0; char sgnx, sgny; float yL = y1;//方格左边界的y坐标 float yR;//方格右边界的y坐标 float fx1 = x1 + 0.5f, fy1 = y1 + 0.5f; float fx2 = x2 + 0.5f, fy2 = y2 + 0.5f; if (x1 < x2) sgnx = 1; else sgnx = -1; if (y1 < y2) sgny = 1; else sgny = -1; for (short x = x1; (x >= MIN(x1, x2)) && (x <= MAX(x1, x2)); x += sgnx) { if (((sgnx == 1) && (x < x2)) || ((sgnx == -1) && (x > x2))) yR = fy1 + (fy2 - fy1)*((float)x + (sgnx + 1) / 2 - fx1) / (fx2 - fx1);//直线两点式 else yR = y2; for (short y = (short)yL; (y >= MIN((short)yL, (short)yR)) && (y <= MAX((short)yL, (short)yR)); y += sgny) if (test_obstacle(x, y) == 2) return 1; yL = yR; } return 0; } //将A*算法规划的路径优化后输出 void output(Grid *grid) { //直接展示A*算法结果 // while (grid != NULL) // { // draw_point(grid->x, grid->y); // grid = grid->parent; // } // show(); Grid *p = grid;//终点方格 short count = 0, num = 0;//用于固定点在探路过程中的移动 short x = start_x, y = start_y;//从起点出发的固定坐标 short node1x = start_x, node1y = start_y;//固定点一步一步走直线时的直线起点 short node2x = start_x, node2y = start_y;//固定点一步一步走直线时的直线终点 while (1) { if (!has_obstacle(x, y, end_x, end_y))//终点就在眼前 { draw_line(x, y, end_x, end_y); break; } while (has_obstacle(grid->x, grid->y, x, y)) grid = grid->parent; if (ABS(grid->x - x) <= 1 && ABS(grid->y - y) <= 1)//两点距离过近直接连线 { node1x = node2x = x = grid->x; node1y = node2y = y = grid->y; draw_point(x, y); grid = p; continue; } if ((grid->x == node2x) && (grid->y == node2y))//方向没有变，再往前走一步 { count++; x = node1x + short(float((node2x - node1x)*count) / float(num) + 0.5f); y = node1y + short(float((node2y - node1y)*count) / float(num) + 0.5f); draw_point(x, y); if (count == num)//走到跟前了，重新规划路线 { node1x = node2x = x = grid->x; node1y = node2y = y = grid->y; } } else//还没走的跟前方向就变了，重新规划路线 { node1x = x; node1y = y; node2x = grid->x; node2y = grid->y; num = MAX(ABS(node2x - node1x), ABS(node2y - node1y)); count = 0; } grid = p; } show(); } /*******************A*算法以及主程序*******************/ //生成当前方格的相邻方格 //root为当前方格，grid为相邻方格，i为从右邻开始逆时针计数的序号，从1到8 Grid *generate_neighbor(Grid *root, char i) { Grid *grid = new Grid; grid->x = root->x; grid->y = root->y; switch (i) { case 1:grid->x++; break; case 2:grid->x++; grid->y--; break; case 3:grid->y--; break; case 4:grid->x--; grid->y--; break; case 5:grid->x--; break; case 6:grid->x--; grid->y++; break; case 7:grid->y++; break; case 8:grid->x++; grid->y++; break; default:break; } return grid; } //检查输入坐标是否在关闭列表中或是否为终点或不可抵达 char check_valid(short x, short y) { if (test_obstacle(x, y)) return 1;//障碍物 if ((x == end_x) && (y == end_y)) return 2;//终点 if (test_closelist(x, y)) return 3;//在关闭列表中 return 0;//新坐标 } short function_g(char x) { if (x % 2) return 10;//标准为10，为鼓励每次判断时优先走直线而改成9。 else return 14; } //目标点与终点的距离 short end_distance(short point_x, short point_y) { short dx = ABS(point_x - end_x); short dy = ABS(point_y - end_y); short ans = 14 * MIN(dx, dy) + 10 * ABS(dx - dy); return ans; } int main() { bool success = 0; OpenList heap;//把起点设为当前方格，加入开启列表 Grid *P;//当前方格 Grid *Normal;//相邻方格 char status;//用于判断方格状态 read_map(); while (1) { P = new Grid; success = heap.pop_openlist(P); if (!success) exit(1);//开启列表的堆已空 add_closelist(P->x, P->y);//将当前方格画灰色，视为移到关闭列表 for (char i = 1; i <= 8; i++)//遍历当前方格的8个相邻方格 { Normal = generate_neighbor(P, i);//临时生成的存储空间 status = check_valid(Normal->x, Normal->y); if (status == 1)//障碍物 { delete Normal; Normal = NULL; continue; } if (status == 3)//在关闭列表中 continue; if (status == 2)//终点 { Normal->parent = P; output(P); return 0; } success = heap.check