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
15 #define OFFSET_CORN_X 150
16 #define OFFSET_CORN_Y 222
17 #define STEP_CORN_X 225
18 #define STEP_CORN_Y 250
22 #define WAYPOINTS_NBX 13
23 #define WAYPOINTS_NBY 8
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
32 /* XXX enum possible ? else just rename */
50 struct djpoint *parent;
58 uint8_t corn_table[CORN_NB];
59 static struct djpoint djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY];
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
66 uint8_t corn_side_confs[9][2] = {
77 uint8_t corn_center_confs[4][2] = {
85 /* return index from neigh pointer */
86 #define PT2IDX(neigh) ( ((void *)(neigh)-(void *)(&djpoints)) / sizeof(*neigh) )
94 for (i=0; i<WAYPOINTS_NBX; i++) {
99 for (j=WAYPOINTS_NBY*2-1; j>=0; j--) {
105 for (i=0; i<WAYPOINTS_NBX; i++) {
106 pt = &djpoints[i][j/2];
108 if (((i+j) & 1) == 0)
111 if (pt->type == TYPE_OBSTACLE)
113 else if (pt->type == TYPE_DANGEROUS)
115 else if (pt->type == TYPE_WHITE_CORN)
117 else if (pt->type == TYPE_BLACK_CORN)
119 else if (pt->type == TYPE_WAYPOINT)
120 printf(" %5d ", pt->weight);
128 static inline uint8_t opposite_position(uint8_t pos)
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)
139 int8_t idx = PT2IDX(pt);
140 *x = idx / WAYPOINTS_NBY;
141 *y = idx % WAYPOINTS_NBY;
144 /* get the neighbour of the point at specified position */
145 static struct djpoint *get_neigh(struct djpoint *pt,
149 struct djpoint *neigh;
151 djpoint2ij(pt, &i, &j);
179 if (i < 0 || j < 0 || i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY)
182 neigh = &djpoints[i][j];
184 if (neigh->type != TYPE_WAYPOINT)
190 static struct djpoint *get_next_neigh(struct djpoint *pt, uint8_t *position)
192 struct djpoint *neigh = NULL;
193 uint8_t pos = *position + 1;
195 for (pos = *position + 1; pos < END; pos++) {
196 neigh = get_neigh(pt, pos);
205 /* browse all points */
206 #define POINT_FOREACH(cur) \
207 for (cur = &djpoints[0][0]; \
208 cur < &djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY]; \
212 #define NEIGH_FOREACH(neigh, pos, point) \
213 for (pos = START, neigh = get_next_neigh(point, &pos); \
215 neigh = get_next_neigh(point, &pos))
217 int dijkstra_init(void)
222 static uint16_t dist(struct djpoint *p1, struct djpoint *p2)
225 vx = p2->pos.x - p1->pos.x;
226 vy = p2->pos.y - p1->pos.y;
227 return sqrt(vx * vx + vy * vy);
230 void dijkstra_process_neighs(struct djpoint *pt)
232 struct djpoint *neigh;
233 uint8_t pos, parent_pos;
237 djpoint2ij(pt, &i, &j);
238 printf("at %d %d:\n", i, j);
240 NEIGH_FOREACH(neigh, pos, pt) {
241 weight = pt->weight + dist(pt, neigh);
242 parent_pos = opposite_position(pos);
244 /* bonus if we keep the same direction */
245 if (parent_pos == pt->parent_pos ||
246 pt->parent_pos == END)
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;
262 int dijkstra(struct djpoint *start)
265 uint8_t todolist = 1;
272 /* process all neighbours of todo list */
276 dijkstra_process_neighs(cur);
280 /* convert updated list in todo list */
293 int8_t coord_to_corn_idx(int8_t i, int8_t j)
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;
316 int8_t corn_get_sym(int8_t i)
323 void init_corn_table(uint8_t conf_side, uint8_t conf_center)
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;
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;
340 if (i == corn_center_confs[conf_center][0] ||
341 i == corn_center_confs[conf_center][1]) {
342 corn_table[i] = TYPE_BLACK_CORN;
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;
351 corn_table[i] = TYPE_WHITE_CORN;
355 /* init waypoints position */
356 void init_waypoints(void)
363 for (i=0; i<WAYPOINTS_NBX; i++) {
366 y = OFFSET_CORN_Y - STEP_CORN_Y/2;
370 for (j=0; j<WAYPOINTS_NBY; j++) {
371 pt = &djpoints[i][j];
376 pt->type = TYPE_WAYPOINT;
377 pt->parent_pos = END;
388 /* update the type and weight of waypoints, before starting
390 void update_waypoints(void)
395 for (i=0; i<WAYPOINTS_NBX; i++) {
397 for (j=0; j<WAYPOINTS_NBY; j++) {
398 pt = &djpoints[i][j];
403 c = coord_to_corn_idx(i, j);
405 pt->type = corn_table[c];
408 /* too close of border */
409 if ((i & 1) == 1 && j == (WAYPOINTS_NBY-1)) {
410 pt->type = TYPE_OBSTACLE;
414 if (i >= 2 && i < (WAYPOINTS_NBX-2) && j < 2) {
415 pt->type = TYPE_OBSTACLE;
418 /* dangerous points */
419 if (i == 0 || i == (WAYPOINTS_NBX-1)) {
420 pt->type = TYPE_DANGEROUS;
423 if ( (i&1) == 0 && j == (WAYPOINTS_NBY-1)) {
424 pt->type = TYPE_DANGEROUS;
427 pt->type = TYPE_WAYPOINT;
432 int get_path(struct djpoint *start, struct djpoint *end)
435 uint8_t prev_direction = 0;
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;
444 printf("%d %d\n", end->pos.x, end->pos.y);
451 struct djpoint *start;
454 start = &djpoints[1][1];
455 end = &djpoints[12][1];
457 init_corn_table(0, 0);
464 get_path(start, end);