Add A-star module
[ccan] / ccan / a_star / test / run.c
1 /*
2         Copyright (C) 2016 Stephen M. Cameron
3         Author: Stephen M. Cameron
4
5         This file is part of Spacenerds In Space.
6
7         Spacenerds in Space is free software; you can redistribute it and/or modify
8         it under the terms of the GNU General Public License as published by
9         the Free Software Foundation; either version 2 of the License, or
10         (at your option) any later version.
11
12         Spacenerds in Space is distributed in the hope that it will be useful,
13         but WITHOUT ANY WARRANTY; without even the implied warranty of
14         MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15         GNU General Public License for more details.
16
17         You should have received a copy of the GNU General Public License
18         along with Spacenerds in Space; if not, write to the Free Software
19         Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
20 */
21
22 #include <stdio.h>
23 #include <string.h>
24 #include <stdlib.h>
25 #include <math.h>
26
27 #include <ccan/a_star/a_star.h>
28 #include <ccan/a_star/a_star.c>
29
30 /* Here is the maze we want to solve with A* algorithm */
31 static char maze[] =
32         "##########@################x#################\n"
33         "#                  #                        #\n"
34         "#                  #                        #\n"
35         "#  ###########     ###################      #\n"
36         "#  #        #      #                 #      #\n"
37         "####        #      #                 #      #\n"
38         "#     #            #   ########      #      #\n"
39         "#     #    #########   #      #      #      #\n"
40         "#     #            #          #             #\n"
41         "#     #            #          #             #\n"
42         "#     #            #####   ##################\n"
43         "#     ##########   #                        #\n"
44         "#     #        #   #                        #\n"
45         "#              #########################    #\n"
46         "#          #           #           #        #\n"
47         "############           #     #     #        #\n"
48         "#                  #         #              #\n"
49         "#############################################\n";
50
51 static const char solution[] =
52         "##########@################x#################\n"
53         "#         .....    #       .                #\n"
54         "#             .    #       ............     #\n"
55         "#  ###########.    ###################.     #\n"
56         "#  #        # .    #                 #.     #\n"
57         "#### .......#..    #  ............   #.     #\n"
58         "#    .#    ...     #  .########  ..  #.     #\n"
59         "#    .#    #########  .#      #   .  #.     #\n"
60         "#    .#            #  .....   #   .....     #\n"
61         "#    .#            #      .   #             #\n"
62         "#    .#            #####  .##################\n"
63         "#    .##########   #      ..                #\n"
64         "#    .#        #   #       ..............   #\n"
65         "#    ..........#########################.   #\n"
66         "#          #  .........#.........  #.....   #\n"
67         "############          .#.    #  ...#.       #\n"
68         "#                  #  ...    #    ...       #\n"
69         "#############################################\n";
70
71
72 /* Return the width of the maze */
73 static int maze_width(char *maze)
74 {
75         char *n;
76
77         n = strchr(maze, '\n');
78         return 1 + n - maze;
79 }
80
81 /* offsets for 4 points north, east south, west */
82 static int xoff[] = { 0, 1, 0, -1 };
83 static int yoff[] = { 1, 0, -1, 0 };
84
85 /* Convert x,y coords into a pointer to an element in the maze */
86 static char *maze_node(char *maze, int x, int y)
87 {
88         return maze + y * maze_width(maze) + x;
89 }
90
91 /* Return ptr to nth traversable neighbor of a node p, where 0 <= n <= 3, NULL if n out of range */
92 static void *nth_neighbor(void *context, void *p, int n)
93 {
94         char *maze = context;
95         char *c = p;
96         int i, x, y, offset = c - maze;
97
98         /* convert ptr to x,y coords */
99         x = offset % maze_width(maze);
100         y = offset / maze_width(maze);
101
102         for (i = n; i < 4; i++) {
103                 int tx = x + xoff[i];
104                 int ty = y + yoff[i];
105                 if (tx < 0 || ty < 0) /* x,y out of range? */
106                         continue;
107                 if (ty * maze_width(maze) + tx > strlen(maze)) /* out of range? */
108                         continue;
109                 c = maze_node(maze, x + xoff[i], y + yoff[i]);
110                 if (*c != '#') /* traversable? */
111                         return c;
112         }
113         return NULL;
114 }
115
116 static float maze_cost(void *context, void *first, void *second)
117 {
118         char *maze = context;
119         char *f = first;
120         char *s = second;
121         int sx, sy, fx, fy;
122         float d, dx, dy;
123
124         int fp = f - maze;
125         int sp = s - maze;
126
127         sx = sp % maze_width(maze);
128         sy = sp / maze_width(maze);
129         fx = fp % maze_width(maze);
130         fy = fp / maze_width(maze);
131
132         dx = (float) sx - fx;
133         dy = (float) sy - fy;
134         d = (float) (abs(dx) + abs(dy)); /* manhattan distance */
135         return d;
136 }
137
138 int main(int argc, char *argv[])
139 {
140         static int maxnodes, i;
141         char *start, *goal;
142         struct a_star_path *path;
143
144         start = strchr(maze, '@');
145         goal = strchr(maze, 'x');
146         maxnodes = strlen(maze);
147
148         path = a_star((void *) maze, start, goal, maxnodes, maze_cost, maze_cost, nth_neighbor);
149         if (!path) {
150                 printf("a_star() failed to return a path.\n");
151                 return 0;
152         }
153         for (i = 0; i < path->node_count; i++) {
154                 char *p = path->path[i];
155                 *p = '.';
156         }
157         *goal = 'x';
158         *start = '@';
159
160         printf("%s\n", maze);
161
162         free(path);
163
164         if (strcmp(solution, maze) == 0)
165                 return 0;
166         else
167                 return 1;
168 }