#include #include #include #include #define LINE_MAX_LENGTH 256 #define NUM_0_CHARCODE 48 #define equal(a,b) ((a)[0] == (b)[0] && (a)[1] == (b)[1]) #define MAX_NODES 128*1024 #define MAX_PATH 1024 #define BIT_0 0x0000001 #define BIT_1 0x0000002 #define BIT_2 0x0000004 #define BIT_3 0x0000008 typedef struct node { int position[2]; int weight; int dir[2]; int forward_count; } node_t; void insert_to_queue(node_t *nodes, int *nodes_num, node_t node); long dir_to_bit(int dir[2]); void set_tmp(node_t *tmp, int next_x, int next_y, uint8_t map[LINE_MAX_LENGTH][LINE_MAX_LENGTH], node_t curr, int next_dir[2], int fwd); void find_path(node_t *curr, int start[2], int finish[2], uint8_t map[LINE_MAX_LENGTH][LINE_MAX_LENGTH], int x, int y, int min_fwd, int max_fwd); int main() { char *p, *buf, c; buf = (char *)malloc(LINE_MAX_LENGTH); memset(buf, 0, LINE_MAX_LENGTH); p = buf; uint8_t map[LINE_MAX_LENGTH][LINE_MAX_LENGTH]; memset(map, 0, LINE_MAX_LENGTH * LINE_MAX_LENGTH * sizeof(uint8_t)); int x = 0, y = 0; while ((c = getchar()) != EOF) { *p++ = c; if (c == '\n') { p = buf; x = 0; while (*p != '\n') { map[x][y] = *p - NUM_0_CHARCODE; p++; x++; } y++; memset(buf, 0, LINE_MAX_LENGTH); p = buf; } } int start[2] = {0, 0}; int finish[2] = {x - 1, y - 1}; node_t curr; find_path(&curr, start, finish, map, x, y, 0, 3); printf("%i\n", curr.weight); find_path(&curr, start, finish, map, x, y, 4, 10); printf("%i\n", curr.weight); free(buf); } void insert_to_queue(node_t *nodes, int *nodes_num, node_t node) { int inserted = 0; for (int i = 0; i < *nodes_num; i++) { if (node.weight < nodes[i].weight) { // Shift to the right for (int j = *nodes_num; j > i; j--) { memcpy(&nodes[j], &nodes[j-1], sizeof(node_t)); } memcpy(&nodes[i], &node, sizeof(node_t)); inserted = 1; break; } } if (!inserted) { memcpy(&nodes[*nodes_num], &node, sizeof(node_t)); } (*nodes_num)++; } long dir_to_bit(int dir[2]) { if (dir[0] == 1) { return BIT_0; } if (dir[0] == -1) { return BIT_1; } if (dir[1] == 1) { return BIT_2; } return BIT_3; } void set_tmp(node_t *tmp, int next_x, int next_y, uint8_t map[LINE_MAX_LENGTH][LINE_MAX_LENGTH], node_t curr, int next_dir[2], int fwd) { tmp->position[0] = next_x; tmp->position[1] = next_y; tmp->weight = curr.weight + map[next_x][next_y]; tmp->dir[0] = next_dir[0]; tmp->dir[1] = next_dir[1]; tmp->forward_count = fwd ? curr.forward_count + 1 : 1; } void find_path(node_t *curr, int start[2], int finish[2], uint8_t map[LINE_MAX_LENGTH][LINE_MAX_LENGTH], int x, int y, int min_fwd, int max_fwd) { node_t tmp; tmp.position[0] = start[0]; tmp.position[1] = start[1]; tmp.weight = 0; tmp.dir[0] = 1; tmp.dir[1] = 0; tmp.forward_count = 1; long visited[LINE_MAX_LENGTH][LINE_MAX_LENGTH]; memset(visited, 0, LINE_MAX_LENGTH * LINE_MAX_LENGTH * sizeof(long)); node_t *nodes = (node_t*)malloc(MAX_NODES * sizeof(node_t)); memset(nodes, 0, MAX_NODES * sizeof(node_t)); int nodes_num = 1; memcpy(&nodes[0], &tmp, sizeof(node_t)); int next_x, next_y, next_dir[2]; for (;;) { // Pop first node memcpy(curr, &nodes[0], sizeof(node_t)); if (equal(curr->position, finish) && curr->forward_count >= min_fwd) { break; } // Shift entries for (int i = 0; i < nodes_num - 1; i++) { memcpy(&nodes[i], &nodes[i+1], sizeof(node_t)); } nodes_num--; // Check forward direction if (curr->forward_count != max_fwd) { next_dir[0] = curr->dir[0]; next_dir[1] = curr->dir[1]; next_x = curr->position[0] + next_dir[0]; next_y = curr->position[1] + next_dir[1]; if (next_x >= 0 && next_x < x && next_y >= 0 && next_y < y && !(visited[next_x][next_y] & (dir_to_bit(next_dir) << (curr->forward_count * 4)))) { visited[next_x][next_y] |= (dir_to_bit(next_dir) << (curr->forward_count * 4)); set_tmp(&tmp, next_x, next_y, map, *curr, next_dir, 1); insert_to_queue(nodes, &nodes_num, tmp); } } next_dir[0] = curr->dir[0] == 0 ? 1 : 0; next_dir[1] = curr->dir[1] == 0 ? 1 : 0; next_x = curr->position[0] + next_dir[0]; next_y = curr->position[1] + next_dir[1]; if (curr->forward_count >= min_fwd && next_x >= 0 && next_x < x && next_y >= 0 && next_y < y && !(visited[next_x][next_y] & (dir_to_bit(next_dir) << (max_fwd * 4)))) { visited[next_x][next_y] |= (dir_to_bit(next_dir) << (max_fwd * 4)); set_tmp(&tmp, next_x, next_y, map, *curr, next_dir, 0); insert_to_queue(nodes, &nodes_num, tmp); } next_dir[0] = curr->dir[0] == 0 ? -1 : 0; next_dir[1] = curr->dir[1] == 0 ? -1 : 0; next_x = curr->position[0] + next_dir[0]; next_y = curr->position[1] + next_dir[1]; if (curr->forward_count >= min_fwd && next_x >= 0 && next_x < x && next_y >= 0 && next_y < y && !(visited[next_x][next_y] & (dir_to_bit(next_dir) << (max_fwd * 4)))) { visited[next_x][next_y] |= (dir_to_bit(next_dir) << (max_fwd * 4)); set_tmp(&tmp, next_x, next_y, map, *curr, next_dir, 0); insert_to_queue(nodes, &nodes_num, tmp); } if (nodes_num == 0) { printf("No path to end\n"); break; } } free(nodes); }