Vous ne pouvez pas sélectionner plus de 25 sujets Les noms de sujets doivent commencer par une lettre ou un nombre, peuvent contenir des tirets ('-') et peuvent comporter jusqu'à 35 caractères.

162 lignes
3.8KB

  1. #include "control.h"
  2. #include <math.h>
  3. #include <stdlib.h>
  4. #include <assert.h>
  5. struct angle {
  6. double a;
  7. int c;
  8. };
  9. typedef struct angle angle;
  10. gint cmp_angle(gconstpointer _a, gconstpointer _b, gpointer p) {
  11. const angle *a = (const angle*) _a, *b = (const angle*) _b;
  12. UNUSED(p);
  13. return (a->a == b->a) ? 0 : (a->a < b->a) ? -1 : 1;
  14. }
  15. void angle_free(angle* a) {
  16. g_slice_free(angle, a);
  17. }
  18. angle* angle_new(double a, int c) {
  19. angle *x = g_slice_new(angle);
  20. x->a = a; x->c = c;
  21. return x;
  22. }
  23. static double fixangle(double a) {
  24. while (a < -180) a += 360;
  25. while (a > 180) a -= 360;
  26. return a;
  27. }
  28. int distance_weight(double distance) {
  29. if (distance > 50) {
  30. if (distance > 150) return 0;
  31. else return 1;
  32. } else {
  33. if (distance > 20) return 10;
  34. else return 200;
  35. }
  36. }
  37. static inline gboolean insert_object(GSequence *angles, object *o, vehicle *v, double ve_ho_angle) {
  38. double phi, phi2, avoid = 0, oangle;
  39. double distance;
  40. int distweight;
  41. distance = sqrt((o->y - v->y) * (o->y - v->y) + (o->x - v->x) * (o->x - v->x));
  42. distweight = distance_weight(distance - o->rad);
  43. if (distweight == 0) return FALSE;
  44. oangle = fixangle(atan2(-o->y, -o->x)*180/M_PI - ve_ho_angle);
  45. if (fabs(oangle) > 90) return FALSE;
  46. phi = fixangle(atan2(o->y - v->y, o->x - v->x)*180/M_PI - ve_ho_angle);
  47. if (fabs(phi) > 120) return FALSE;
  48. switch (o->type) {
  49. case BOLDER:
  50. avoid = 1 * o->rad + VEHICLE_RADIUS + 1; break;
  51. case CRATER:
  52. avoid = 1 * o->rad + VEHICLE_RADIUS + 2; break;
  53. case MARTIAN:
  54. /* todo: guess martian movement */
  55. avoid = o->rad + VEHICLE_RADIUS + 5; break;
  56. }
  57. phi2 = atan(avoid / distance )*180/M_PI;
  58. g_sequence_insert_sorted(angles, angle_new(phi - phi2, 1), cmp_angle, NULL);
  59. g_sequence_insert_sorted(angles, angle_new(phi + phi2, -1), cmp_angle, NULL);
  60. return TRUE;
  61. }
  62. void goradar(trial *t) {
  63. vehicle v;
  64. telemetry *tm;
  65. GSequence *angles = g_sequence_new((GDestroyNotify) angle_free);
  66. GSequenceIter *prev, *cur, *end;
  67. angle *acur, *aprev;
  68. double ve_ho_angle, ang = 1000, weight = 0;
  69. int c;
  70. GHashTableIter iter;
  71. guint i, objects = 0;
  72. gpointer key;
  73. object *value;
  74. v = t->sim.tm.vehicle;
  75. ve_ho_angle = atan2(-v.y, -v.x)*180/M_PI;
  76. tm = &t->sim.tm;
  77. g_sequence_insert_sorted(angles, angle_new(-120, 0), cmp_angle, NULL);
  78. g_sequence_insert_sorted(angles, angle_new(120, 0), cmp_angle, NULL);
  79. for (i = 0; i < tm->objects->len; i++) {
  80. object *o = &g_array_index(tm->objects, object, i);
  81. if (insert_object(angles, o, &v, ve_ho_angle)) objects++;
  82. }
  83. g_hash_table_iter_init(&iter, t->map.solid_objects);
  84. while (g_hash_table_iter_next(&iter, &key, (gpointer*) &value)) {
  85. if (insert_object(angles, value, &v, ve_ho_angle)) objects++;
  86. }
  87. prev = g_sequence_get_begin_iter(angles), end = g_sequence_get_end_iter(angles);
  88. if (prev != end) {
  89. aprev = (angle*) g_sequence_get(prev);
  90. c = aprev->c;
  91. while ((cur = g_sequence_iter_next(prev)) != end) {
  92. acur = (angle*) g_sequence_get(cur);
  93. assert(aprev->a <= acur->a);
  94. if (acur->a - aprev->a > 1) {
  95. double ax, w;
  96. if (aprev->a <= 0) {
  97. if (acur->a <= 0) {
  98. ax = acur->a;
  99. } else {
  100. ax = 0;
  101. }
  102. } else {
  103. ax = aprev->a;
  104. }
  105. w = 30 - fabs(ax)/30 - c;
  106. // fprintf(stderr, "Angle %f, weight %f\n", ax, w);
  107. if (w > weight) {
  108. // fprintf(stderr, "Selected %f\n", ax);
  109. ang = ax;
  110. weight = w;
  111. }
  112. }
  113. c += acur->c;
  114. prev = cur;
  115. aprev = acur;
  116. }
  117. }
  118. ang = fixangle(ang - v.dir + ve_ho_angle);
  119. // fprintf(stderr, "Angle: %f, objects: %u\n", ang, objects);
  120. if (ang < 1) {
  121. if (ang < -15) {
  122. vehicle_hard_right(t);
  123. } else {
  124. vehicle_right(t);
  125. }
  126. } else if (ang > 1) {
  127. if (ang > 15) {
  128. vehicle_hard_left(t);
  129. } else {
  130. vehicle_left(t);
  131. }
  132. }
  133. ang = fabs(ang);
  134. if (ang > 90) {
  135. vehicle_break(t);
  136. } else if (ang > 60) {
  137. vehicle_roll(t);
  138. } else {
  139. vehicle_accel(t);
  140. }
  141. }