List *Path(World *world, Point *source, Point *destination)
{
int tentative_gscore;
List *result = NULL;
int tries = 0;
Hashmap *nodes = Hashmap_create(cmp, hash);
Hashmap *closedset = Hashmap_create(cmp, hash);
PQueue *openset = PQueue_create(cmp, hash);
Node *current;
Node *start = Node_create(source, 0, 0, NULL);
Hashmap_set(nodes, start, start);
start->fscore = start->gscore + heuristic_cost_estimate(start->point, destination);
PQueue_push(openset, start, start->fscore);
while(!PQueue_empty(openset) &&
tries < 300) {
tries++;
current = PQueue_pop(openset);
Hashmap_set(closedset, current, current);
if(POINT_EQ(current->point, destination)) {
result = reconstruct_path(current);
break;
} else {
List *neighbours = neighbours_list(world, current->point, destination, nodes);
LIST_FOREACH(neighbours, first, next, cur) {
Node *neighbour = cur->value;
if(Hashmap_get(closedset, neighbour) == NULL) {
tentative_gscore = current->gscore + 1;
if(!PQueue_contains(openset, neighbour) ||
tentative_gscore > neighbour->gscore) {
if(!PQueue_contains(openset, neighbour)) {
neighbour->came_from = current;
neighbour->gscore = tentative_gscore;
neighbour->fscore = neighbour->gscore + heuristic_cost_estimate(neighbour->point, destination);
PQueue_push(openset, neighbour, neighbour->fscore);
}
}
}
}
List_destroy(neighbours);
}
}
void PathPlanner::fill_heuristic( )
{
double hVal;
for (int i = 0; i < _grid.size(); i++)
{
for(int j = 0; j< _grid[i].size(); j++)
{
if(_grid[i][j].cell_color == 0){
cell cl(j, i);
hVal = heuristic_cost_estimate(cl);
_grid[i][j].h_val = hVal;
}
}
}
}
// Implementation from:
// https://en.wikipedia.org/wiki/A*_search_algorithm#Pseudocode
int a_star(list_t graph, int width, int height, point_t* start, point_t* goal)
{
list_t closed;
list_create(&closed, width * height);
start->dist = 0;
f_score[start->x][start->y] = start->dist + heuristic_cost_estimate(start, goal);
heap_t open;
heap_create(&open, width * height);
heap_insert(open, 0, start);
while (heap_size(open) > 0)
{
// find node with minimal f_score value
point_t* current;
heap_remove(open, ¤t);
if (current == goal)
{
list_free(closed, NULL, NULL);
heap_free(open, NULL, NULL);
return EXIT_SUCCESS;
}
// remove from open list and insert into closed list
list_insert(closed, current->id, current);
// for neighbours of current
for (int i = MAX(current->x - 1, 0); i <= MIN(current->x + 1, width - 1); ++i)
{
for (int j = MAX(current->y - 1, 0); j <= MIN(current->y + 1, height - 1); ++j)
{
if (i != current->x || j!= current->y) // skip self
{
point_t* neighbour = graph_find(graph, width, height, i, j);
if (list_search(closed, neighbour->id, NULL))
{
continue; // ignore neighbour which is already evaluated
}
int tentative_g_score = current->dist + neighbour->cost;
if (tentative_g_score < neighbour->dist)
{
neighbour->prev = current;
neighbour->dist = tentative_g_score;
f_score[i][j] = neighbour->dist + heuristic_cost_estimate(neighbour, goal);
heap_insert(open, neighbour->dist, neighbour);
}
}
}
}
}
list_free(closed, NULL, NULL);
heap_free(open, NULL, NULL);
return EXIT_FAILURE;
}
