From c199bfa0012d763f1e4684c3cd0ab6c07134ddce Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 23 Apr 2010 00:34:18 +0200 Subject: digital/avr/modules/path/astar: new A* module --- digital/avr/modules/path/astar/Makefile.module | 1 + digital/avr/modules/path/astar/README | 34 ++++ digital/avr/modules/path/astar/astar.c | 102 ++++++++++++ digital/avr/modules/path/astar/astar.h | 95 +++++++++++ digital/avr/modules/path/astar/avrconfig.h | 34 ++++ digital/avr/modules/path/astar/test/Makefile | 15 ++ digital/avr/modules/path/astar/test/avrconfig.h | 34 ++++ digital/avr/modules/path/astar/test/test_astar.c | 183 ++++++++++++++++++++++ digital/avr/modules/path/astar/test/test_astar.pl | 22 +++ 9 files changed, 520 insertions(+) create mode 100644 digital/avr/modules/path/astar/Makefile.module create mode 100644 digital/avr/modules/path/astar/README create mode 100644 digital/avr/modules/path/astar/astar.c create mode 100644 digital/avr/modules/path/astar/astar.h create mode 100644 digital/avr/modules/path/astar/avrconfig.h create mode 100644 digital/avr/modules/path/astar/test/Makefile create mode 100644 digital/avr/modules/path/astar/test/avrconfig.h create mode 100644 digital/avr/modules/path/astar/test/test_astar.c create mode 100644 digital/avr/modules/path/astar/test/test_astar.pl (limited to 'digital') 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 +#include +#include + +#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{^// }{}, ; + chomp @r; + die unless "@r" eq "@out"; +} -- cgit v1.2.3