From 850cc292ac7ea847f6be2afcdc5bae89418cabda Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 30 May 2011 23:42:52 +0200 Subject: digital/io-hub: add path finding --- digital/io-hub/src/robospierre/path.c | 398 +++++++++++++++++++++++++++++++++- 1 file changed, 396 insertions(+), 2 deletions(-) (limited to 'digital/io-hub/src/robospierre/path.c') diff --git a/digital/io-hub/src/robospierre/path.c b/digital/io-hub/src/robospierre/path.c index b3c91ac8..1122c684 100644 --- a/digital/io-hub/src/robospierre/path.c +++ b/digital/io-hub/src/robospierre/path.c @@ -23,18 +23,235 @@ * * }}} */ #include "common.h" +#include "defs.h" #include "path.h" +#include "bot.h" +#include "playground.h" + +#include "modules/path/astar/astar.h" +#include "modules/utils/utils.h" +#include "modules/math/geometry/distance.h" + +#define PATH_DEBUG 0 + +#if PATH_DEBUG +#include "debug.host.h" +#endif + +/** + * This year, due to the large number of obstacles, a grid like structure is + * used for path finding on the playground. The A* algorithm is used to find + * path along nodes. + * + * The grid is composed of 11 columns of 4 node each. They are numbered by + * column. Even columns are aligned with center of squares, while odd columns + * are at squares intersections. Therefore, odd columns have a offset of + * 352/2 mm, and that is the reason why code should handle odd and even + * columns differently. + * + * All those tricks are used to reduce the number of nodes. + */ + +/** Number of possible obstacles. */ +#define PATH_OBSTACLES_NB AC_PATH_OBSTACLES_NB + +/** Number of nodes in a column. */ +#define PATH_COLUMN_NODES_NB 4 + +/** Number of columns. */ +#define PATH_COLUMNS_NB 11 + +/** Number of nodes in the grid. */ +#define PATH_GRID_NODES_NB (PATH_COLUMNS_NB * PATH_COLUMN_NODES_NB) + +/** Number of nodes in search graph, last two nodes are destination and source + * nodes. */ +#define PATH_NODES_NB (PATH_GRID_NODES_NB + 2) + +/** Index of destination node. */ +#define PATH_DST_NODE_INDEX PATH_GRID_NODES_NB + +/** Index of source node. */ +#define PATH_SRC_NODE_INDEX (PATH_DST_NODE_INDEX + 1) + +/** Information on a node. */ +struct path_node_t +{ + /** Whether this node can be used. */ + uint8_t usable; +}; /** Context. */ struct path_t { + /** List of obstacles. */ + struct path_obstacle_t obstacles[PATH_OBSTACLES_NB]; + /** Escape factor, 0 if none. */ + uint8_t escape_factor; + /** List of nodes used for A*. */ + struct astar_node_t astar_nodes[PATH_NODES_NB]; + /** Cache of whether a node is blocked. */ + uint8_t valid[PATH_GRID_NODES_NB]; /** Position of end points. */ vect_t endpoints[2]; /** Whether the last update was a success. */ uint8_t found; + /** Which node to look at for next step. */ + uint8_t get; }; static struct path_t path; +/** Static information on nodes. */ +static const struct path_node_t path_nodes[PATH_GRID_NODES_NB] = { + /* {{{ */ + { 1 }, /* 0 column 0. */ + { 1 }, /* 1 */ + { 1 }, /* 2 */ + { 1 }, /* 3 */ + { 1 }, /* 4 column 1. */ + { 1 }, /* 5 */ + { 1 }, /* 6 */ + { 1 }, /* 7 */ + { 1 }, /* 8 column 2. */ + { 1 }, /* 9 */ + { 1 }, /* 10 */ + { 1 }, /* 11 */ + { 1 }, /* 12 column 3. */ + { 1 }, /* 13 */ + { 1 }, /* 14 */ + { 1 }, /* 15 */ + { 1 }, /* 16 column 4. */ + { 1 }, /* 17 */ + { 1 }, /* 18 */ + { 1 }, /* 19 */ + { 1 }, /* 20 column 5. */ + { 1 }, /* 21 */ + { 1 }, /* 22 */ + { 1 }, /* 23 */ + { 1 }, /* 24 column 6. */ + { 1 }, /* 25 */ + { 1 }, /* 26 */ + { 1 }, /* 27 */ + { 1 }, /* 28 column 7. */ + { 1 }, /* 29 */ + { 1 }, /* 30 */ + { 1 }, /* 31 */ + { 1 }, /* 32 column 8. */ + { 1 }, /* 33 */ + { 1 }, /* 34 */ + { 1 }, /* 35 */ + { 1 }, /* 36 column 9. */ + { 1 }, /* 37 */ + { 1 }, /* 38 */ + { 1 }, /* 39 */ + { 1 }, /* 40 column 10. */ + { 1 }, /* 41 */ + { 1 }, /* 42 */ + { 1 }, /* 43 */ + /* }}} */ +}; + +/** Compute position of a node. */ +static void +path_pos (uint8_t node, vect_t *pos) +{ + assert (node < PATH_NODES_NB); + if (node < PATH_GRID_NODES_NB) + { + uint8_t col = node / PATH_COLUMN_NODES_NB; + uint8_t line = node - col * PATH_COLUMN_NODES_NB; + pos->x = 400 + 50 + 350 / 2 + col * 350 / 2; + pos->y = 2100 - 350 - 350 / 2 + + (col % 2 ? 350 / 2 : 0) + - line * 350; + } + else + { + *pos = path.endpoints[node - PATH_GRID_NODES_NB]; + } +} + +/** Return 1 if the direct path between a and b nodes is blocked, also compute + * distance. */ +static uint8_t +path_blocking (uint8_t a, uint8_t b, int16_t *dp) +{ + uint8_t i; + vect_t va; + vect_t vb; + uint8_t escape_factor = 0; + if (a == PATH_SRC_NODE_INDEX || b == PATH_SRC_NODE_INDEX) + escape_factor = path.escape_factor; + path_pos (a, &va); + path_pos (b, &vb); + /* Test for a blocking obstacle. */ + for (i = 0; i < PATH_OBSTACLES_NB; i++) + { + if (path.obstacles[i].valid) + { + uint16_t d = distance_segment_point (&va, &vb, + &path.obstacles[i].c); + if (d < path.obstacles[i].r) + { + if (escape_factor) + { + int16_t d = distance_point_point (&va, &vb); + *dp = d * escape_factor; + return 0; + } + else + return 1; + } + } + } + /* Compute distance. */ + int16_t d = distance_point_point (&va, &vb); + if (d == 0) + { + *dp = 0; + return 0; + } + /* No blocking. */ + *dp = d; + return 0; +} + +/** Update the cache of blocked nodes. */ +static void +path_blocked_update (void) +{ + uint8_t i, j; + for (i = 0; i < PATH_GRID_NODES_NB; i++) + { + uint8_t valid = 1; + /* First, gather information from tables. */ + if (!path_nodes[i].usable) + valid = 0; + else + { + vect_t pos; + path_pos (i, &pos); + /* Then, test for obstacles. */ + for (j = 0; j < PATH_OBSTACLES_NB; j++) + { + if (path.obstacles[j].valid) + { + vect_t v = pos; vect_sub (&v, &path.obstacles[j].c); + uint32_t dsq = vect_dot_product (&v, &v); + uint32_t r = path.obstacles[j].r; + if (dsq <= r * r) + { + valid = 0; + break; + } + } + } + } + /* Update cache. */ + path.valid[i] = valid; + } +} + void path_init (void) { @@ -50,23 +267,51 @@ path_endpoints (vect_t s, vect_t d) void path_escape (uint8_t factor) { + path.escape_factor = factor; } void path_obstacle (uint8_t i, vect_t c, uint16_t r, uint8_t factor, uint16_t valid) { + assert (i < AC_PATH_OBSTACLES_NB); + assert (factor == 0); + path.obstacles[i].c = c; + path.obstacles[i].r = r; + path.obstacles[i].valid = valid; } void path_decay (void) { + uint8_t i; + for (i = 0; i < PATH_OBSTACLES_NB; i++) + { + if (path.obstacles[i].valid + && path.obstacles[i].valid != PATH_OBSTACLE_VALID_ALWAYS) + path.obstacles[i].valid--; + } } void path_update (void) { - path.found = 1; + path_blocked_update (); + path.found = astar (path.astar_nodes, PATH_NODES_NB, PATH_DST_NODE_INDEX, + PATH_SRC_NODE_INDEX); + path.get = PATH_SRC_NODE_INDEX; +#if AC_PATH_REPORT + if (path.found) + { + uint8_t n, len = 0; + vect_t points[PATH_NODES_NB]; + for (n = path.get; n != PATH_DST_NODE_INDEX; n = path.astar_nodes[n].prev) + path_pos (n, &points[len++]); + path_pos (n, &points[len++]); + AC_PATH_REPORT_CALLBACK (points, len, path.obstacles, + PATH_OBSTACLES_NB); + } +#endif } uint8_t @@ -74,10 +319,159 @@ path_get_next (vect_t *p) { if (path.found) { - *p = path.endpoints[0]; + assert (path.get != PATH_DST_NODE_INDEX); + uint8_t prev = path.get; + vect_t pp; + path_pos (prev, &pp); + uint8_t next = path.astar_nodes[path.get].prev; + path.get = next; + path_pos (next, p); + while (next != 0xff) + { + /* Try to remove useless points. */ + uint8_t next = path.astar_nodes[path.get].prev; + if (next == 0xff || next == PATH_DST_NODE_INDEX) + break; + vect_t np; + path_pos (next, &np); + vect_t vnp = np; vect_sub (&vnp, &pp); + vect_t vp = *p; vect_sub (&vp, &pp); + if (vect_normal_dot_product (&vp, &vnp) == 0) + { + path.get = next; + *p = np; + } + else + break; + } return 1; } else return 0; } +/** Neighbors callback for nodes in grid. */ +static uint8_t +path_astar_neighbor_callback_grid (uint8_t node, + struct astar_neighbor_t *neighbors) +{ + uint8_t neighbors_nb = 0; + uint8_t i; + /* Add neighbors in all 8 directions. */ + static const struct { + /** Column offset of this neighbor. */ + int8_t column_offset; + /** Line offset of this neighbor, for even columns. */ + int8_t even_line_offset; + /** Line offset for odd columns. */ + int8_t odd_line_offset; + /** Distance to this neighbor. */ + uint16_t weight; + } star_n[] = { + { 0, -1, -1, 350 }, /* N */ + { -1, 0, -1, 248 }, /* NW */ + { -2, 0, 0, 350 }, /* W */ + { -1, 1, 0, 248 }, /* SW */ + { 0, 1, 1, 350 }, /* S */ + { 1, 1, 0, 248 }, /* SE */ + { 2, 0, 0, 350 }, /* E */ + { 1, 0, -1, 248 }, /* NE */ + }; + uint8_t col = node / PATH_COLUMN_NODES_NB; + uint8_t line = node - col * PATH_COLUMN_NODES_NB; + uint8_t odd = col % 2; + for (i = 0; i < UTILS_COUNT (star_n); i++) + { + int8_t new_col = col + star_n[i].column_offset; + int8_t new_line = line + (odd ? star_n[i].odd_line_offset + : star_n[i].even_line_offset); + int8_t new_node = new_col * PATH_COLUMN_NODES_NB + new_line; + if (new_col >= 0 && new_col < PATH_COLUMNS_NB + && new_line >= 0 && new_line < PATH_COLUMN_NODES_NB) + { + uint8_t valid = path.valid[new_node]; + if (valid) + { + neighbors[neighbors_nb].node = new_node; + neighbors[neighbors_nb].weight = star_n[i].weight + 1; + neighbors_nb++; + } + } + } + /* Check if direct path OK. */ + int16_t d; + if (!path_blocking (node, PATH_SRC_NODE_INDEX, &d)) + { + /* Add this neighbor. */ + neighbors[neighbors_nb].node = PATH_SRC_NODE_INDEX; + neighbors[neighbors_nb].weight = d + 1; + neighbors_nb++; + } +#if PATH_DEBUG + for (i = 0; i < neighbors_nb; i++) + DPRINTF (" n %d %d\n", neighbors[i].node, neighbors[i].weight); +#endif + return neighbors_nb; +} + +/** Neighbors callback for endpoints. */ +static uint8_t +path_astar_neighbor_callback_endpoints (uint8_t node, + struct astar_neighbor_t *neighbors) +{ + uint8_t neighbors_nb = 0; + uint8_t i; + assert (node == PATH_DST_NODE_INDEX); + /* Select neighbors in the grid. */ + for (i = 0; i < PATH_GRID_NODES_NB; i++) + { + /* Discard blocking nodes. */ + if (!path.valid[i]) + continue; + /* Check if there is an obstacle along the path. */ + int16_t d; + if (path_blocking (PATH_DST_NODE_INDEX, i, &d)) + continue; + /* Add this neighbor. */ + neighbors[neighbors_nb].node = i; + neighbors[neighbors_nb].weight = d + 1; + neighbors_nb++; + } + /* Check if direct path OK. */ + int16_t d; + if (!path_blocking (PATH_DST_NODE_INDEX, PATH_SRC_NODE_INDEX, &d)) + { + /* Add this neighbor. */ + neighbors[neighbors_nb].node = PATH_SRC_NODE_INDEX; + neighbors[neighbors_nb].weight = d + 1; + neighbors_nb++; + } +#if PATH_DEBUG + for (i = 0; i < neighbors_nb; i++) + DPRINTF (" n %d %d\n", neighbors[i].node, neighbors[i].weight); +#endif + return neighbors_nb; +} + +uint8_t +path_astar_neighbor_callback (uint8_t node, + struct astar_neighbor_t *neighbors) +{ +#if PATH_DEBUG + DPRINTF ("neighbor %d\n", node); +#endif + if (node < PATH_GRID_NODES_NB) + return path_astar_neighbor_callback_grid (node, neighbors); + else + return path_astar_neighbor_callback_endpoints (node, neighbors); +} + +uint16_t +path_astar_heuristic_callback (uint8_t node) +{ + /* TODO: a better and faster heuristic can be found, considering that + * movement is only allowed on the grid. */ + vect_t pos; + path_pos (node, &pos); + return distance_point_point (&pos, &path.endpoints[0]); +} -- cgit v1.2.3