bool do_search(const TMap& map, const TCoords& start_p, const TCoords& finish_p) { static struct { int x, y, d; } dirs[] = { { -1, -1, 14 },{ 0, -1, 10 },{ 1, -1, 14 },{ -1, 0, 10 },{ 1, 0, 10 },{ -1, 1, 14 },{ 0, 1, 10 },{ 1, 1, 14 } }; memset(attrs, 0, sizeof(Attributes) * H * W); while (!opened.empty()) opened.pop(); AttrsPtr current = opened_push(start_p, cost_estimate(start_p, finish_p)); while (!opened.empty()) { current = opened_pop(); if (current.pos.x == finish_p.x && current.pos.y == finish_p.y) return true; for (int i = 0; i < 8; ++i) { int dx = dirs[i].x; int dy = dirs[i].y; TCoords npos; npos.x = current.pos.x + dx; npos.y = current.pos.y + dy; if (!inbound(npos.x, npos.y) || map.isobstacle(npos.x, npos.y)) continue; size_t ni = index2d(npos.x, npos.y); if (attrs[ni].state == st_Closed) continue; TWeight t_gscore = current.pa->gscore + dirs[i].d; if (attrs[ni].state == st_Wild) { opened_push(npos, t_gscore + cost_estimate(npos, finish_p)); } else { if (t_gscore >= attrs[ni].gscore) continue; rearrange(&attrs[ni], t_gscore + cost_estimate(npos, finish_p)); } attrs[ni].ofsx = dx; attrs[ni].ofsy = dy; attrs[ni].gscore = t_gscore; } } return false; }
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; }