**diff options**

Diffstat (limited to 'digital/avr/modules/path/astar/astar.c')

-rw-r--r-- | digital/avr/modules/path/astar/astar.c | 96 |

1 files changed, 96 insertions, 0 deletions

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 + |