summaryrefslogtreecommitdiffhomepage
path: root/digital/avr
diff options
context:
space:
mode:
Diffstat (limited to 'digital/avr')
-rw-r--r--digital/avr/modules/path/astar/astar.c96
-rw-r--r--digital/avr/modules/path/astar/astar.h26
-rw-r--r--digital/avr/modules/path/astar/test/test_astar.c16
3 files changed, 138 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
+
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++)