summaryrefslogtreecommitdiff
path: root/digital/avr
diff options
context:
space:
mode:
Diffstat (limited to 'digital/avr')
-rw-r--r--digital/avr/modules/path/astar/Makefile.module1
-rw-r--r--digital/avr/modules/path/astar/README34
-rw-r--r--digital/avr/modules/path/astar/astar.c102
-rw-r--r--digital/avr/modules/path/astar/astar.h95
-rw-r--r--digital/avr/modules/path/astar/avrconfig.h34
-rw-r--r--digital/avr/modules/path/astar/test/Makefile15
-rw-r--r--digital/avr/modules/path/astar/test/avrconfig.h34
-rw-r--r--digital/avr/modules/path/astar/test/test_astar.c183
-rw-r--r--digital/avr/modules/path/astar/test/test_astar.pl22
9 files changed, 520 insertions, 0 deletions
diff --git a/digital/avr/modules/path/astar/Makefile.module b/digital/avr/modules/path/astar/Makefile.module
new file mode 100644
index 00000000..60d32538
--- /dev/null
+++ b/digital/avr/modules/path/astar/Makefile.module
@@ -0,0 +1 @@
+path_astar_SOURCES = astar.c
diff --git a/digital/avr/modules/path/astar/README b/digital/avr/modules/path/astar/README
new file mode 100644
index 00000000..a0af2d86
--- /dev/null
+++ b/digital/avr/modules/path/astar/README
@@ -0,0 +1,34 @@
+avr.path.astar - A* path finding module.
+
+Path finding in a user provided graph. One of the main idea behind this
+module is to be able to operate with a minimum memory footprint even with a
+large number of nodes.
+
+This can be useful with a grid based graph, where there is many nodes, but not
+so many arcs, and arc weight (which are not stored by this module) can be
+computed quickly.
+
+This module use an indexed nodes storage, therefore, memory consumption is
+O(n), with n the number of nodes. For very large grids, another
+implementation could be done where unused nodes are not stored in memory.
+
+
+Copyright (C) 2010 Nicolas Schodet
+
+APBTeam:
+ Web: http://apbteam.org/
+ Email: team AT apbteam DOT org
+
+This program is free software; you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation; either version 2 of the License, or
+(at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program; if not, write to the Free Software
+Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
diff --git a/digital/avr/modules/path/astar/astar.c b/digital/avr/modules/path/astar/astar.c
new file mode 100644
index 00000000..a5d4bd48
--- /dev/null
+++ b/digital/avr/modules/path/astar/astar.c
@@ -0,0 +1,102 @@
+/* astar.c */
+/* avr.path.astar - A* path finding module. {{{
+ *
+ * Copyright (C) 2010 Nicolas Schodet
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+#include "common.h"
+#include "astar.h"
+
+uint8_t
+astar (struct astar_node_t *nodes, uint8_t nodes_nb, uint8_t initial,
+ uint8_t goal)
+{
+ uint8_t i;
+ /* Initialise all nodes. */
+ for (i = 0; i < nodes_nb; i++)
+ {
+ nodes[i].score = ASTAR_NODE_SCORE_UNVISITED;
+ nodes[i].heuristic = 0;
+ }
+ /* Start with the initial node. */
+ nodes[initial].prev = 0xff; /* Not really read, no previous node. */
+ nodes[initial].score = 0;
+ nodes[initial].heuristic = AC_ASTAR_HEURISTIC_CALLBACK (initial);
+ /* Loop until no node to consider. */
+ while (1)
+ {
+ /* Find the node with the lowest total weight (score + heuristic) in
+ * the open set. The score field special values has been chosen so
+ * that there is no extra test to select only nodes in the open set.
+ *
+ * If this code is too slow, this search should be replaced using a
+ * proper data structure (sorted list, heap...). */
+ uint8_t lowest_node = 0;
+ uint16_t lowest_weight = nodes[0].score + nodes[0].heuristic;
+ for (i = 1; i < nodes_nb; i++)
+ {
+ if (nodes[i].score + nodes[i].heuristic < lowest_weight)
+ {
+ lowest_node = i;
+ lowest_weight = nodes[i].score + nodes[i].heuristic;
+ }
+ }
+ /* If "found" node is not in the open set, there was no node found,
+ * abort. */
+ uint16_t lowest_score = nodes[lowest_node].score;
+ if (!ASTAR_NODE_SCORE_OPEN (lowest_score))
+ return 0;
+ /* If this is the goal, report our success. */
+ if (lowest_node == goal)
+ return 1;
+ /* OK, there is some work, move this node to the closed set, it will
+ * never be consider again. */
+ nodes[lowest_node].score = ASTAR_NODE_SCORE_CLOSED;
+ /* 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 nodes in the closed set. */
+ if (nodes[neighbor].score == ASTAR_NODE_SCORE_CLOSED)
+ continue;
+ /* See if our lowest_node is better to arrive to this neighbor
+ * node (node not considered yet, of new score is better). Note
+ * that due to the score assigned to unvisited nodes, there is
+ * only one test. */
+ uint16_t tentative_score = lowest_score + neighbors[i].weight;
+ if (tentative_score < nodes[neighbor].score)
+ {
+ nodes[neighbor].prev = lowest_node;
+ /* Assign the new score, which as the side effect to put this
+ * node in the open set. */
+ nodes[neighbor].score = tentative_score;
+ /* Update heuristic if not done yet. */
+ if (!nodes[neighbor].heuristic)
+ nodes[neighbor].heuristic =
+ AC_ASTAR_HEURISTIC_CALLBACK (neighbor);
+ }
+ }
+ }
+}
+
diff --git a/digital/avr/modules/path/astar/astar.h b/digital/avr/modules/path/astar/astar.h
new file mode 100644
index 00000000..5122b6c3
--- /dev/null
+++ b/digital/avr/modules/path/astar/astar.h
@@ -0,0 +1,95 @@
+#ifndef astar_h
+#define astar_h
+/* astar.h */
+/* avr.path.astar - A* path finding module. {{{
+ *
+ * Copyright (C) 2010 Nicolas Schodet
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+
+/** Used instead of score for a unvisited node. */
+#define ASTAR_NODE_SCORE_UNVISITED 0xffff
+/** Used instead of score for a node in the closed set. */
+#define ASTAR_NODE_SCORE_CLOSED 0xfffe
+/** Test whether a node is in the open set. */
+#define ASTAR_NODE_SCORE_OPEN(score) ((score) < ASTAR_NODE_SCORE_CLOSED)
+
+/** Node information. Instantiated by the client in an array to store
+ * information on every nodes in the graph. */
+struct astar_node_t
+{
+ /** Previous node in path. */
+ uint8_t prev;
+ /** Score of this node (often named g(x) in A* literature),
+ * ASTAR_NODE_SCORE_UNVISITED if unvisited, or ASTAR_NODE_SCORE_CLOSED if
+ * in the closed set. Score is the sum of all weights from the initial
+ * node to this node. Every nodes with a score are in the open set. */
+ uint16_t score;
+ /** Heuristic weight from this node to the goal (often named h(x) in A*
+ * literature). It will be computed only once, so this is initialised to
+ * 0 and set using a user provided function. */
+ uint16_t heuristic;
+};
+
+/** Information filled by the user neighbor callback. */
+struct astar_neighbor_t
+{
+ /** Neighbor node to inspect. */
+ uint8_t node;
+ /** Weight of the arc between the requested node and this neighbor
+ * node. */
+ uint16_t weight;
+};
+
+/** Neighbor callback.
+ * - node: currently inspected node.
+ * - neighbors: array to be filled by this callback.
+ * - returns: number of neighbors.
+ * This callback is to be provided by the user. It should fill the neighbor
+ * array with all neighbors reachable from the requested node. */
+uint8_t
+AC_ASTAR_NEIGHBOR_CALLBACK (uint8_t node, struct astar_neighbor_t *neighbors);
+
+/** Heuristic callback.
+ * - node: considered node.
+ * - returns: heuristic weight from the considered node to the goal.
+ * This callback is to be provided by the user. This should be an optimistic
+ * heuristic or A* may not found the best path. */
+uint16_t
+AC_ASTAR_HEURISTIC_CALLBACK (uint8_t node);
+
+/** A* algorithm.
+ * - nodes: array of all nodes.
+ * - nodes_nb: number of nodes.
+ * - initial: initial node index.
+ * - goal: goal node index.
+ * - returns: non zero if a path is found.
+ * If a path is found, user can extract the found path by following the prev
+ * member, starting from the goal node.
+ *
+ * It may be more convenient to invert initial and goal so that the path can
+ * be followed from the starting point to the destination point instead of the
+ * other way round. */
+uint8_t
+astar (struct astar_node_t *nodes, uint8_t nodes_nb, uint8_t initial,
+ uint8_t goal);
+
+#endif /* astar_h */
diff --git a/digital/avr/modules/path/astar/avrconfig.h b/digital/avr/modules/path/astar/avrconfig.h
new file mode 100644
index 00000000..9084a96f
--- /dev/null
+++ b/digital/avr/modules/path/astar/avrconfig.h
@@ -0,0 +1,34 @@
+#ifndef avrconfig_h
+#define avrconfig_h
+/* avrconfig.h - Configuration template. */
+/* avr.path.astar - A* path finding module. {{{
+ *
+ * Copyright (C) 2010 Nicolas Schodet
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+
+/* astar - A* path finding module. */
+/** Neighbor callback. */
+#define AC_ASTAR_NEIGHBOR_CALLBACK astar_neighbor_callback
+/** Heuristic callback. */
+#define AC_ASTAR_HEURISTIC_CALLBACK astar_heuristic_callback
+
+#endif /* avrconfig_h */
diff --git a/digital/avr/modules/path/astar/test/Makefile b/digital/avr/modules/path/astar/test/Makefile
new file mode 100644
index 00000000..c6b81faf
--- /dev/null
+++ b/digital/avr/modules/path/astar/test/Makefile
@@ -0,0 +1,15 @@
+BASE = ../../../..
+HOST_PROGS = test_astar
+test_astar_SOURCES = test_astar.c
+MODULES = path/astar
+CONFIGFILE = avrconfig.h
+# atmega8, atmega8535, atmega128...
+AVR_MCU = atmega128
+# -O2 : speed
+# -Os : size
+OPTIMIZE = -O2
+
+include $(BASE)/make/Makefile.gen
+
+check: test_astar.pl test_astar.host
+ perl $^
diff --git a/digital/avr/modules/path/astar/test/avrconfig.h b/digital/avr/modules/path/astar/test/avrconfig.h
new file mode 100644
index 00000000..279d1477
--- /dev/null
+++ b/digital/avr/modules/path/astar/test/avrconfig.h
@@ -0,0 +1,34 @@
+#ifndef avrconfig_h
+#define avrconfig_h
+/* avrconfig.h */
+/* avr.path.astar - A* path finding module. {{{
+ *
+ * Copyright (C) 2010 Nicolas Schodet
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+
+/* astar - A* path finding module. */
+/** Neighbor callback. */
+#define AC_ASTAR_NEIGHBOR_CALLBACK test_neighbor_callback
+/** Heuristic callback. */
+#define AC_ASTAR_HEURISTIC_CALLBACK test_heuristic_callback
+
+#endif /* avrconfig_h */
diff --git a/digital/avr/modules/path/astar/test/test_astar.c b/digital/avr/modules/path/astar/test/test_astar.c
new file mode 100644
index 00000000..ff1d8fb4
--- /dev/null
+++ b/digital/avr/modules/path/astar/test/test_astar.c
@@ -0,0 +1,183 @@
+/* test_astar.c */
+/* avr.path.astar - A* path finding module. {{{
+ *
+ * Copyright (C) 2010 Nicolas Schodet
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+#include "common.h"
+#include "modules/path/astar/astar.h"
+#include "modules/utils/utils.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#define ASTAR_DEBUG 0
+
+void
+syntax (void)
+{
+ fprintf (stderr,
+ "test_astar map...\n"
+ " map: map lines, use `_' for free nodes, `X' for blocked"
+ " nodes, `i' for\n"
+ "initial node and `g' for goal node.\n"
+ "example: test_astar iX___\n"
+ " _X_X_\n"
+ " ___Xg\n"
+ "This test program will use a rectangular grid but the astar"
+ " module can be\n"
+ "used with many other graph.\n");
+ exit (1);
+}
+
+/** Cost of an orthogonal move. */
+#define TEST_WEIGHT_ORTH 100
+
+/** Cost of a diagonal move. */
+#define TEST_WEIGHT_DIAG 141
+
+/** Global as there is no context. */
+int test_width, test_height, test_initial_x, test_initial_y;
+
+/** Global map, see syntax. */
+char *test_map;
+
+int
+main (int argc, const char **argv)
+{
+ int x, y, i;
+ const char **maps, *p;
+ int initial = -1, goal = -1;
+ if (argc < 2)
+ syntax ();
+ test_width = strlen (argv[1]);
+ test_height = argc - 1;
+ char map[test_width * test_height + 1];
+ test_map = map;
+ /* Read map. */
+ for (y = 0, i = 0, maps = &argv[1];
+ y < test_height;
+ y++, maps++)
+ for (x = 0, p = *maps;
+ x < test_width;
+ x++, i++)
+ {
+ map[i] = *p;
+ switch (*p++)
+ {
+ case 'g':
+ goal = y * test_width + x;
+ break;
+ case 'i':
+ initial = y * test_width + x;
+ test_initial_x = x;
+ test_initial_y = y;
+ break;
+ case '_':
+ case 'X':
+ break;
+ default:
+ syntax ();
+ }
+ }
+ map[i] = '\0';
+ if (initial == -1 || goal == -1)
+ syntax ();
+ /* Compute path, invert initial and goal so that path can be easily
+ * extracted. */
+ struct astar_node_t nodes[test_width * test_height];
+ if (astar (nodes, test_width * test_height, goal, initial))
+ {
+ int n = initial, nn = -1, ord = 0;
+ while (nn != goal)
+ {
+ int x = n % test_width,
+ y = n / test_width;
+ printf ("%d, %d\n", x, y);
+ map[n] = '0' + ord++ % 10;
+ nn = n;
+ n = nodes[n].prev;
+ }
+ printf ("\n");
+ for (y = 0; y < test_height; y++)
+ printf ("// %.*s\n", test_width, map + y * test_width);
+ }
+ else
+ {
+ printf ("failure\n");
+ }
+ return 0;
+}
+
+uint8_t
+test_neighbor_callback (uint8_t node, struct astar_neighbor_t *neighbors)
+{
+ static const struct {
+ int dx, dy, weight;
+ } n[] = {
+ { -1, -1, TEST_WEIGHT_DIAG }, /* NW */
+ { 0, -1, TEST_WEIGHT_ORTH }, /* N */
+ { 1, -1, TEST_WEIGHT_DIAG }, /* NE */
+ { -1, 0, TEST_WEIGHT_ORTH }, /* W */
+ { 1, 0, TEST_WEIGHT_ORTH }, /* E */
+ { -1, 1, TEST_WEIGHT_DIAG }, /* SW */
+ { 0, 1, TEST_WEIGHT_ORTH }, /* S */
+ { 1, 1, TEST_WEIGHT_DIAG }, /* SE */
+ };
+ int x = node % test_width,
+ y = node / test_width;
+ if (ASTAR_DEBUG)
+ printf ("neighbors %d, %d\n", x, y);
+ int neighbors_nb = 0;
+ int i;
+ for (i = 0; i < (int) UTILS_COUNT (n); i++)
+ {
+ int nx = x + n[i].dx,
+ ny = y + n[i].dy;
+ if (nx < 0 || nx >= test_width || ny < 0 || ny >= test_height)
+ continue;
+ int node = ny * test_width + nx;
+ if (test_map[node] == 'X')
+ continue;
+ neighbors[neighbors_nb].node = node;
+ neighbors[neighbors_nb].weight = n[i].weight;
+ neighbors_nb++;
+ }
+ return neighbors_nb;
+}
+
+uint16_t
+test_heuristic_callback (uint8_t node)
+{
+ /* Diagonal move are allowed, but not any angle. */
+ int x = node % test_width,
+ y = node / test_width;
+ int dx = UTILS_ABS (test_initial_x - x),
+ dy = UTILS_ABS (test_initial_y - y);
+ int min = UTILS_MIN (dx, dy),
+ max = UTILS_MAX (dx, dy);
+ int h = (max - min) * TEST_WEIGHT_ORTH + min * TEST_WEIGHT_DIAG;
+ if (ASTAR_DEBUG)
+ printf ("heuristic %d, %d, %d\n", x, y, h);
+ return h;
+}
+
diff --git a/digital/avr/modules/path/astar/test/test_astar.pl b/digital/avr/modules/path/astar/test/test_astar.pl
new file mode 100644
index 00000000..88fcc984
--- /dev/null
+++ b/digital/avr/modules/path/astar/test/test_astar.pl
@@ -0,0 +1,22 @@
+#!/usr/bin/perl
+use strict;
+use warnings;
+
+my @tests = (
+ [ 'iX___',
+ '_X_X_',
+ '___Xg' ],
+ [ '0X_4_',
+ '1X3X5',
+ '_2_X6' ],
+ );
+
+while (@tests)
+{
+ my @in = @{shift @tests};
+ my @out = @{shift @tests};
+ open OUT, "./test_astar.host @in|" or die;
+ my @r = grep s{^// }{}, <OUT>;
+ chomp @r;
+ die unless "@r" eq "@out";
+}