Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <stdio.h>
- #include <stdlib.h>
- #include <stdbool.h>
- #include <limits.h>
- #include <assert.h>
- #include <math.h>
- #define INF 2147483647
- typedef struct Graph {
- int dest_node, weight, rotated;
- struct Graph *next;
- } Graph;
- Graph **graph[2];
- void insert(Graph **g, int x, int y, int z) {
- Graph *q = malloc(sizeof(Graph));
- q->dest_node = y;
- q->weight = z;
- q->next = g[x];
- g[x] = q;
- }
- typedef struct {
- double f;
- int g;
- int node;
- int dir;
- } HeapNode;
- typedef struct Heap {
- HeapNode *array;
- int front;
- int size;
- } Heap;
- Heap makeHeap() {
- Heap h;
- h.array = malloc(1 * sizeof(HeapNode));
- assert(h.array != NULL);
- h.front = 1;
- h.size = 1;
- return h;
- }
- int isEmptyHeap(Heap h) {
- return (h.front == 1);
- }
- void heapEmptyError() {
- printf("heap empty\n");
- abort();
- }
- void doubleHeapSize(Heap *hp) {
- int newSize = 2 * hp->size;
- HeapNode *newArray = malloc(newSize * sizeof(HeapNode));
- assert(newArray != NULL);
- for (int i = 1; i < hp->front; i++) {
- newArray[i] = hp->array[i];
- }
- free(hp->array);
- hp->array = newArray;
- hp->size = newSize;
- }
- void upheap(Heap *hp, int n) {
- HeapNode val = hp->array[n];
- while (n > 1 && hp->array[n / 2].f > val.f) {
- hp->array[n] = hp->array[n / 2];
- n = n / 2;
- }
- hp->array[n] = val;
- }
- void downheap(Heap *hp, int n) {
- HeapNode val = hp->array[n];
- int size = hp->front;
- while (2 * n < size) {
- int j = 2 * n;
- if (j + 1 < size && hp->array[j + 1].f < hp->array[j].f) j++;
- if (val.f <= hp->array[j].f) break;
- hp->array[n] = hp->array[j];
- n = j;
- }
- hp->array[n] = val;
- }
- void enqueue(HeapNode hnode, Heap *hp) {
- int fr = hp->front;
- if (fr == hp->size) {
- doubleHeapSize(hp);
- }
- hp->array[fr] = hnode;
- upheap(hp, fr);
- hp->front = fr + 1;
- }
- HeapNode removeMax(Heap *hp) {
- HeapNode hnode;
- if (isEmptyHeap(*hp)) heapEmptyError();
- hnode = hp->array[1];
- hp->front--;
- hp->array[1] = hp->array[hp->front];
- downheap(hp, 1);
- return hnode;
- }
- double heuristic(int node, int end_node, double *X, double *Y) {
- double dx = X[node] - X[end_node];
- double dy = Y[node] - Y[end_node];
- return sqrt(dx * dx + dy * dy);
- }
- void a_star(int start_node, int end_node, int n, int *rotated, double *X, double *Y) {
- int **dist = malloc(n * sizeof(int *));
- int **visited = malloc(n * sizeof(int *));
- int **prev = malloc(n * sizeof(int *));
- int **usedR = malloc(n * sizeof(int *));
- for (int i = 0; i < n; i++) {
- dist[i] = malloc(2 * sizeof(int));
- visited[i] = malloc(2 * sizeof(int));
- prev[i] = malloc(2 * sizeof(int));
- usedR[i] = malloc(2 * sizeof(int));
- dist[i][0] = dist[i][1] = INF;
- visited[i][0] = visited[i][1] = 0;
- prev[i][0] = prev[i][1] = -1;
- usedR[i][0] = usedR[i][1] = 0;
- }
- Heap pq = makeHeap();
- dist[start_node][0] = 0;
- enqueue((HeapNode){0 + heuristic(start_node, end_node, X, Y), 0, start_node, 0}, &pq);
- while (!isEmptyHeap(pq)) {
- HeapNode cur = removeMax(&pq);
- int node = cur.node;
- int dir = cur.dir;
- if (visited[node][dir]) continue;
- visited[node][dir] = 1;
- Graph *c = graph[dir][node];
- while (c) {
- int to = c->dest_node;
- int w = c->weight;
- if (dist[node][dir] + w < dist[to][dir]) {
- dist[to][dir] = dist[node][dir] + w;
- prev[to][dir] = node;
- usedR[to][dir] = 0;
- int new_g = dist[to][dir];
- double f = new_g + heuristic(to, end_node, X, Y);
- enqueue((HeapNode){f, new_g, to, dir}, &pq);
- }
- c = c->next;
- }
- if (rotated[node]) {
- int ndir = 1 - dir;
- if (dist[node][dir] < dist[node][ndir]) {
- dist[node][ndir] = dist[node][dir];
- prev[node][ndir] = node;
- usedR[node][ndir] = 1;
- int new_g = dist[node][dir];
- double f = new_g + heuristic(node, end_node, X, Y);
- enqueue((HeapNode){f, new_g, node, ndir}, &pq);
- }
- }
- }
- if (dist[end_node][0] == INF && dist[end_node][1] == INF) {
- printf("IMPOSSIBLE\n");
- for (int i = 0; i < n; i++) {
- free(dist[i]);
- free(visited[i]);
- free(prev[i]);
- free(usedR[i]);
- }
- free(dist);
- free(visited);
- free(prev);
- free(usedR);
- free(pq.array);
- return;
- }
- int d = (dist[end_node][0] <= dist[end_node][1]) ? 0 : 1;
- printf("%d\n", dist[end_node][d]);
- int *path = malloc(2 * n * sizeof(int));
- int *rflags = malloc(2 * n * sizeof(int));
- int idx = 0;
- int cur_node = end_node;
- int cur_dir = d;
- path[idx] = cur_node;
- rflags[idx] = 0;
- while (1) {
- if (prev[cur_node][cur_dir] == -1) break;
- if (usedR[cur_node][cur_dir]) {
- rflags[idx] = 1;
- cur_dir = 1 - cur_dir;
- } else {
- int p = prev[cur_node][cur_dir];
- idx++;
- path[idx] = p;
- rflags[idx] = 0;
- cur_node = p;
- }
- }
- for (int i = idx; i >= 0; i--) {
- printf("%d", path[i]);
- if (i != 0 && rflags[i]) printf(" R");
- printf("\n");
- }
- for (int i = 0; i < n; i++) {
- free(dist[i]);
- free(visited[i]);
- free(prev[i]);
- free(usedR[i]);
- }
- free(dist);
- free(visited);
- free(prev);
- free(usedR);
- free(path);
- free(rflags);
- free(pq.array);
- }
- int main() {
- int orig_n, m;
- scanf("%d%d", &orig_n, &m);
- int n = orig_n + 1;
- int *rotated = calloc(n, sizeof(int));
- graph[0] = malloc(n * sizeof(Graph *));
- graph[1] = malloc(n * sizeof(Graph *));
- for (int i = 0; i < n; i++) {
- graph[0][i] = NULL;
- graph[1][i] = NULL;
- }
- int x;
- while (true) {
- scanf("%d", &x);
- if (x == -1) break;
- rotated[x] = 1;
- }
- for (int i = 0; i < m; i++) {
- int a, b, l;
- scanf("%d%d%d", &a, &b, &l);
- insert(graph[0], a, b, l);
- insert(graph[1], b, a, l);
- }
- double *X = malloc(n * sizeof(double));
- double *Y = malloc(n * sizeof(double));
- for (int i = 1; i < n; i++) {
- scanf("%lf %lf", &X[i], &Y[i]);
- }
- a_star(1, n - 1, n, rotated, X, Y);
- for (int i = 0; i < n; i++) {
- Graph *c = graph[0][i];
- while (c) {
- Graph *tmp = c;
- c = c->next;
- free(tmp);
- }
- c = graph[1][i];
- while (c) {
- Graph *tmp = c;
- c = c->next;
- free(tmp);
- }
- }
- free(graph[0]);
- free(graph[1]);
- free(rotated);
- free(X);
- free(Y);
- return 0;
- }
Advertisement
Add Comment
Please, Sign In to add comment