Add A-star module

Signed-off-by: Stephen M. Cameron <stephenmcameron@gmail.com>

ccan/a_star/LICENSE

+../../licenses/GPL-2
\ No newline at end of file

ccan/a_star/_info

+#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;
+}

ccan/a_star/a_star.c

+/*
+	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;
+}

ccan/a_star/a_star.h

+#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

ccan/a_star/test/run.c

+/*
+	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;
+}