Add A-star module
authorStephen M. Cameron <stephenmcameron@gmail.com>
Tue, 3 May 2016 06:02:26 +0000 (23:02 -0700)
committerStephen M. Cameron <stephenmcameron@gmail.com>
Tue, 3 May 2016 07:55:48 +0000 (00:55 -0700)
Signed-off-by: Stephen M. Cameron <stephenmcameron@gmail.com>
ccan/a_star/LICENSE [new symlink]
ccan/a_star/_info [new file with mode: 0644]
ccan/a_star/a_star.c [new file with mode: 0644]
ccan/a_star/a_star.h [new file with mode: 0644]
ccan/a_star/test/run.c [new file with mode: 0644]

diff --git a/ccan/a_star/LICENSE b/ccan/a_star/LICENSE
new file mode 120000 (symlink)
index 0000000..9961ca9
--- /dev/null
@@ -0,0 +1 @@
+../../licenses/GPL-2
\ No newline at end of file
diff --git a/ccan/a_star/_info b/ccan/a_star/_info
new file mode 100644 (file)
index 0000000..2cfd941
--- /dev/null
@@ -0,0 +1,147 @@
+#include "config.h"
+#include <stdio.h>
+#include <string.h>
+
+/**
+ * a_star - A straightforward implementation of the a-star path finding algorithm
+ *
+ * This code implements the A-star pathfinding algorithm without relying on any
+ * particular representation of the graph being searched other than that each node
+ * can be represented uniquely by a void pointer, which it treats as an opaque
+ * identifier.  To use it, you need to provide, besides a start and a goal node,
+ * a function to estimate the distance (however you choose to define that) and
+ * cost (however you choose to define that)  as a floating point value between
+ * two nodes, and a function to iterate over the neighbor nodes of any given
+ * node.   Additionally you may provide a context pointer (cookie) which is simply
+ * passed back to your callback functions
+ *
+ * Example:
+ * #include <stdio.h>
+ * #include <string.h>
+ * #include <stdlib.h>
+ * #include <math.h>
+ *
+ * #include <ccan/a_star/a_star.h>
+ *
+ * static char maze[] =
+ *     "##########@################x#################\n"
+ *     "#                  #                        #\n"
+ *     "#                  #                        #\n"
+ *     "#  ###########     ###################      #\n"
+ *     "#  #        #      #                 #      #\n"
+ *     "####        #      #                 #      #\n"
+ *     "#     #            #   ########      #      #\n"
+ *     "#     #    #########   #      #      #      #\n"
+ *     "#     #            #          #             #\n"
+ *     "#     #            #          #             #\n"
+ *     "#     #            #####   ##################\n"
+ *     "#     ##########   #                        #\n"
+ *     "#     #        #   #                        #\n"
+ *     "#              #########################    #\n"
+ *     "#          #           #           #        #\n"
+ *     "############           #     #     #        #\n"
+ *     "#                  #         #              #\n"
+ *     "#############################################\n";
+ *
+ * static int maze_width(char *maze)
+ * {
+ *     char *n;
+ *
+ *     n = strchr(maze, '\n');
+ *     return 1 + n - maze;
+ * }
+ *
+ * static int xoff[] = { 0, 1, 0, -1 };
+ * static int yoff[] = { 1, 0, -1, 0 };
+ *
+ * static char *maze_node(char *maze, int x, int y)
+ * {
+ *     return maze + y * maze_width(maze) + x;
+ * }
+ *
+ * static void *nth_neighbor(void *context, void *p, int n)
+ * {
+ *     char *maze = context;
+ *     char *c = p;
+ *     int i, x, y, offset = c - maze;
+ *
+ *     x = offset % maze_width(maze);
+ *     y = offset / maze_width(maze);
+ *
+ *     for (i = n; i < 4; i++) {
+ *             int tx = x + xoff[i];
+ *             int ty = y + yoff[i];
+ *             if (tx < 0 || ty < 0)
+ *                     continue;
+ *             if (ty * maze_width(maze) + tx > strlen(maze))
+ *                     continue;
+ *             c = maze_node(maze, x + xoff[i], y + yoff[i]);
+ *             if (*c != '#')
+ *                     return c;
+ *     }
+ *     return NULL;
+ * }
+ *
+ * static float maze_cost(void *context, void *first, void *second)
+ * {
+ *     char *maze = context;
+ *     char *f = first;
+ *     char *s = second;
+ *     int sx, sy, fx, fy;
+ *     float d, dx, dy;
+ *
+ *     int fp = f - maze;
+ *     int sp = s - maze;
+ *
+ *     sx = sp % maze_width(maze);
+ *     sy = sp / maze_width(maze);
+ *     fx = fp % maze_width(maze);
+ *     fy = fp / maze_width(maze);
+ *
+ *     dx = (float) sx - fx;
+ *     dy = (float) sy - fy;
+ *     d = (float) (abs(dx) + abs(dy));
+ *     return d;
+ * }
+ *
+ * int main(int argc, char *argv[])
+ * {
+ *     static int maxnodes, i;
+ *     char *start, *goal;
+ *     struct a_star_path *path;
+ *
+ *     start = strchr(maze, '@');
+ *     goal = strchr(maze, 'x');
+ *     maxnodes = strlen(maze);
+ *
+ *     path = a_star((void *) maze, start, goal, maxnodes, maze_cost, maze_cost, nth_neighbor);
+ *     if (!path) {
+ *             printf("a_star() failed to return a path.\n");
+ *             return 0;
+ *     }
+ *     for (i = 0; i < path->node_count; i++) {
+ *             char *p = path->path[i];
+ *             *p = '.';
+ *     }
+ *     *goal = 'x';
+ *     *start = '@';
+ *
+ *     printf("%s\n", maze);
+ *
+ *     free(path);
+ *     return 0;
+ * }
+ * License: GPL (v2 or any later version)
+ */
+int main(int argc, char *argv[])
+{
+       /* Expect exactly one argument */
+       if (argc != 2)
+               return 1;
+
+       if (strcmp(argv[1], "depends") == 0) {
+               return 0;
+       }
+
+       return 1;
+}
diff --git a/ccan/a_star/a_star.c b/ccan/a_star/a_star.c
new file mode 100644 (file)
index 0000000..ee08cab
--- /dev/null
@@ -0,0 +1,294 @@
+/*
+       Copyright (C) 2016 Stephen M. Cameron
+       Author: Stephen M. Cameron
+
+       This file is part of Spacenerds In Space.
+
+       Spacenerds in Space 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.
+
+       Spacenerds in Space 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 Spacenerds in Space; if not, write to the Free Software
+       Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+#include "a_star.h"
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <assert.h>
+
+struct nodeset {
+       int nmembers;
+       int maxmembers;
+       __extension__ void *node[0];
+};
+
+static int nodeset_empty(struct nodeset *n)
+{
+       return (n->nmembers == 0);
+}
+
+static void nodeset_add_node(struct nodeset *n, void *node)
+{
+       int i;
+
+       for (i = 0; i < n->nmembers; i++) {
+               if (n->node[i] == node)
+                       return;
+       }
+       assert(n->nmembers < n->maxmembers);
+       n->node[n->nmembers] = node;
+       n->nmembers++;
+}
+
+static void nodeset_remove_node(struct nodeset *n, void *node)
+{
+       int i;
+
+       for (i = 0; i < n->nmembers; i++) {
+               if (n->node[i] != node)
+                       continue;
+               if (i == n->nmembers - 1) {
+                       n->node[i] = NULL;
+                       n->nmembers--;
+                       return;
+               }
+               n->node[i] = n->node[n->nmembers - 1];
+               n->nmembers--;
+               n->node[n->nmembers] = NULL;
+               return;
+       }
+}
+
+static int nodeset_contains_node(struct nodeset *n, void *node)
+{
+       int i;
+
+       for (i = 0; i < n->nmembers; i++)
+               if (n->node[i] == node)
+                       return 1;
+       return 0;
+}
+
+static struct nodeset *nodeset_new(int maxnodes)
+{
+       struct nodeset *n;
+
+       n = malloc(sizeof(*n) + maxnodes * sizeof(void *));
+       memset(n, 0, sizeof(*n) + maxnodes * sizeof(void *));
+       n->maxmembers = maxnodes;
+       return n;
+}
+
+struct node_pair {
+       void *from, *to;
+};
+
+struct node_map {
+       int nelements;
+       __extension__ struct node_pair p[0];
+};
+
+struct score_entry {
+       void *node;
+       float score;
+};
+
+struct score_map {
+       int nelements;
+       __extension__ struct score_entry s[0];
+};
+
+static float score_map_get_score(struct score_map *m, void *node)
+{
+       int i;
+
+       for (i = 0; i < m->nelements; i++)
+               if (m->s[i].node == node)
+                       return m->s[i].score;
+       assert(0);
+}
+
+static void *lowest_score(struct nodeset *candidates, struct score_map *s)
+{
+
+       int i;
+       float score, lowest_score;
+       void *lowest = NULL;
+
+       for (i = 0; i < candidates->nmembers; i++) {
+               score = score_map_get_score(s, candidates->node[i]);
+               if (lowest != NULL && score > lowest_score)
+                       continue;
+               lowest = candidates->node[i];
+               lowest_score = score;
+       }
+       return lowest;
+}
+
+static struct score_map *score_map_new(int maxnodes)
+{
+       struct score_map *s;
+
+       s = malloc(sizeof(*s) + sizeof(struct score_entry) * maxnodes);
+       memset(s, 0, sizeof(*s) + sizeof(struct score_entry) * maxnodes);
+       s->nelements = maxnodes;
+       return s;
+}
+
+static void score_map_add_score(struct score_map *s, void *node, float score)
+{
+       int i;
+
+       for (i = 0; i < s->nelements; i++) {
+               if (s->s[i].node != node)
+                       continue;
+               s->s[i].score = score;
+               return;
+       }
+       for (i = 0; i < s->nelements; i++) {
+               if (s->s[i].node != NULL)
+                       continue;
+               s->s[i].node = node;
+               s->s[i].score = score;
+               return;
+       }
+       assert(0);
+}
+
+static struct node_map *node_map_new(int maxnodes)
+{
+       struct node_map *n;
+
+       n = malloc(sizeof(*n) + sizeof(struct node_pair) * maxnodes);
+       memset(n, 0, sizeof(*n) + sizeof(struct node_pair) * maxnodes);
+       n->nelements = maxnodes;
+       return n;
+}
+
+static void node_map_set_from(struct node_map *n, void *to, void *from)
+{
+       int i;
+
+       for (i = 0; i < n->nelements; i++) {
+               if (n->p[i].to != to)
+                       continue;
+               n->p[i].from = from;
+               return;
+       }
+       /* didn't find it, pick a NULL entry */
+       for (i = 0; i < n->nelements; i++) {
+               if (n->p[i].to != NULL)
+                       continue;
+               n->p[i].to = to;
+               n->p[i].from = from;
+               return;
+       }
+       assert(0); /* should never get here */
+}
+
+static void *node_map_get_from(struct node_map *n, void *to)
+{
+       int i;
+
+       for (i = 0; i < n->nelements; i++)
+               if (n->p[i].to == to)
+                       return n->p[i].from;
+       return NULL;
+}
+
+static void reconstruct_path(struct node_map *came_from, void *current, void ***path, int *nodecount, int maxnodes)
+{
+       int i;
+       void **p = malloc(sizeof(*p) * maxnodes);
+       memset(p, 0, sizeof(*p) * maxnodes);
+
+       for (i = 0; i < came_from->nelements; i++)
+               if (came_from->p[i].to == NULL)
+                       break;
+       p[0] = current;
+       i = 1;
+       while ((current = node_map_get_from(came_from, current))) {
+               p[i] = current;
+               i++;
+       }
+       *nodecount = i;
+       *path = p;
+}
+
+struct a_star_path *a_star(void *context, void *start, void *goal,
+                               int maxnodes,
+                               a_star_node_cost_fn distance,
+                               a_star_node_cost_fn cost_estimate,
+                               a_star_neighbor_iterator_fn nth_neighbor)
+{
+       struct nodeset *openset, *closedset;
+       struct node_map *came_from;
+       struct score_map *gscore, *fscore;
+       void *neighbor, *current;
+       float tentative_gscore;
+       int i, n;
+       void **answer = NULL;
+       int answer_count = 0;
+       struct a_star_path *return_value;
+
+       closedset = nodeset_new(maxnodes);
+       openset = nodeset_new(maxnodes);
+       came_from = node_map_new(maxnodes);
+       gscore = score_map_new(maxnodes);
+       fscore = score_map_new(maxnodes);
+
+       nodeset_add_node(openset, start);
+       score_map_add_score(gscore, start, 0.0);
+       score_map_add_score(fscore, start, cost_estimate(context, start, goal));
+
+       while (!nodeset_empty(openset)) {
+               current = lowest_score(openset, fscore);
+               if (current == goal) {
+                       reconstruct_path(came_from, current, &answer, &answer_count, maxnodes);
+                       break;
+               }
+               nodeset_remove_node(openset, current);
+               nodeset_add_node(closedset, current);
+               n = 0;
+               while ((neighbor = nth_neighbor(context, current, n))) {
+                       n++;
+                       if (nodeset_contains_node(closedset, neighbor))
+                               continue;
+                       tentative_gscore = score_map_get_score(gscore, current) + distance(context, current, neighbor);
+                       if (!nodeset_contains_node(openset, neighbor))
+                               nodeset_add_node(openset, neighbor);
+                       else if (tentative_gscore >= score_map_get_score(gscore, neighbor))
+                               continue;
+                       node_map_set_from(came_from, neighbor, current);
+                       score_map_add_score(gscore, neighbor, tentative_gscore);
+                       score_map_add_score(fscore, neighbor,
+                                       score_map_get_score(gscore, neighbor) +
+                                               cost_estimate(context, neighbor, goal));
+               }
+       }
+       free(closedset);
+       free(openset);
+       free(came_from);
+       free(gscore);
+       free(fscore);
+       if (answer_count == 0) {
+               return_value = NULL;
+       } else {
+               return_value = malloc(sizeof(*return_value) + sizeof(return_value->path[0]) * answer_count);
+               return_value->node_count = answer_count;
+               for (i = 0; i < answer_count; i++) {
+                       return_value->path[answer_count - i - 1] = answer[i];
+               }
+       }
+       free(answer);
+       return return_value;
+}
diff --git a/ccan/a_star/a_star.h b/ccan/a_star/a_star.h
new file mode 100644 (file)
index 0000000..c37a36b
--- /dev/null
@@ -0,0 +1,69 @@
+#ifndef A_STAR_H__
+#define A_STAR_H__
+/*
+       Copyright (C) 2016 Stephen M. Cameron
+       Author: Stephen M. Cameron
+
+       This file is part of Spacenerds In Space.
+
+       Spacenerds in Space 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.
+
+       Spacenerds in Space 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 Spacenerds in Space; if not, write to the Free Software
+       Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+struct a_star_path {
+       int node_count;
+       __extension__ void *path[0];
+};
+
+typedef float (*a_star_node_cost_fn)(void *context, void *first, void *second);
+typedef void *(*a_star_neighbor_iterator_fn)(void *context, void *node, int neighbor);
+
+/**
+ *
+ * a_star - runs the 'A*' algorithm to find a path from the start to the goal.
+ *
+ * @context: an opaque pointer which the algorithm does not use, but passes
+ *           along intact to the callback functions described below.
+ * @start: an opaque pointer to the start node
+ * @goal:  an opaque pointer to the goal node
+ *
+ * @maxnodes: the maximum number of nodes the algorithm will encounter (ie.
+ *           the number of possible locations in a maze you are traversing.)
+ *
+ * @distance: a function which you provide which returns the distance (however you
+ *            choose to define that) between two arbitrary nodes
+ *
+ * @cost_estimate: a function you provide which provides a heuristic cost estimate
+ *                for traversing between two nodes
+ *
+ * @nth_neighbor: a function you provide which returns the nth neighbor of a given
+ *                node, or NULL if n is greater than the number of neighbors - 1.
+ *
+ * Returns:
+ *   The return value is an allocated struct a_star_path.  the node_count is the
+ *   number of nodes in the path, and the path array is a pointer to an array of
+ *   node_count nodes in order from start to goal.
+ *
+ * See test/run.c example of how to use this.
+ *
+ */
+
+struct a_star_path *a_star(void *context,
+               void *start,
+               void *goal,
+               int maxnodes,
+               a_star_node_cost_fn distance,
+               a_star_node_cost_fn cost_estimate,
+               a_star_neighbor_iterator_fn nth_neighbor);
+#endif
diff --git a/ccan/a_star/test/run.c b/ccan/a_star/test/run.c
new file mode 100644 (file)
index 0000000..6189ebb
--- /dev/null
@@ -0,0 +1,168 @@
+/*
+       Copyright (C) 2016 Stephen M. Cameron
+       Author: Stephen M. Cameron
+
+       This file is part of Spacenerds In Space.
+
+       Spacenerds in Space 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.
+
+       Spacenerds in Space 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 Spacenerds in Space; if not, write to the Free Software
+       Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <math.h>
+
+#include <ccan/a_star/a_star.h>
+#include <ccan/a_star/a_star.c>
+
+/* Here is the maze we want to solve with A* algorithm */
+static char maze[] =
+       "##########@################x#################\n"
+       "#                  #                        #\n"
+       "#                  #                        #\n"
+       "#  ###########     ###################      #\n"
+       "#  #        #      #                 #      #\n"
+       "####        #      #                 #      #\n"
+       "#     #            #   ########      #      #\n"
+       "#     #    #########   #      #      #      #\n"
+       "#     #            #          #             #\n"
+       "#     #            #          #             #\n"
+       "#     #            #####   ##################\n"
+       "#     ##########   #                        #\n"
+       "#     #        #   #                        #\n"
+       "#              #########################    #\n"
+       "#          #           #           #        #\n"
+       "############           #     #     #        #\n"
+       "#                  #         #              #\n"
+       "#############################################\n";
+
+static const char solution[] =
+       "##########@################x#################\n"
+       "#         .....    #       .                #\n"
+       "#             .    #       ............     #\n"
+       "#  ###########.    ###################.     #\n"
+       "#  #        # .    #                 #.     #\n"
+       "#### .......#..    #  ............   #.     #\n"
+       "#    .#    ...     #  .########  ..  #.     #\n"
+       "#    .#    #########  .#      #   .  #.     #\n"
+       "#    .#            #  .....   #   .....     #\n"
+       "#    .#            #      .   #             #\n"
+       "#    .#            #####  .##################\n"
+       "#    .##########   #      ..                #\n"
+       "#    .#        #   #       ..............   #\n"
+       "#    ..........#########################.   #\n"
+       "#          #  .........#.........  #.....   #\n"
+       "############          .#.    #  ...#.       #\n"
+       "#                  #  ...    #    ...       #\n"
+       "#############################################\n";
+
+
+/* Return the width of the maze */
+static int maze_width(char *maze)
+{
+       char *n;
+
+       n = strchr(maze, '\n');
+       return 1 + n - maze;
+}
+
+/* offsets for 4 points north, east south, west */
+static int xoff[] = { 0, 1, 0, -1 };
+static int yoff[] = { 1, 0, -1, 0 };
+
+/* Convert x,y coords into a pointer to an element in the maze */
+static char *maze_node(char *maze, int x, int y)
+{
+       return maze + y * maze_width(maze) + x;
+}
+
+/* Return ptr to nth traversable neighbor of a node p, where 0 <= n <= 3, NULL if n out of range */
+static void *nth_neighbor(void *context, void *p, int n)
+{
+       char *maze = context;
+       char *c = p;
+       int i, x, y, offset = c - maze;
+
+       /* convert ptr to x,y coords */
+       x = offset % maze_width(maze);
+       y = offset / maze_width(maze);
+
+       for (i = n; i < 4; i++) {
+               int tx = x + xoff[i];
+               int ty = y + yoff[i];
+               if (tx < 0 || ty < 0) /* x,y out of range? */
+                       continue;
+               if (ty * maze_width(maze) + tx > strlen(maze)) /* out of range? */
+                       continue;
+               c = maze_node(maze, x + xoff[i], y + yoff[i]);
+               if (*c != '#') /* traversable? */
+                       return c;
+       }
+       return NULL;
+}
+
+static float maze_cost(void *context, void *first, void *second)
+{
+       char *maze = context;
+       char *f = first;
+       char *s = second;
+       int sx, sy, fx, fy;
+       float d, dx, dy;
+
+       int fp = f - maze;
+       int sp = s - maze;
+
+       sx = sp % maze_width(maze);
+       sy = sp / maze_width(maze);
+       fx = fp % maze_width(maze);
+       fy = fp / maze_width(maze);
+
+       dx = (float) sx - fx;
+       dy = (float) sy - fy;
+       d = (float) (abs(dx) + abs(dy)); /* manhattan distance */
+       return d;
+}
+
+int main(int argc, char *argv[])
+{
+       static int maxnodes, i;
+       char *start, *goal;
+       struct a_star_path *path;
+
+       start = strchr(maze, '@');
+       goal = strchr(maze, 'x');
+       maxnodes = strlen(maze);
+
+       path = a_star((void *) maze, start, goal, maxnodes, maze_cost, maze_cost, nth_neighbor);
+       if (!path) {
+               printf("a_star() failed to return a path.\n");
+               return 0;
+       }
+       for (i = 0; i < path->node_count; i++) {
+               char *p = path->path[i];
+               *p = '.';
+       }
+       *goal = 'x';
+       *start = '@';
+
+       printf("%s\n", maze);
+
+       free(path);
+
+       if (strcmp(solution, maze) == 0)
+               return 0;
+       else
+               return 1;
+}