C++ AStar::solve方法代码示例

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