#include <vector>
#include "nuvieDefs.h"
#include "DirFinder.h"
#include "AStarPath.h"
AStarPath::AStarPath() : final_node(0)
{}void AStarPath::create_path()
{ astar_node *i = final_node; // iterator through steps, from back
delete_path();
std::vector<astar_node *> reverse_list;
while(i)
{
reverse_list.push_back(i);
i = i->parent;
}
while(!reverse_list.empty())
{
i = reverse_list.back();
add_step(i->loc);
reverse_list.pop_back();
}
set_path_size(step_count);
}/* Get a new neighbor to nnode and score it, returning true if it's usable. */
bool AStarPath::score_to_neighbor(sint8 dir, astar_node *nnode, astar_node *neighbor,
sint32 &nnode_to_neighbor)
{ sint8 sx = -1, sy = -1;
DirFinder::get_adjacent_dir(sx, sy, dir); // sx,sy = neighbor -1,-1 + dir
// get neighbor of nnode towards sx,sy, and cost to that neighbor
neighbor->loc = nnode->loc.abs_coords(sx, sy);
nnode_to_neighbor = step_cost(nnode->loc, neighbor->loc);
if(nnode_to_neighbor == -1)
{
delete neighbor; // this neighbor is blocked
return false;
}
return true;
}/* Compare a node's score to the start node to already scored neighbors. */
Path
Search::search() {
expanded = 0;
Node* cur;
Path ans(cost_maps[0]);
make_goal(goal);
fringe.push(make_start(start));
double cur_f = -1;
int iter = 0;
while (! fringe.is_empty()) {
iter++;
cur = fringe.pop();
cur->set_state(CLOSED);
/// printf("Pop ");
/// cur->print(stdout);
if (cur_f - cur->f > 1e-6 ) {
fprintf(stderr, "f value of node being expanded decreased\n");
fprintf(stdout, "Node above has f-value lower than previously closed node (%.1lf < %.1lf)\n", cur->f, cur_f);
}
cur_f = cur->f;
expanded++;
// If we are done, construct the path
if (cur->check_state(GOAL)) {
do {
ans.add_node(Node(cur));
cur = cur->prev;
} while (cur != NULL);
break;
}
// Consider neighbours
Point next_pos(cur->pos);
while (next_successor(cur->pos, next_pos)) {
double cost = step_cost(cur->pos, next_pos);
if (cost >= 0 && cost < HUGE_VAL) {
double ncost = cur->g + cost;
Node* next = get_node(next_pos);
if (next->check_state(CLOSED)) {
continue;
} else if (next->check_state(FRINGE)) {
if (ncost < next->g) {
// update it
next->g = ncost;
next->f = next->g + next->h;
next->prev = cur;
fringe.update(next);
}
} else {
next->h = heuristic(next_pos);
next->g = ncost;
next->f = next->g + next->h;
next->prev = cur;
/// printf("Push 0 ");
/// next->print(stdout);
fringe.push(next);
}
}
}
}
/// for (int i = 0; i < levels; i++) {
/// printf("level %d:\n", i);
/// printf("Map Costs\n");
/// cost_maps[i]->print(stdout);
/// printf("Path costs\n");
/// path_maps[i]->print(stdout);
/// }
/// ans.print(stdout);
return ans;
}
