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