From 2692a63cd1b58b3f6ad90cf7541b0843fd799164 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 28 May 2012 23:19:24 +0200 Subject: digital/avr/modules/path/astar: new algorithm to evaluate many goals --- digital/avr/modules/path/astar/astar.c | 96 ++++++++++++++++++++++++ digital/avr/modules/path/astar/astar.h | 26 +++++++ digital/avr/modules/path/astar/test/test_astar.c | 16 ++++ 3 files changed, 138 insertions(+) (limited to 'digital/avr') diff --git a/digital/avr/modules/path/astar/astar.c b/digital/avr/modules/path/astar/astar.c index 4b898a99..4103682c 100644 --- a/digital/avr/modules/path/astar/astar.c +++ b/digital/avr/modules/path/astar/astar.c @@ -101,3 +101,99 @@ astar (struct astar_node_t *nodes, uint8_t nodes_nb, uint8_t initial, } } +/* Warning! + * The heuristic field is used to known if a node has been visited. */ +#define unvisited heuristic + +void +astar_dijkstra_prepare (struct astar_node_t *nodes, uint8_t nodes_nb, + uint8_t initial, uint8_t goal) +{ + /* Warning! + * What is following is NOT the A* algorithm. This look more like a + * Dijkstra algorithm as every nodes are processed. This is done because + * goal node information (and therefore heuristic) is unknown for now. */ + uint8_t i; + /* Initialise all nodes. */ + for (i = 0; i < nodes_nb; i++) + { + nodes[i].score = ASTAR_NODE_SCORE_UNVISITED; + nodes[i].unvisited = 1; + } + /* Start with the initial node and explore every single node. */ + nodes[initial].prev = 0xff; /* Not really read, no previous node. */ + nodes[initial].score = 0; + uint8_t lowest_node = initial; + uint16_t lowest_score = 0; + /* Loop until no node to consider. */ + while (1) + { + /* Mark the current node as visited, it will never be considered + * again. */ + nodes[lowest_node].unvisited = 0; + /* Now, process all its neighbors. */ + struct astar_neighbor_t neighbors[nodes_nb - 1]; + uint8_t neighbors_nb = AC_ASTAR_NEIGHBOR_CALLBACK (lowest_node, + neighbors); + for (i = 0; i < neighbors_nb; i++) + { + uint8_t neighbor = neighbors[i].node; + /* Never consider visited nodes. */ + if (!nodes[neighbor].unvisited) + continue; + /* See if the current node is better to arrive to this neighbor + * node. */ + uint16_t tentative_score = lowest_score + neighbors[i].weight; + if (tentative_score < nodes[neighbor].score) + { + nodes[neighbor].prev = lowest_node; + nodes[neighbor].score = tentative_score; + } + } + /* Find the next unvisited node with the lowest score. */ + lowest_node = 0xff; + lowest_score = ASTAR_NODE_SCORE_UNVISITED; + for (i = 0; i < nodes_nb; i++) + { + if (nodes[i].unvisited && i != goal + && nodes[i].score < lowest_score) + { + lowest_node = i; + lowest_score = nodes[i].score; + } + } + /* Found node is visited or not reachable from initial node, abort. */ + if (lowest_node == 0xff) + break; + } +} + +uint16_t +astar_dijkstra_finish (struct astar_node_t *nodes, uint8_t nodes_nb, + uint8_t goal) +{ + uint8_t i; + /* Find the shortest path from all of its neighbors. */ + struct astar_neighbor_t neighbors[nodes_nb - 1]; + uint8_t neighbors_nb = AC_ASTAR_NEIGHBOR_CALLBACK (goal, neighbors); + uint16_t best_score = (uint16_t) -1; + for (i = 0; i < neighbors_nb; i++) + { + uint8_t neighbor = neighbors[i].node; + /* Only consider reachable neighbors. */ + if (ASTAR_NODE_SCORE_OPEN (nodes[neighbor].score)) + { + uint16_t neighbor_score = nodes[neighbor].score + + neighbors[i].weight; + if (neighbor_score < best_score) + { + best_score = neighbor_score; + nodes[goal].prev = neighbor; + } + } + } + return best_score; +} + +#undef unvisited + diff --git a/digital/avr/modules/path/astar/astar.h b/digital/avr/modules/path/astar/astar.h index ee82b2cc..873ffba0 100644 --- a/digital/avr/modules/path/astar/astar.h +++ b/digital/avr/modules/path/astar/astar.h @@ -92,4 +92,30 @@ uint8_t astar (struct astar_node_t *nodes, uint8_t nodes_nb, uint8_t initial, uint8_t goal); +/** Not A* algorithm, to be used when a large number of goals must be tested. + * - nodes: array of all nodes. + * - nodes_nb: number of nodes. + * - initial: initial node index. + * - goal: goal node index, this node is not ready yet. + * + * This function will prepare nodes so that a large number of goal can be + * tested using the astar_dijkstra_finish function. The goal node is never + * inspected as it is not supposed to be ready yet. + * + * This is not using the A* algorithm, but put here anyway as it uses the same + * structures and callbacks. */ +void +astar_dijkstra_prepare (struct astar_node_t *nodes, uint8_t nodes_nb, + uint8_t initial, uint8_t goal); + +/** Not A* algorithm, to be called after astar_dijkstra_prepare to get the + * weight of the path to the goal node in a efficient way when a large number + * of goals must be tested. + * - nodes: array of all nodes. + * - nodes_nb: number of nodes. + * - goal: goal node index. */ +uint16_t +astar_dijkstra_finish (struct astar_node_t *nodes, uint8_t nodes_nb, + uint8_t goal); + #endif /* astar_h */ diff --git a/digital/avr/modules/path/astar/test/test_astar.c b/digital/avr/modules/path/astar/test/test_astar.c index ff1d8fb4..914fc065 100644 --- a/digital/avr/modules/path/astar/test/test_astar.c +++ b/digital/avr/modules/path/astar/test/test_astar.c @@ -125,6 +125,20 @@ main (int argc, const char **argv) { printf ("failure\n"); } + /* Print every nodes path weight. */ + printf ("\n"); + astar_dijkstra_prepare (nodes, test_width * test_height, initial, goal); + for (y = 0; y < test_height; y++) + { + printf ("///"); + for (x = 0; x < test_width; x++) + { + uint16_t w = astar_dijkstra_finish + (nodes, test_width * test_height, y * test_width + x); + printf (" %5d", (int16_t) w); + } + printf ("\n"); + } return 0; } @@ -147,6 +161,8 @@ test_neighbor_callback (uint8_t node, struct astar_neighbor_t *neighbors) y = node / test_width; if (ASTAR_DEBUG) printf ("neighbors %d, %d\n", x, y); + if (test_map[node] == 'X') + return 0; int neighbors_nb = 0; int i; for (i = 0; i < (int) UTILS_COUNT (n); i++) -- cgit v1.2.1