Das Programmierspiel von der Gulasch-Programmier-Nacht 11

(Latest mirror + merged latest fork by qr4 on Lua 5.3)

Entropia info page https://entropia.de/GPN11:Programmierspiel (dead links)
Origin Gitlab
https://code.nerd2nerd.org/n2n/WeltraumProgrammierNacht

route.c 8.9KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352
  1. #include <math.h>
  2. #include <stdio.h>
  3. #include <stdlib.h>
  4. #include "route.h"
  5. #include "globals.h"
  6. // Abstand zwischen zwei Punkten
  7. double dist(pixel_t* A, pixel_t* B) {
  8. return hypotf(B->x - A->x, B->y - A->y);
  9. }
  10. // Wo auf der Linie zwischen A und B sitzt der Fußpunkt des Lots von C auf AB?
  11. // Werte kleiner Null bedeuten vor A entlang AB
  12. // Werte zwischen 0 und 1 geben an nach welchem Bruchteil von AB der Fußpunkt kommt
  13. // Werte größer 1 bedeuten daß der Fußpunkt auf der Verlängerung von AB hinter B liegt
  14. // Wert den Code mit A=B aufruf ist doof und verdient daß alles platzt
  15. double dividing_ratio(pixel_t* A, pixel_t* B, pixel_t* C) {
  16. return ((C->x - A->x)*(B->x - A->x) + (C->y - A->y)*(B->y - A->y))/pow(dist(A, B),2);
  17. }
  18. // Was ist der Minimal-Abstand von C zum Linien AB?
  19. // Achtung dies liefert den vorzeichenbehafteten Abstand
  20. // Werte kleiner Null bedeuten das C "links" der Verbindungslinie liegt wenn man von A Richtung B schaut
  21. // Werte größer Null dementsprechend recht, exakt null auf der Verbindungslinie
  22. double dist_to_line(pixel_t* A, pixel_t* B, pixel_t* C) {
  23. //return ((A->y - B->y)*C->x + (A->x - B->x)*C->y + (A->x * B->y - A->y * B->x))/dist(A,B);
  24. //return ((B->y - A->y)*C->x + (A->x - B->x)* C->y - ((B->y - A->y)* A->x + (A->x - B->x)* A->y))/dist(A,B);
  25. return ((A->y - B->y)*C->x - (A->x - B->x)* C->y + ((B->y - A->y)* A->x + (A->x - B->x)* A->y))/dist(A,B);
  26. }
  27. // Was ist der Minimal-Abstand von C zum Liniensegment AB?
  28. // Die ist der Abstand zwischen C und dem Fußpunkt des Lots auf AB fall dieser zwischen A und B fällt
  29. // Ansonsten der Abstand zu A btw B
  30. double dist_to_seg(pixel_t* A, pixel_t* B, pixel_t* C) {
  31. double r = dividing_ratio(A, B, C);
  32. if(r <= 0) {
  33. return dist(A, C);
  34. } else if (r >= 1) {
  35. return dist(B, C);
  36. } else {
  37. return fabs(dist_to_line(A, B, C));
  38. }
  39. }
  40. waypoint_t* go_around(pixel_t* A, pixel_t* B, pixel_t* C) {
  41. double r = dividing_ratio(A, B, C);
  42. pixel_t X = { A->x + r*(B->x - A->x), A->y + r*(B->y - A->y) };
  43. double d = dist(&X, C);
  44. pixel_t W = {C->x + safety_radius*sqrt(2)*(X.x - C->x) / d, C->y + safety_radius*sqrt(2)*(X.y - C->y) / d};
  45. waypoint_t* wp = malloc(sizeof(waypoint_t));
  46. wp->next = NULL;
  47. wp->point.x = W.x;
  48. wp->point.y = W.y;
  49. //printf("Suggest you go via (%f,%f) to avoid (%f,%f)\n", W.x, W.y, C->x, C->y);
  50. return wp;
  51. }
  52. waypoint_t* route(pixel_t* start, pixel_t* stop, pixel_t* points, int n_points) {
  53. //printf("Running route\n");
  54. /*
  55. pixel_t A = {0, 0};
  56. pixel_t B = {1000, 0};
  57. pixel_t C1 = {500, 200};
  58. printf("A = (0,0), B=(1000,0), C1=(500,200)\n");
  59. printf("dist(A,B) = %f\n", dist(&A, &B));
  60. printf("r(AB, C1) = %f\n", dividing_ratio(&A, &B, &C1));
  61. printf("dist_to_seg(A, B, C1) = %f\n", dist_to_seg(&A, &B, &C1));
  62. pixel_t C2 = {-100, 200};
  63. printf("C2 = (-100,200)\n");
  64. printf("r(AB, C2) = %f\n", dividing_ratio(&A, &B, &C2));
  65. printf("dist_to_seg(A, B, C2) = %f\n", dist_to_seg(&A, &B, &C2));
  66. pixel_t C3 = {200, -400};
  67. printf("C3 = (200,-400)\n");
  68. printf("r(AB, C3) = %f\n", dividing_ratio(&A, &B, &C2));
  69. printf("dist_to_seg(A, B, C3) = %f\n", dist_to_seg(&A, &B, &C3));
  70. */
  71. int i;
  72. int i_min = -1;
  73. double r_min = 1;
  74. for(i = 0; i < n_points; i++) {
  75. double r = dividing_ratio(start, stop, &(points[i]));
  76. if (r > 0 && r < 1) {
  77. double d = dist_to_line(start, stop, &(points[i]));
  78. if (fabs(d) < safety_radius) {
  79. //printf("Point #%d at (%f,%f) is an obstacle %f pixel away from the course\n", i, points[i].x, points[i].y, d);
  80. if(r < r_min) {
  81. i_min = i;
  82. r_min = r;
  83. }
  84. }
  85. }
  86. }
  87. if(i_min >= 0) {
  88. printf("Point #%d at (%f,%f) is the first obstacle %f pixel down the course\n", i_min, points[i_min].x, points[i_min].y, r_min * dist(start, stop));
  89. waypoint_t* wp = go_around(start, stop, &(points[i_min]));
  90. waypoint_t* part1 = route(start, &(wp->point), points, n_points);
  91. if(part1 == NULL) {
  92. part1 = wp;
  93. } else {
  94. waypoint_t* t = part1;
  95. while(t->next != NULL) {
  96. t = t->next;
  97. }
  98. t->next = wp;
  99. }
  100. waypoint_t* part2 = route(&(wp->point), stop, points, n_points);
  101. if(part2 != 0) {
  102. waypoint_t* t = part1;
  103. while(t->next != NULL) {
  104. t = t->next;
  105. }
  106. t->next = part2;
  107. }
  108. return part1;
  109. } else {
  110. return NULL;
  111. }
  112. }
  113. waypoint_t* plotCourse(pixel_t* start, pixel_t* stop, pixel_t* points, int n) {
  114. int n_points = n*n;
  115. waypoint_t* wp_start = malloc(sizeof(waypoint_t));
  116. waypoint_t* wp_stop = malloc(sizeof(waypoint_t));
  117. wp_start->point.x = start->x;
  118. wp_start->point.y = start->y;
  119. wp_stop->point.x = stop->x;
  120. wp_stop->point.y = stop->y;
  121. wp_start->next = route(start, stop, points, n_points);
  122. wp_stop->next = NULL;
  123. waypoint_t* t = wp_start;
  124. while (t->next != NULL) {
  125. t = t->next;
  126. }
  127. t->next = wp_stop;
  128. return wp_start;
  129. }
  130. void get_surrounding_points(pixel_t *surrounding_points, pixel_t *points, int n, int face_x, int face_y) {
  131. surrounding_points[0] = points[(face_y - 1) * n + (face_x - 1)];
  132. surrounding_points[1] = points[(face_y) * n + (face_x - 1)];
  133. surrounding_points[2] = points[(face_y) * n + (face_x)];
  134. surrounding_points[3] = points[(face_y - 1) * n + (face_x)];
  135. }
  136. int get_line_intersection(pixel_t *P0, pixel_t *P1, pixel_t *P2, pixel_t *P3, pixel_t *result)
  137. {
  138. float s1_x, s1_y, s2_x, s2_y;
  139. s1_x = P1->x - P0->x;
  140. s1_y = P1->y - P0->y;
  141. s2_x = P3->x - P2->x;
  142. s2_y = P3->y - P2->y;
  143. float s, t;
  144. s = (-s1_y * (P0->x - P2->x) + s1_x * (P0->y - P2->y)) / (-s2_x * s1_y + s1_x * s2_y);
  145. t = ( s2_x * (P0->y - P2->y) - s2_y * (P0->x - P2->x)) / (-s2_x * s1_y + s1_x * s2_y);
  146. if (s >= 0 && s <= 1 && t >= 0 && t <= 1)
  147. {
  148. if (result) {
  149. result->x = P0->x + (t * s1_x);
  150. result->y = P0->y + (t * s1_y);
  151. }
  152. return 1;
  153. }
  154. return 0;
  155. }
  156. void find_face(pixel_t *p, pixel_t *points, int n, int *x, int *y) {
  157. int face_x = p->x / (GLOBALS.WIDTH / (n + 1));
  158. int face_y = p->y / (GLOBALS.HEIGHT / (n + 1));
  159. int edge;
  160. int retry = 1;
  161. int count = 0;
  162. pixel_t surrounding[4];
  163. while (retry && count < 4) {
  164. retry = 0;
  165. get_surrounding_points(surrounding, points, n, face_x, face_y);
  166. for (edge = 0; edge < 4; edge++) {
  167. double r = dist_to_line(surrounding + edge, surrounding + (edge + 1) % 4, p);
  168. if (r > 0) {
  169. printf("edge: %d, r %f, x %d, y %d\n", edge, r, face_x, face_y);
  170. retry = 1;
  171. count++;
  172. switch (edge) {
  173. case 0 :
  174. face_x--;
  175. break;
  176. case 1 :
  177. face_y++;
  178. break;
  179. case 2 :
  180. face_x++;
  181. break;
  182. case 3 :
  183. face_y--;
  184. break;
  185. }
  186. break;
  187. }
  188. }
  189. }
  190. *x = face_x;
  191. *y = face_y;
  192. }
  193. waypoint_t *smooth(waypoint_t *way, int res) {
  194. waypoint_t *end = way;
  195. waypoint_t *working_start;
  196. waypoint_t *working;
  197. pixel_t startp;
  198. pixel_t midp;
  199. pixel_t endp;
  200. pixel_t t1, t2;
  201. pixel_t v1, v2;
  202. pixel_t s;
  203. int i;
  204. working_start = malloc(sizeof(waypoint_t));
  205. *working_start = *way;
  206. working_start->next = NULL;
  207. working = working_start;
  208. if (end) {
  209. midp = end->point;
  210. end = end->next;
  211. } else return NULL;
  212. if (end) {
  213. endp = end->point;
  214. end = end->next;
  215. } else return NULL;
  216. while (end != NULL) {
  217. startp = midp;
  218. midp = endp;
  219. endp = end->point;
  220. t1.x = (startp.x + midp.x) / 2.;
  221. t1.y = (startp.y + midp.y) / 2.;
  222. t2 = midp;
  223. v1.x = (midp.x - startp.x) / 2. / res;
  224. v1.y = (midp.y - startp.y) / 2. / res;
  225. v2.x = (endp.x - midp.x) / 2. / res;
  226. v2.y = (endp.y - midp.y) / 2. / res;
  227. for (i = 0; i < res; i++) {
  228. s.x = t1.x + ((t2.x - t1.x) * i) / res;
  229. s.y = t1.y + ((t2.y - t1.y) * i) / res;
  230. t1.x += v1.x;
  231. t1.y += v1.y;
  232. t2.x += v2.x;
  233. t2.y += v2.y;
  234. working->next = malloc (sizeof(waypoint_t));
  235. working = working->next;
  236. working->point = s;
  237. }
  238. working->next = end;
  239. end = end->next;
  240. }
  241. return working_start;
  242. }
  243. waypoint_t* plotCourse_gridbased(pixel_t *start, pixel_t *stop, pixel_t *points, int n) {
  244. int face_x, face_y, face_s_x, face_s_y;
  245. int edge;
  246. int last_edge = -1;
  247. find_face(start, points, n, &face_x, &face_y);
  248. find_face(stop, points, n, &face_s_x, &face_s_y);
  249. pixel_t surrounding[4];
  250. waypoint_t *wp_start = malloc (sizeof(waypoint_t));
  251. wp_start->point = *start;
  252. wp_start->next = NULL;
  253. waypoint_t *working = wp_start;
  254. while (face_x != face_s_x || face_y != face_s_y) {
  255. pixel_t c;
  256. get_surrounding_points(surrounding, points, n, face_x, face_y);
  257. for (edge = 0; edge < 4; edge++) {
  258. if (edge == last_edge) continue;
  259. c = working->point;
  260. if (get_line_intersection(surrounding + edge, surrounding + (edge + 1)%4, &c, stop, NULL)) {
  261. c.x = (surrounding[edge].x + surrounding[(edge + 1) % 4].x) / 2;
  262. c.y = (surrounding[edge].y + surrounding[(edge + 1) % 4].y) / 2;
  263. break;
  264. }
  265. }
  266. working->next = malloc(sizeof(waypoint_t));
  267. working = working->next;
  268. working->point = c;
  269. working->next = NULL;
  270. switch (edge) {
  271. case 0 :
  272. face_x--;
  273. break;
  274. case 1 :
  275. face_y++;
  276. break;
  277. case 2 :
  278. face_x++;
  279. break;
  280. case 3 :
  281. face_y--;
  282. break;
  283. default:
  284. return wp_start;
  285. }
  286. last_edge = (edge + 2) % 4;
  287. }
  288. working->next = malloc(sizeof(waypoint_t));
  289. working = working->next;
  290. working->point = *stop;
  291. working->next = NULL;
  292. return wp_start;
  293. }