A星算法(纯C实现)

PS：写漂亮的博客太麻烦了，今晚还有欧洲杯，我就懒了，但我会加入备注，如果发现代码逻辑有问题，请@我！！！（代码我自测过了，但是不一定全面）

#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>#define DIMENSION 2
#define DIRECTION 4
/** PS：我第一次写的时候row、col的下标是从1开始的，你能知道为什么我不用1开始吗？*（答案在最后哦，但是请先思考哦） */
#define POS(row, col, map_col) ((row)*(map_col)+(col))
#define ROW(pos, map_col)      ((pos)/(map_col))
#define COL(pos, map_col)      ((pos)%(map_col))
/** PS：问，我为什么heap的下标从1开始？ （答案在最后哦，但是请先思考哦）*/
#define LEFT(i)   (2*(i))
#define RIGHT(i)  (2*(i)+1)
#define PARENT(i) ((i)/2)typedef struct node_info_st {int g_score;int h_score;int f_score;int came_from;
} Node_Info;/** PS：这个目的很简单，就是为了方便，不让代码又臭又长。*/
int neighbor[DIRECTION][DIMENSION] = {{-1, 0}, /* Up */{1,  0},    /* Down */{0, -1},  /* Left */{0,  1}       /* Right */
};/** PS：用于判断row、col是否可用以及地图上这点是否可通行。*/
bool valid(bool **map, int row, int col, int map_row, int map_col) {return row >= 0 && row < map_row &&col >= 0 && col < map_col &&map[row][col];
}int get_h_score(int s_row, int s_col, int d_row, int d_col) {return abs(d_row - s_row) + abs(d_col - s_col);
}/** PS：我下面这段代码可能写的比较复杂，但是整体思路是维护一个f_score的最小堆，* 虽然min查找是快了，但是最小堆不好更新啊，所以，我加入了openhash记录pos与* 堆中位置的索引，这样更新就能通过O(logn）实现。* PS：1.最小堆中记录的是pos，而不是f_score！*     2.最小堆虽然不记录f_score，但是是通过f_score进行比较的！所以写了comp*       3.其实openset、openhash、info，openset_size应该要是全局变量的，不然代码* 就会像我一样，太丑了，但是我有强迫症！^_^，建议大家使用全局变量，但是在真实场景中* 注意全局变量的初始化，这点刷题的小伙伴可能会有体会吧！*       4.swap是为了更好的维护openhash* NOTE：如果有更好的方式请@我，谢谢！！！！*/
/* operator < */
bool comp(Node_Info *info, int pos1, int pos2) {return info[pos1].f_score < info[pos2].f_score;
}void swap(int *arr, int i, int j, int *openhash) {int tmp = arr[i];arr[i] = arr[j];arr[j] = tmp;openhash[arr[i]] = i;openhash[arr[j]] = j;
}void MIN_HEAP_heapify(int *heap, int heap_size, int i, Node_Info *info, int *openhash) {int l = LEFT(i);int r = RIGHT(i);int min = i;if (l <= heap_size && comp(info, heap[l], heap[min]))min = l;if (r <= heap_size && comp(info, heap[r], heap[min]))min = r;if (min != i) {swap(heap, i, min, openhash);MIN_HEAP_heapify(heap, heap_size, min, info, openhash);}
}int MIN_HEAP_extract_min(int *heap, int *heap_size, Node_Info *info, int *openhash) {int min = heap[1];heap[1] = heap[(*heap_size)--];MIN_HEAP_heapify(heap, *heap_size, 1, info, openhash);openhash[min] = 0;return min;
}void MIN_HEAP_update(int *heap, int i, int key, int *openhash, Node_Info *info) {heap[i] = key;openhash[key] = i;int p = PARENT(i);while (p && comp(info, heap[i], heap[p])) {swap(heap, p, i, openhash);i = p;p = PARENT(i);}
}void MIN_HEAP_insert(int *heap, int *heap_size, int key, int *openhash, Node_Info *info) {heap[++*heap_size] = INT_MAX;MIN_HEAP_update(heap, *heap_size, key, openhash, info);
}void reconstruct_path(Node_Info *info, int src, int map_col) {do {printf("(%d,%d) ", ROW(src, map_col), COL(src, map_col));src = info[src].came_from;} while (src != -1);printf("\n");
}void a_star(bool **map, int map_row, int map_col, int s_row, int s_col,int d_row, int d_col) {int *openset = NULL;int  openset_size = 0;int *openhash = NULL;Node_Info *info = NULL;int src_pos = POS(s_row, s_col, map_col);int dst_pos  = POS(d_row, d_col, map_col);openset = (int *)calloc(map_row * map_col, sizeof(int));openhash = (int *)calloc(map_row * map_col, sizeof(int));info = (Node_Info *)malloc(sizeof(Node_Info) * map_row * map_col);for (int r = 0; r < map_row; ++r) {for (int c = 0; c < map_col; ++c) {info[POS(r, c, map_col)].g_score = INT_MAX;info[POS(r, c, map_col)].h_score = get_h_score(r, c, map_row, map_col);info[POS(r, c, map_col)].came_from = -1;}}info[src_pos].g_score = 0;MIN_HEAP_insert(openset, &openset_size, src_pos, openhash, info);while (openset_size) {int min_pos = MIN_HEAP_extract_min(openset, &openset_size, info, openhash);if (min_pos == dst_pos)break;for (int i = 0; i < DIRECTION; ++i) {int cur_row = ROW(min_pos, map_col) + neighbor[i][0];int cur_col = COL(min_pos, map_col) + neighbor[i][1];int cur_pos = cur_row * map_col + cur_col;int tmp_g_score = info[min_pos].g_score + 1;/* 位置有效? 可通行? */if (!valid(map, cur_row, cur_col, map_row, map_col))continue;/** PS：根据Wiki的说法，只要h(）是一致的，能保证每个节点只会被* remove一次！什么是一致呢，很简单就是你h的预估值h_score不能* 超过实际值，有没有小盆友想问如果我最优路径预估的h_score大，* 其他路径预估值小呢？不会错吗？我只说3点：*       1.h_score一定大于0,我猜的。额...毕竟A星是Dijkstra's * algorithm衍生的，然后Dijkstra's algorithm 大部分时候无法正常* 处理负权的图。*       2.最优路径的h_score再预估的大，因为一致你也不能超过实际路* 径，则f_score最终还是小于等于最优路径值。*      3.其他路径预测的再小，你最终f_score还是大于最优路径值* ,无非就是终点通过你这条路径先加入了openset，但是由于* 终点的f_score至少大于最优路径上节点的f_score，* 则终点一直不会被移除，直到最优路径测试完毕.。*/if (tmp_g_score < info[cur_pos].g_score) {info[cur_pos].g_score = tmp_g_score;info[cur_pos].f_score = tmp_g_score + info[cur_pos].h_score;info[cur_pos].came_from = min_pos;if (openhash[cur_pos])MIN_HEAP_update(openset, openhash[cur_pos], cur_pos, openhash, info);elseMIN_HEAP_insert(openset, &openset_size, cur_pos, openhash, info);}  }}reconstruct_path(info, dst_pos, map_col);
} int main() {// case v1int map_row = 5;int map_col = 7;int s_row = 2;int s_col = 1;int d_row = 2;int d_col = 5;bool **map = (bool **)malloc(sizeof(bool *) * (map_row));for (int i = 0; i < map_row; ++i) {map[i] = (bool *)malloc(sizeof(bool) * (map_col));for (int j = 0; j < map_col; ++j)map[i][j] = true;}// case v1map[1][1] = false;map[1][2] = false;map[1][3] = false;map[1][3] = false;map[1][4] = false;map[1][5] = false;map[2][3] = false;map[3][3] = false;map[4][3] = false;a_star(map, map_row, map_col, s_row, s_col, d_row, d_col);return 0;
}
/*Q1.下标使用1的话，通过pos去计算row、col需要特殊处理，比从0开始麻烦.e.g.比如第一行，最后一列的pos==map_col，你无法通过pos % map_col 直接求出来，但是其他的列是可以的，有的时候你去造轮子，会理解的更深入。Q2.如果从堆的下标1开始，可以很简单通过LEFT、RIGHT、PARENT计算父子节点。有兴趣的可以阅读一下《算法导论》堆的相关章节，不用太死抠证明，但能理解当然更好0.0。*/

A星算法(纯C实现)相关推荐

A星算法（Java实现）
2019独角兽企业重金招聘Python工程师标准>>> 一.适用场景在一张地图中,绘制从起点移动到终点的最优路径,地图中会有障碍物,必须绕开障碍物. 二.算法思路 1. 回溯法得到 ...
cocos2d-x游戏实例（5）-A星算法（1）
小满(bill man)个人原创,欢迎转载,转载请注明地址,小满(bill man)的专栏地址http://blog.csdn.net/bill_man 继续上一篇地图上的处理,不过和本篇相比,我们之 ...
cocos2d-x游戏实例（8）-A星算法（4）
小满(bill man)个人原创,欢迎转载,转载请注明地址,小满(bill man)的专栏地址http://blog.csdn.net/bill_man 继续A星算法,我们在经历了地图的检测,并且检测 ...
北斗导航 | Select Satellite 选星算法
================================================ 博主github:https://github.com/MichaelBeechan 博主CSDN:h ...
cocos2d-x游戏实例（9）-A星算法（5）
小满(bill man)个人原创,欢迎转载,转载请注明地址,小满(bill man)的专栏地址http://blog.csdn.net/bill_man 上一篇我们已经完成了A星算法,那么如何使用呢, ...
cocos2d-x游戏实例（7）-A星算法（3）
小满(bill man)个人原创,欢迎转载,转载请注明地址,小满(bill man)的专栏地址http://blog.csdn.net/bill_man 继续上一篇的内容,我们再看我们上一篇进行的部分 ...
cocos2d-x游戏实例（6）-A星算法（2）
小满(bill man)个人原创,欢迎转载,转载请注明地址,小满(bill man)的专栏地址http://blog.csdn.net/bill_man 上一篇中我们研究了A星算法的基本概念,本篇介绍 ...
c语言八数码A星算法代码解析,八数码问题c语言a星算法详细实验报告含代码解析...
八数码问题c语言a星算法详细实验报告含代码解析 (13页) 本资源提供全文预览,点击全文预览即可全文预览,如果喜欢文档就下载吧,查找使用更方便哦! 14.9 积分一.实验内容和要求八数码问题:在3 ...
Cocos2d-x 3.1.1 学习日志16--A星算法(A*搜索算法)学问
A *搜索算法称为A星算法.这是一个在图形平面,路径.求出最低通过成本的算法. 经常使用于游戏中的NPC的移动计算,或线上游戏的BOT的移动计算上. 首先:1.在Map地图中任取2个点,開始点和结束点 ...

A星算法(纯C实现)

A星算法(纯C实现)相关推荐

最新文章

热门文章