/** 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);
}
