LA77

Untitled

Apr 7th, 2025
66
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 7.05 KB | None | 0 0
  1. #include <stdio.h>
  2. #include <stdlib.h>
  3. #include <stdbool.h>
  4. #include <limits.h>
  5. #include <assert.h>
  6. #include <math.h>
  7.  
  8. #define INF 2147483647
  9.  
  10. typedef struct Graph {
  11. int dest_node, weight, rotated;
  12. struct Graph *next;
  13. } Graph;
  14.  
  15. Graph **graph[2];
  16.  
  17. void insert(Graph **g, int x, int y, int z) {
  18. Graph *q = malloc(sizeof(Graph));
  19. q->dest_node = y;
  20. q->weight = z;
  21. q->next = g[x];
  22. g[x] = q;
  23. }
  24.  
  25. typedef struct {
  26. double f;
  27. int g;
  28. int node;
  29. int dir;
  30. } HeapNode;
  31.  
  32. typedef struct Heap {
  33. HeapNode *array;
  34. int front;
  35. int size;
  36. } Heap;
  37.  
  38. Heap makeHeap() {
  39. Heap h;
  40. h.array = malloc(1 * sizeof(HeapNode));
  41. assert(h.array != NULL);
  42. h.front = 1;
  43. h.size = 1;
  44. return h;
  45. }
  46.  
  47. int isEmptyHeap(Heap h) {
  48. return (h.front == 1);
  49. }
  50.  
  51. void heapEmptyError() {
  52. printf("heap empty\n");
  53. abort();
  54. }
  55.  
  56. void doubleHeapSize(Heap *hp) {
  57. int newSize = 2 * hp->size;
  58. HeapNode *newArray = malloc(newSize * sizeof(HeapNode));
  59. assert(newArray != NULL);
  60. for (int i = 1; i < hp->front; i++) {
  61. newArray[i] = hp->array[i];
  62. }
  63. free(hp->array);
  64. hp->array = newArray;
  65. hp->size = newSize;
  66. }
  67.  
  68. void upheap(Heap *hp, int n) {
  69. HeapNode val = hp->array[n];
  70. while (n > 1 && hp->array[n / 2].f > val.f) {
  71. hp->array[n] = hp->array[n / 2];
  72. n = n / 2;
  73. }
  74. hp->array[n] = val;
  75. }
  76.  
  77. void downheap(Heap *hp, int n) {
  78. HeapNode val = hp->array[n];
  79. int size = hp->front;
  80. while (2 * n < size) {
  81. int j = 2 * n;
  82. if (j + 1 < size && hp->array[j + 1].f < hp->array[j].f) j++;
  83. if (val.f <= hp->array[j].f) break;
  84. hp->array[n] = hp->array[j];
  85. n = j;
  86. }
  87. hp->array[n] = val;
  88. }
  89.  
  90. void enqueue(HeapNode hnode, Heap *hp) {
  91. int fr = hp->front;
  92. if (fr == hp->size) {
  93. doubleHeapSize(hp);
  94. }
  95. hp->array[fr] = hnode;
  96. upheap(hp, fr);
  97. hp->front = fr + 1;
  98. }
  99.  
  100. HeapNode removeMax(Heap *hp) {
  101. HeapNode hnode;
  102. if (isEmptyHeap(*hp)) heapEmptyError();
  103. hnode = hp->array[1];
  104. hp->front--;
  105. hp->array[1] = hp->array[hp->front];
  106. downheap(hp, 1);
  107. return hnode;
  108. }
  109.  
  110. double heuristic(int node, int end_node, double *X, double *Y) {
  111. double dx = X[node] - X[end_node];
  112. double dy = Y[node] - Y[end_node];
  113. return sqrt(dx * dx + dy * dy);
  114. }
  115.  
  116. void a_star(int start_node, int end_node, int n, int *rotated, double *X, double *Y) {
  117. int **dist = malloc(n * sizeof(int *));
  118. int **visited = malloc(n * sizeof(int *));
  119. int **prev = malloc(n * sizeof(int *));
  120. int **usedR = malloc(n * sizeof(int *));
  121. for (int i = 0; i < n; i++) {
  122. dist[i] = malloc(2 * sizeof(int));
  123. visited[i] = malloc(2 * sizeof(int));
  124. prev[i] = malloc(2 * sizeof(int));
  125. usedR[i] = malloc(2 * sizeof(int));
  126. dist[i][0] = dist[i][1] = INF;
  127. visited[i][0] = visited[i][1] = 0;
  128. prev[i][0] = prev[i][1] = -1;
  129. usedR[i][0] = usedR[i][1] = 0;
  130. }
  131. Heap pq = makeHeap();
  132. dist[start_node][0] = 0;
  133. enqueue((HeapNode){0 + heuristic(start_node, end_node, X, Y), 0, start_node, 0}, &pq);
  134. while (!isEmptyHeap(pq)) {
  135. HeapNode cur = removeMax(&pq);
  136. int node = cur.node;
  137. int dir = cur.dir;
  138. if (visited[node][dir]) continue;
  139. visited[node][dir] = 1;
  140. Graph *c = graph[dir][node];
  141. while (c) {
  142. int to = c->dest_node;
  143. int w = c->weight;
  144. if (dist[node][dir] + w < dist[to][dir]) {
  145. dist[to][dir] = dist[node][dir] + w;
  146. prev[to][dir] = node;
  147. usedR[to][dir] = 0;
  148. int new_g = dist[to][dir];
  149. double f = new_g + heuristic(to, end_node, X, Y);
  150. enqueue((HeapNode){f, new_g, to, dir}, &pq);
  151. }
  152. c = c->next;
  153. }
  154. if (rotated[node]) {
  155. int ndir = 1 - dir;
  156. if (dist[node][dir] < dist[node][ndir]) {
  157. dist[node][ndir] = dist[node][dir];
  158. prev[node][ndir] = node;
  159. usedR[node][ndir] = 1;
  160. int new_g = dist[node][dir];
  161. double f = new_g + heuristic(node, end_node, X, Y);
  162. enqueue((HeapNode){f, new_g, node, ndir}, &pq);
  163. }
  164. }
  165. }
  166. if (dist[end_node][0] == INF && dist[end_node][1] == INF) {
  167. printf("IMPOSSIBLE\n");
  168. for (int i = 0; i < n; i++) {
  169. free(dist[i]);
  170. free(visited[i]);
  171. free(prev[i]);
  172. free(usedR[i]);
  173. }
  174. free(dist);
  175. free(visited);
  176. free(prev);
  177. free(usedR);
  178. free(pq.array);
  179. return;
  180. }
  181. int d = (dist[end_node][0] <= dist[end_node][1]) ? 0 : 1;
  182. printf("%d\n", dist[end_node][d]);
  183. int *path = malloc(2 * n * sizeof(int));
  184. int *rflags = malloc(2 * n * sizeof(int));
  185. int idx = 0;
  186. int cur_node = end_node;
  187. int cur_dir = d;
  188. path[idx] = cur_node;
  189. rflags[idx] = 0;
  190. while (1) {
  191. if (prev[cur_node][cur_dir] == -1) break;
  192. if (usedR[cur_node][cur_dir]) {
  193. rflags[idx] = 1;
  194. cur_dir = 1 - cur_dir;
  195. } else {
  196. int p = prev[cur_node][cur_dir];
  197. idx++;
  198. path[idx] = p;
  199. rflags[idx] = 0;
  200. cur_node = p;
  201. }
  202. }
  203. for (int i = idx; i >= 0; i--) {
  204. printf("%d", path[i]);
  205. if (i != 0 && rflags[i]) printf(" R");
  206. printf("\n");
  207. }
  208. for (int i = 0; i < n; i++) {
  209. free(dist[i]);
  210. free(visited[i]);
  211. free(prev[i]);
  212. free(usedR[i]);
  213. }
  214. free(dist);
  215. free(visited);
  216. free(prev);
  217. free(usedR);
  218. free(path);
  219. free(rflags);
  220. free(pq.array);
  221. }
  222.  
  223. int main() {
  224. int orig_n, m;
  225. scanf("%d%d", &orig_n, &m);
  226. int n = orig_n + 1;
  227. int *rotated = calloc(n, sizeof(int));
  228. graph[0] = malloc(n * sizeof(Graph *));
  229. graph[1] = malloc(n * sizeof(Graph *));
  230. for (int i = 0; i < n; i++) {
  231. graph[0][i] = NULL;
  232. graph[1][i] = NULL;
  233. }
  234. int x;
  235. while (true) {
  236. scanf("%d", &x);
  237. if (x == -1) break;
  238. rotated[x] = 1;
  239. }
  240. for (int i = 0; i < m; i++) {
  241. int a, b, l;
  242. scanf("%d%d%d", &a, &b, &l);
  243. insert(graph[0], a, b, l);
  244. insert(graph[1], b, a, l);
  245. }
  246. double *X = malloc(n * sizeof(double));
  247. double *Y = malloc(n * sizeof(double));
  248. for (int i = 1; i < n; i++) {
  249. scanf("%lf %lf", &X[i], &Y[i]);
  250. }
  251. a_star(1, n - 1, n, rotated, X, Y);
  252. for (int i = 0; i < n; i++) {
  253. Graph *c = graph[0][i];
  254. while (c) {
  255. Graph *tmp = c;
  256. c = c->next;
  257. free(tmp);
  258. }
  259. c = graph[1][i];
  260. while (c) {
  261. Graph *tmp = c;
  262. c = c->next;
  263. free(tmp);
  264. }
  265. }
  266. free(graph[0]);
  267. free(graph[1]);
  268. free(rotated);
  269. free(X);
  270. free(Y);
  271. return 0;
  272. }
Advertisement
Add Comment
Please, Sign In to add comment