You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

radar.c 3.8KB

12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161
  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. }