summaryrefslogtreecommitdiff
path: root/digital/avr/modules/path/astar/astar.c
diff options
context:
space:
mode:
Diffstat (limited to 'digital/avr/modules/path/astar/astar.c')
-rw-r--r--digital/avr/modules/path/astar/astar.c96
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
+