projects/microb2010/tests/oa/main.c

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3 #include <stdint.h>
   4 #include <math.h>
   5
   6 /* XXX TODO
   7 static
   8 const
   9 change x,y -> i,j to avoid confusion with coords
  10 could be optimized in mem space: it is not needed to store the x,y coord,
  11    we can process it from idx. however it will be less optimized for speed
  12
  13 */
  14
  15 #define OFFSET_CORN_X 150
  16 #define OFFSET_CORN_Y 222
  17 #define STEP_CORN_X 225
  18 #define STEP_CORN_Y 250
  19
  20 #define CORN_NB 18
  21
  22 #define WAYPOINTS_NBX 13
  23 #define WAYPOINTS_NBY 8
  24
  25 /* enum is better */
  26 #define TYPE_WAYPOINT 0
  27 #define TYPE_DANGEROUS 1
  28 #define TYPE_WHITE_CORN 2
  29 #define TYPE_BLACK_CORN 3
  30 #define TYPE_OBSTACLE 4
  31
  32 /* XXX enum possible ? else just rename */
  33 #define START      0
  34 #define UP         1
  35 #define UP_RIGHT   2
  36 #define DOWN_RIGHT 3
  37 #define DOWN       4
  38 #define DOWN_LEFT  5
  39 #define UP_LEFT    6
  40 #define END        7
  41
  42 struct point {
  43         int32_t x;
  44         int32_t y;
  45 };
  46
  47 struct djpoint {
  48         struct point pos;
  49         uint16_t weight;
  50         struct djpoint *parent;
  51
  52         uint8_t type:3;
  53         uint8_t parent_pos:3;
  54         uint8_t updated:1;
  55         uint8_t todo:1;
  56 };
  57
  58 uint8_t corn_table[CORN_NB];
  59 static struct djpoint djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY];
  60
  61 /* table to find the symetric idx */
  62 uint8_t corn_sym[] = {
  63         15, 16, 17, 12, 13, 14, 10, 11, 8, 9, 6, 7, 3, 4, 5, 0, 1, 2
  64 };
  65
  66 uint8_t corn_side_confs[9][2] = {
  67         { 1, 4 },
  68         { 0, 4 },
  69         { 2, 4 },
  70         { 2, 3 },
  71         { 0, 3 },
  72         { 1, 3 },
  73         { 1, 6 },
  74         { 0, 6 },
  75         { 2, 6 },
  76 };
  77 uint8_t corn_center_confs[4][2] = {
  78         { 5, 8 },
  79         { 7, 8 },
  80         { 5, 9 },
  81         { 7, 8 },
  82 };
  83
  84
  85 /* return index from neigh pointer */
  86 #define PT2IDX(neigh) ( ((void *)(neigh)-(void *)(&djpoints)) / sizeof(*neigh) )
  87
  88 void dump(void)
  89 {
  90         int8_t i, j;
  91         struct djpoint *pt;
  92
  93         printf("         ");
  94         for (i=0; i<WAYPOINTS_NBX; i++) {
  95                 printf(" %2d ", i);
  96         }
  97         printf("\n");
  98
  99         for (j=WAYPOINTS_NBY*2-1; j>=0; j--) {
 100                 printf("%3d   ", j/2);
 101
 102                 if ((j&1) == 0)
 103                         printf("    ");
 104
 105                 for (i=0; i<WAYPOINTS_NBX; i++) {
 106                         pt = &djpoints[i][j/2];
 107
 108                         if (((i+j) & 1) == 0)
 109                                 continue;
 110
 111                         if (pt->type == TYPE_OBSTACLE)
 112                                 printf("     X  ");
 113                         else if (pt->type == TYPE_DANGEROUS)
 114                                 printf("     D  ");
 115                         else if (pt->type == TYPE_WHITE_CORN)
 116                                 printf("     W  ");
 117                         else if (pt->type == TYPE_BLACK_CORN)
 118                                 printf("     B  ");
 119                         else if (pt->type == TYPE_WAYPOINT)
 120                                 printf(" %5d  ", pt->weight);
 121                         else
 122                                 printf("     ?  ");
 123                 }
 124                 printf("\n");
 125         }
 126 }
 127
 128 static inline uint8_t opposite_position(uint8_t pos)
 129 {
 130         pos += 3;
 131         if (pos > UP_LEFT)
 132                 pos -= 6;
 133         return pos;
 134 }
 135
 136 /* return coord of the entry in the table from the pointer */
 137 static void djpoint2ij(struct djpoint *pt, int8_t *x, int8_t *y)
 138 {
 139         int8_t idx = PT2IDX(pt);
 140         *x = idx / WAYPOINTS_NBY;
 141         *y = idx % WAYPOINTS_NBY;
 142 }
 143
 144 /* get the neighbour of the point at specified position */
 145 static struct djpoint *get_neigh(struct djpoint *pt,
 146                                  uint8_t position)
 147 {
 148         int8_t i,j;
 149         struct djpoint *neigh;
 150
 151         djpoint2ij(pt, &i, &j);
 152
 153         switch (position) {
 154         case UP:
 155                 j++;
 156                 break;
 157         case UP_RIGHT:
 158                 if (!(i & 1)) j++;
 159                 i++;
 160                 break;
 161         case DOWN_RIGHT:
 162                 if (i & 1) j--;
 163                 i++;
 164                 break;
 165         case DOWN:
 166                 j--;
 167                 break;
 168         case DOWN_LEFT:
 169                 if (i & 1) j--;
 170                 i--;
 171                 break;
 172         case UP_LEFT:
 173                 if (!(i & 1)) j++;
 174                 i--;
 175                 break;
 176         default:
 177                 return NULL;
 178         }
 179         if (i < 0 || j < 0 || i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY)
 180                 return NULL;
 181
 182         neigh = &djpoints[i][j];
 183
 184         if (neigh->type != TYPE_WAYPOINT)
 185                 return NULL;
 186
 187         return neigh;
 188 }
 189
 190 static struct djpoint *get_next_neigh(struct djpoint *pt, uint8_t *position)
 191 {
 192         struct djpoint *neigh = NULL;
 193         uint8_t pos = *position + 1;
 194
 195         for (pos = *position + 1; pos < END; pos++) {
 196                 neigh = get_neigh(pt, pos);
 197                 if (neigh != NULL)
 198                         break;
 199         }
 200
 201         *position = pos;
 202         return neigh;
 203 }
 204
 205 /* browse all points */
 206 #define POINT_FOREACH(cur)                                              \
 207         for (cur = &djpoints[0][0];                                     \
 208              cur < &djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY];             \
 209              cur ++)
 210
 211 /* XXX comment */
 212 #define NEIGH_FOREACH(neigh, pos, point)                        \
 213         for (pos = START, neigh = get_next_neigh(point, &pos);  \
 214              neigh;                                             \
 215              neigh = get_next_neigh(point, &pos))
 216
 217 int dijkstra_init(void)
 218 {
 219         return 0;
 220 }
 221
 222 static uint16_t dist(struct djpoint *p1, struct djpoint *p2)
 223 {
 224         double vx, vy;
 225         vx = p2->pos.x - p1->pos.x;
 226         vy = p2->pos.y - p1->pos.y;
 227         return sqrt(vx * vx + vy * vy);
 228 }
 229
 230 void dijkstra_process_neighs(struct djpoint *pt)
 231 {
 232         struct djpoint *neigh;
 233         uint8_t pos, parent_pos;
 234         uint16_t weight;
 235         int8_t i,j;
 236
 237         djpoint2ij(pt, &i, &j);
 238         printf("at %d %d:\n", i, j);
 239
 240         NEIGH_FOREACH(neigh, pos, pt) {
 241                 weight = pt->weight + dist(pt, neigh);
 242                 parent_pos = opposite_position(pos);
 243
 244                 /* bonus if we keep the same direction */
 245                 if (parent_pos == pt->parent_pos ||
 246                     pt->parent_pos == END)
 247                         weight = weight - 1;
 248
 249                 printf("  pos=%d: ppos=%d opos=%d nw=%d w=%d\n", pos,
 250                        pt->parent_pos, parent_pos,
 251                        neigh->weight, weight);
 252                 if (neigh->weight == 0 || weight < neigh->weight) {
 253                         djpoint2ij(neigh, &i, &j);
 254                         //printf("    %d,%d updated\n", i, j);
 255                         neigh->weight = weight;
 256                         neigh->parent_pos = parent_pos;
 257                         neigh->updated = 1;
 258                 }
 259         }
 260 }
 261
 262 int dijkstra(struct djpoint *start)
 263 {
 264         struct djpoint *cur;
 265         uint8_t todolist = 1;
 266
 267         start->todo = 1;
 268
 269         while (todolist) {
 270                 printf("\n");
 271                 dump();
 272                 /* process all neighbours of todo list */
 273                 POINT_FOREACH(cur) {
 274                         if (!cur->todo)
 275                                 continue;
 276                         dijkstra_process_neighs(cur);
 277                         cur->todo = 0;
 278                 }
 279
 280                 /* convert updated list in todo list */
 281                 todolist = 0;
 282                 POINT_FOREACH(cur) {
 283                         if (!cur->updated)
 284                                 continue;
 285                         todolist = 1;
 286                         cur->todo = 1;
 287                         cur->updated = 0;
 288                 }
 289         }
 290         return 0; /* XXX */
 291 }
 292
 293 int8_t coord_to_corn_idx(int8_t i, int8_t j)
 294 {
 295         if (i == 0 && j == 2) return 0;
 296         if (i == 0 && j == 4) return 1;
 297         if (i == 0 && j == 6) return 2;
 298         if (i == 2 && j == 3) return 3;
 299         if (i == 2 && j == 5) return 4;
 300         if (i == 2 && j == 7) return 5;
 301         if (i == 4 && j == 4) return 6;
 302         if (i == 4 && j == 6) return 7;
 303         if (i == 6 && j == 5) return 8;
 304         if (i == 6 && j == 7) return 9;
 305         if (i == 8 && j == 4) return 10;
 306         if (i == 8 && j == 6) return 11;
 307         if (i == 10 && j == 3) return 12;
 308         if (i == 10 && j == 5) return 13;
 309         if (i == 10 && j == 7) return 14;
 310         if (i == 12 && j == 2) return 15;
 311         if (i == 12 && j == 4) return 16;
 312         if (i == 12 && j == 6) return 17;
 313         return -1;
 314 }
 315
 316 int8_t corn_get_sym(int8_t i)
 317 {
 318         if (i >= CORN_NB)
 319                 return -1;
 320         return corn_sym[i];
 321 }
 322
 323 void init_corn_table(uint8_t conf_side, uint8_t conf_center)
 324 {
 325         int8_t sym, i;
 326
 327         printf("confs = %d, %d\n", conf_side, conf_center);
 328         for (i=0; i<CORN_NB; i++) {
 329                 if (i == corn_side_confs[conf_side][0] ||
 330                     i == corn_side_confs[conf_side][1]) {
 331                         corn_table[i] = TYPE_BLACK_CORN;
 332                         continue;
 333                 }
 334                 sym = corn_get_sym(i);
 335                 if (sym == corn_side_confs[conf_side][0] ||
 336                     sym == corn_side_confs[conf_side][1]) {
 337                         corn_table[i] = TYPE_BLACK_CORN;
 338                         continue;
 339                 }
 340                 if (i == corn_center_confs[conf_center][0] ||
 341                     i == corn_center_confs[conf_center][1]) {
 342                         corn_table[i] = TYPE_BLACK_CORN;
 343                         continue;
 344                 }
 345                 sym = corn_get_sym(i);
 346                 if (sym == corn_center_confs[conf_center][0] ||
 347                     sym == corn_center_confs[conf_center][1]) {
 348                         corn_table[i] = TYPE_BLACK_CORN;
 349                         continue;
 350                 }
 351                 corn_table[i] = TYPE_WHITE_CORN;
 352         }
 353 }
 354
 355 /* init waypoints position */
 356 void init_waypoints(void)
 357 {
 358         int8_t i, j;
 359         int32_t x, y;
 360         struct djpoint *pt;
 361
 362         x = OFFSET_CORN_X;
 363         for (i=0; i<WAYPOINTS_NBX; i++) {
 364
 365                 if (i & 1)
 366                         y = OFFSET_CORN_Y - STEP_CORN_Y/2;
 367                 else
 368                         y = OFFSET_CORN_Y;
 369
 370                 for (j=0; j<WAYPOINTS_NBY; j++) {
 371                         pt = &djpoints[i][j];
 372
 373                         pt->pos.x = x;
 374                         pt->pos.y = y;
 375
 376                         pt->type = TYPE_WAYPOINT;
 377                         pt->parent_pos = END;
 378                         pt->updated = 0;
 379                         pt->todo = 0;
 380
 381                         y += STEP_CORN_Y;
 382                 }
 383
 384                 x += STEP_CORN_X;
 385         }
 386 }
 387
 388 /* update the type and weight of waypoints, before starting
 389  * dijkstra */
 390 void update_waypoints(void)
 391 {
 392         int8_t i, j, c;
 393         struct djpoint *pt;
 394
 395         for (i=0; i<WAYPOINTS_NBX; i++) {
 396
 397                 for (j=0; j<WAYPOINTS_NBY; j++) {
 398                         pt = &djpoints[i][j];
 399
 400                         pt->weight = 0;
 401
 402                         /* corn */
 403                         c = coord_to_corn_idx(i, j);
 404                         if (c >= 0) {
 405                                 pt->type = corn_table[c];
 406                                 continue;
 407                         }
 408                         /* too close of border */
 409                         if ((i & 1) == 1 && j == (WAYPOINTS_NBY-1)) {
 410                                 pt->type = TYPE_OBSTACLE;
 411                                 continue;
 412                         }
 413                         /* hill */
 414                         if (i >= 2 && i < (WAYPOINTS_NBX-2) && j < 2) {
 415                                 pt->type = TYPE_OBSTACLE;
 416                                 continue;
 417                         }
 418                         /* dangerous points */
 419                         if (i == 0 || i == (WAYPOINTS_NBX-1)) {
 420                                 pt->type = TYPE_DANGEROUS;
 421                                 continue;
 422                         }
 423                         if ( (i&1) == 0 && j == (WAYPOINTS_NBY-1)) {
 424                                 pt->type = TYPE_DANGEROUS;
 425                                 continue;
 426                         }
 427                         pt->type = TYPE_WAYPOINT;
 428                 }
 429         }
 430 }
 431
 432 int get_path(struct djpoint *start, struct djpoint *end)
 433 {
 434         struct djpoint *cur;
 435         uint8_t prev_direction = 0;
 436
 437         for (cur=start;
 438              cur != NULL && cur->parent_pos != END && cur != end;
 439              cur = get_neigh(cur, cur->parent_pos)) {
 440                 if (prev_direction != cur->parent_pos)
 441                         printf("%d %d (%d)\n", cur->pos.x, cur->pos.y, cur->parent_pos);
 442                 prev_direction = cur->parent_pos;
 443         }
 444         printf("%d %d\n", end->pos.x, end->pos.y);
 445
 446         return 0; /* XXX */
 447 }
 448
 449 int main(void)
 450 {
 451         struct djpoint *start;
 452         struct djpoint *end;
 453
 454         start = &djpoints[1][1];
 455         end = &djpoints[12][1];
 456
 457         init_corn_table(0, 0);
 458         init_waypoints();
 459         update_waypoints();
 460
 461         dijkstra(end);
 462         dump();
 463
 464         get_path(start, end);
 465
 466         return 0;
 467 }