struct node * starting_node(struct node * the_node){
if (the_node->parent->parent != NULL)
return starting_node(the_node->parent);
else
return the_node;
}
std::vector<goap::Action> goap::AStar::plan(const WorldState& start, const WorldState& goal, const std::vector<Action>& actions) {
// Feasible we'd re-use a planner, so clear out the prior results
open_.clear();
closed_.clear();
known_nodes_.clear();
Node starting_node(start, 0, calculateHeuristic(start, goal), 0, nullptr);
// TODO figure out a more memory-friendly way of doing this...
known_nodes_[starting_node.id_] = starting_node;
open_.push_back(std::move(starting_node));
//int iters = 0;
while (open_.size() > 0) {
//++iters;
//std::cout << "\n-----------------------\n";
//std::cout << "Iteration " << iters << std::endl;
// Look for Node with the lowest-F-score on the open list. Switch it to closed,
// and hang onto it -- this is our latest node.
Node& current(popAndClose());
//std::cout << "Open list\n";
//printOpenList();
//std::cout << "Closed list\n";
//printClosedList();
//std::cout << "\nCurrent is " << current << std::endl;
// Is our current state the goal state? If so, we've found a path, yay.
if (current.ws_.meetsGoal(goal)) {
std::vector<Action> the_plan;
do {
the_plan.push_back(*current.action_);
current = known_nodes_.at(current.parent_id_);
} while (current.parent_id_ != 0);
return the_plan;
}
// Check each node REACHABLE from current
for (const auto& action : actions) {
if (action.eligibleFor(current.ws_)) {
WorldState possibility = action.actOn(current.ws_);
//std::cout << "Hmm, " << action.name() << " could work..." << "resulting in " << possibility << std::endl;
// Skip if already closed
if (memberOfClosed(possibility)) {
//std::cout << "...but that one's closed out.\n";
continue;
}
auto needle = memberOfOpen(possibility);
if (needle==end(open_)) { // not a member of open list
// Make a new node, with current as its parent, recording G & H
Node found(possibility, current.g_ + action.cost(), calculateHeuristic(possibility, goal), current.id_, &action);
known_nodes_[found.id_] = found;
// Add it to the open list (maintaining sort-order therein)
addToOpenList(std::move(found));
} else { // already a member of the open list
// check if the current G is better than the recorded G
if ((current.g_ + action.cost()) < needle->g_) {
//std::cout << "My path is better\n";
needle->parent_id_ = current.id_; // make current its parent
needle->g_ = current.g_ + action.cost(); // recalc G & H
needle->h_ = calculateHeuristic(possibility, goal);
std::sort(open_.begin(), open_.end()); // resort open list to account for the new F
}
}
}
}
}
// If there's nothing left to evaluate, then we have no possible path left
throw std::runtime_error("A* planner could not find a path from start to goal");
}
int direction_from_node_to_node(int xStart, int yStart, int xDestination, int yDestination){
if (isValidMoveCell(xStart, yStart) && isValidMoveCell(xDestination,yDestination)){
struct list * open_list = list_create();
struct list * close_list = list_create();
struct node * new_node = node_create();
new_node->x = xStart;
new_node->y = yStart;
new_node->g = 0;
new_node->h = distance_between_two_points(xStart,yStart,xDestination,yDestination);
new_node->f = new_node->h;
open_list->root = new_node;
while (open_list->root != NULL){
struct node * q = node_with_least_f(open_list);
pop_a_node_off_list(open_list,q);
int availableWay = ghost_map[q->x * width + q->y] - '0';
int up_added = 0;
int right_added = 0;
int down_added = 0;
struct node * sucessor[availableWay];
for (int i = 0; i < availableWay; i++){
sucessor[i] = node_create();
sucessor[i]->parent = q;
if (isValidMoveCell(q->x-1,q->y) && (up_added == 0)) {
sucessor[i]->x = q->x-1;
sucessor[i]->y = q->y;
up_added = 1;
} else if (isValidMoveCell(q->x, q->y+1) && (right_added == 0)){
sucessor[i]->x = q->x;
sucessor[i]->y = q->y+1;
right_added = 1;
} else if (isValidMoveCell(q->x+1, q->y) && (down_added == 0)){
sucessor[i]->x = q->x+1;
sucessor[i]->y = q->y;
down_added = 1;
} else if (isValidMoveCell(q->x, q->y-1)){
sucessor[i]->x = q->x;
sucessor[i]->y = q->y-1;
}
if (sucessor[i]->x == xDestination && sucessor[i]->y == yDestination) {
struct node * start_node = starting_node(sucessor[i]);
if (start_node->x < xStart){
free_node_and_list(sucessor[i],q,open_list,close_list);
return 0;
} else if (start_node->x > xStart){
free_node_and_list(sucessor[i],q,open_list,close_list);
return 2;
} else if (start_node->y > yStart){
free_node_and_list(sucessor[i],q,open_list,close_list);
return 1;
} else {
free_node_and_list(sucessor[i],q,open_list,close_list);
return 3;
}
} else {
sucessor[i]->g = q->g + distance_between_two_points(q->x, q->y, sucessor[i]->x, sucessor[i]->y);
sucessor[i]->h = distance_between_two_points(sucessor[i]->x,sucessor[i]->y, xDestination, yDestination);
sucessor[i]->f = sucessor[i]->g + sucessor[i]->h;
if ((ghost_map[sucessor[i]->x * width + sucessor[i]->y] - '0') == 1)
add_a_node_to_list(close_list,sucessor[i]);
else if (better_node_exist(open_list,sucessor[i]))
free(sucessor[i]);
else if (better_node_exist(close_list,sucessor[i]))
free(sucessor[i]);
else
add_a_node_to_list(open_list,sucessor[i]);
}
}
add_a_node_to_list(close_list, q);
}
}
return 4;
}
