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