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);
				else
					MIN_HEAP_insert(openset, &openset_size, cur_pos, openhash, info);
			}	
		}
	}

	reconstruct_path(info, dst_pos, map_col);
} 

int main() {
	// case v1
	int 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 v1
	map[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。
 */