/** Search for a path between two nodes. * This function executes an A* search to find an (optimal) path * from node @p from to node @p to. * @param from node to search from * @param to goal node * @param estimate_func function to estimate the cost from any node to the goal. * Note that the estimate function must be admissible for optimal A* search. That * means that for no query may the calculated estimate be higher than the actual * cost. * @param cost_func function to calculate the cost from a node to another adjacent * node. Note that the cost function is directly related to the estimate function. * For example, the cost can be calculated in terms of distance between nodes, or in * time that it takes to travel from one node to the other. The estimate function must * match the cost function to be admissible. * @param use_constraints true to respect constraints imposed by the constraint * repository, false to ignore the repository searching as if there were no * constraints whatsoever. * @param compute_constraints if true re-compute constraints, otherwise use constraints * as-is, for example if they have been computed before to check for changes. * @return ordered vector of nodes which denote a path from @p from to @p to. * Note that the vector is empty if no path could be found (i.e. there is non * or it was prohibited when using constraints. */ fawkes::NavGraphPath NavGraph::search_path(const NavGraphNode &from, const NavGraphNode &to, navgraph::EstimateFunction estimate_func, navgraph::CostFunction cost_func, bool use_constraints, bool compute_constraints) { if (! reachability_calced_) calc_reachability(); AStar astar; std::vector<AStarState *> a_star_solution; if (use_constraints) { constraint_repo_.lock(); if (compute_constraints && constraint_repo_->has_constraints()) { constraint_repo_->compute(); } NavGraphSearchState *initial_state = new NavGraphSearchState(from, to, this, estimate_func, cost_func, *constraint_repo_); a_star_solution = astar.solve(initial_state); constraint_repo_.unlock(); } else { NavGraphSearchState *initial_state = new NavGraphSearchState(from, to, this, estimate_func, cost_func); a_star_solution = astar.solve(initial_state); } std::vector<fawkes::NavGraphNode> path(a_star_solution.size()); NavGraphSearchState *solstate; for (unsigned int i = 0; i < a_star_solution.size(); ++i ) { solstate = dynamic_cast<NavGraphSearchState *>(a_star_solution[i]); path[i] = solstate->node(); } float cost = (! a_star_solution.empty()) ? a_star_solution[a_star_solution.size() - 1]->total_estimated_cost : -1; return NavGraphPath(this, path, cost); }
int main(void) { //Puzzle::PuzzleState g = { 1, 2, 3, 8, 0, 4, 7, 6, 5 }; //Puzzle start(3); //Puzzle goal(g, 3); AStar aStar; AStar::PuzzleState start = { { 5, 2, 0, 4, 7, 6, 8, 3, 1 }, 0, 0 }; //AStar::PuzzleState start = { { 2, 3, 4, 1, 8, 0, 7, 6, 5 }, 0, 0 }; AStar::PuzzleState goal = { { 1, 2, 3, 8, 0, 4, 7, 6, 5 }, 0, 0 }; aStar.solve(start, goal); return (0); }