2 Copyright (C) 2016 Stephen M. Cameron
3 Author: Stephen M. Cameron
5 This file is part of Spacenerds In Space.
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.
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.
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
31 __extension__ void *node[0];
34 static int nodeset_empty(struct nodeset *n)
36 return (n->nmembers == 0);
39 static void nodeset_add_node(struct nodeset *n, void *node)
43 for (i = 0; i < n->nmembers; i++) {
44 if (n->node[i] == node)
47 assert(n->nmembers < n->maxmembers);
48 n->node[n->nmembers] = node;
52 static void nodeset_remove_node(struct nodeset *n, void *node)
56 for (i = 0; i < n->nmembers; i++) {
57 if (n->node[i] != node)
59 if (i == n->nmembers - 1) {
64 n->node[i] = n->node[n->nmembers - 1];
66 n->node[n->nmembers] = NULL;
71 static int nodeset_contains_node(struct nodeset *n, void *node)
75 for (i = 0; i < n->nmembers; i++)
76 if (n->node[i] == node)
81 static struct nodeset *nodeset_new(int maxnodes)
85 n = malloc(sizeof(*n) + maxnodes * sizeof(void *));
86 memset(n, 0, sizeof(*n) + maxnodes * sizeof(void *));
87 n->maxmembers = maxnodes;
97 __extension__ struct node_pair p[0];
107 __extension__ struct score_entry s[0];
110 static float score_map_get_score(struct score_map *m, void *node)
114 for (i = 0; i < m->nelements; i++)
115 if (m->s[i].node == node)
116 return m->s[i].score;
120 static void *lowest_score(struct nodeset *candidates, struct score_map *s)
124 float score, lowest_score;
127 for (i = 0; i < candidates->nmembers; i++) {
128 score = score_map_get_score(s, candidates->node[i]);
129 if (lowest != NULL && score > lowest_score)
131 lowest = candidates->node[i];
132 lowest_score = score;
137 static struct score_map *score_map_new(int maxnodes)
141 s = malloc(sizeof(*s) + sizeof(struct score_entry) * maxnodes);
142 memset(s, 0, sizeof(*s) + sizeof(struct score_entry) * maxnodes);
143 s->nelements = maxnodes;
147 static void score_map_add_score(struct score_map *s, void *node, float score)
151 for (i = 0; i < s->nelements; i++) {
152 if (s->s[i].node != node)
154 s->s[i].score = score;
157 for (i = 0; i < s->nelements; i++) {
158 if (s->s[i].node != NULL)
161 s->s[i].score = score;
167 static struct node_map *node_map_new(int maxnodes)
171 n = malloc(sizeof(*n) + sizeof(struct node_pair) * maxnodes);
172 memset(n, 0, sizeof(*n) + sizeof(struct node_pair) * maxnodes);
173 n->nelements = maxnodes;
177 static void node_map_set_from(struct node_map *n, void *to, void *from)
181 for (i = 0; i < n->nelements; i++) {
182 if (n->p[i].to != to)
187 /* didn't find it, pick a NULL entry */
188 for (i = 0; i < n->nelements; i++) {
189 if (n->p[i].to != NULL)
195 assert(0); /* should never get here */
198 static void *node_map_get_from(struct node_map *n, void *to)
202 for (i = 0; i < n->nelements; i++)
203 if (n->p[i].to == to)
208 static void reconstruct_path(struct node_map *came_from, void *current, void ***path, int *nodecount, int maxnodes)
211 void **p = malloc(sizeof(*p) * maxnodes);
212 memset(p, 0, sizeof(*p) * maxnodes);
214 for (i = 0; i < came_from->nelements; i++)
215 if (came_from->p[i].to == NULL)
219 while ((current = node_map_get_from(came_from, current))) {
227 struct a_star_path *a_star(void *context, void *start, void *goal,
229 a_star_node_cost_fn distance,
230 a_star_node_cost_fn cost_estimate,
231 a_star_neighbor_iterator_fn nth_neighbor)
233 struct nodeset *openset, *closedset;
234 struct node_map *came_from;
235 struct score_map *gscore, *fscore;
236 void *neighbor, *current;
237 float tentative_gscore;
239 void **answer = NULL;
240 int answer_count = 0;
241 struct a_star_path *return_value;
243 closedset = nodeset_new(maxnodes);
244 openset = nodeset_new(maxnodes);
245 came_from = node_map_new(maxnodes);
246 gscore = score_map_new(maxnodes);
247 fscore = score_map_new(maxnodes);
249 nodeset_add_node(openset, start);
250 score_map_add_score(gscore, start, 0.0);
251 score_map_add_score(fscore, start, cost_estimate(context, start, goal));
253 while (!nodeset_empty(openset)) {
254 current = lowest_score(openset, fscore);
255 if (current == goal) {
256 reconstruct_path(came_from, current, &answer, &answer_count, maxnodes);
259 nodeset_remove_node(openset, current);
260 nodeset_add_node(closedset, current);
262 while ((neighbor = nth_neighbor(context, current, n))) {
264 if (nodeset_contains_node(closedset, neighbor))
266 tentative_gscore = score_map_get_score(gscore, current) + distance(context, current, neighbor);
267 if (!nodeset_contains_node(openset, neighbor))
268 nodeset_add_node(openset, neighbor);
269 else if (tentative_gscore >= score_map_get_score(gscore, neighbor))
271 node_map_set_from(came_from, neighbor, current);
272 score_map_add_score(gscore, neighbor, tentative_gscore);
273 score_map_add_score(fscore, neighbor,
274 score_map_get_score(gscore, neighbor) +
275 cost_estimate(context, neighbor, goal));
283 if (answer_count == 0) {
286 return_value = malloc(sizeof(*return_value) + sizeof(return_value->path[0]) * answer_count);
287 return_value->node_count = answer_count;
288 for (i = 0; i < answer_count; i++) {
289 return_value->path[answer_count - i - 1] = answer[i];