From d045cbf77cbaaa784c5627e5cb8eee9155214229 Mon Sep 17 00:00:00 2001 From: "Stephen M. Cameron" Date: Mon, 2 May 2016 23:02:26 -0700 Subject: [PATCH] Add A-star module Signed-off-by: Stephen M. Cameron --- ccan/a_star/LICENSE | 1 + ccan/a_star/_info | 147 +++++++++++++++++++++ ccan/a_star/a_star.c | 294 +++++++++++++++++++++++++++++++++++++++++ ccan/a_star/a_star.h | 69 ++++++++++ ccan/a_star/test/run.c | 168 +++++++++++++++++++++++ 5 files changed, 679 insertions(+) create mode 120000 ccan/a_star/LICENSE create mode 100644 ccan/a_star/_info create mode 100644 ccan/a_star/a_star.c create mode 100644 ccan/a_star/a_star.h create mode 100644 ccan/a_star/test/run.c diff --git a/ccan/a_star/LICENSE b/ccan/a_star/LICENSE new file mode 120000 index 00000000..9961ca9d --- /dev/null +++ b/ccan/a_star/LICENSE @@ -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 index 00000000..2cfd941a --- /dev/null +++ b/ccan/a_star/_info @@ -0,0 +1,147 @@ +#include "config.h" +#include +#include + +/** + * 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 + * #include + * #include + * #include + * + * #include + * + * 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 index 00000000..ee08cab0 --- /dev/null +++ b/ccan/a_star/a_star.c @@ -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 +#include +#include +#include + +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 index 00000000..c37a36b1 --- /dev/null +++ b/ccan/a_star/a_star.h @@ -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 index 00000000..6189ebb4 --- /dev/null +++ b/ccan/a_star/test/run.c @@ -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 +#include +#include +#include + +#include +#include + +/* 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; +} -- 2.39.2