Method FindPath

libs/routing/base/astar_algorithm.hpp:450–520

template <typename Vertex, typename Edge, typename Weight>
template <typename P>
typename AStarAlgorithm<Vertex, Edge, Weight>::Result AStarAlgorithm<Vertex, Edge, Weight>::FindPath(
    P & params, RoutingResult<Vertex, Weight> & result) const
{
  auto const epsilon = params.m_weightEpsilon;

  result.Clear();

  auto & graph = params.m_graph;
  auto const & finalVertex = params.m_finalVertex;
  auto const & startVertex = params.m_startVertex;

  Context context(graph);
  PeriodicPollCancellable periodicCancellable(params.m_cancellable);
  Result resultCode = Result::NoPath;

  auto const heuristicDiff = [&](Vertex const & vertexFrom, Vertex const & vertexTo)
  { return graph.HeuristicCostEstimate(vertexFrom, finalVertex) - graph.HeuristicCostEstimate(vertexTo, finalVertex); };

  auto const fullToReducedLength = [&](Vertex const & vertexFrom, Vertex const & vertexTo, Weight const length)
  { return length - heuristicDiff(vertexFrom, vertexTo); };

  auto const reducedToFullLength = [&](Vertex const & vertexFrom, Vertex const & vertexTo, Weight const reducedLength)
  { return reducedLength + heuristicDiff(vertexFrom, vertexTo); };

  auto visitVertex = [&](Vertex const & vertex)
  {
    if (periodicCancellable.IsCancelled())
    {
      resultCode = Result::Cancelled;
      return false;
    }

    params.m_onVisitedVertexCallback(vertex, finalVertex);

    if (vertex == finalVertex)
    {
      resultCode = Result::OK;
      return false;
    }

    return true;
  };

  auto const adjustEdgeWeight = [&](Vertex const & vertexV, Edge const & edge)
  {
    auto const reducedWeight = fullToReducedLength(vertexV, edge.GetTarget(), edge.GetWeight());

    CHECK_GREATER_OR_EQUAL(reducedWeight, -epsilon, ("Invariant violated."));

    return std::max(reducedWeight, kZeroDistance);
  };

  auto const reducedToRealLength = [&](State const & state)
  { return reducedToFullLength(startVertex, state.vertex, state.distance); };

  auto const filterStates = [&](State const & state)
  { return params.m_checkLengthCallback(reducedToRealLength(state)); };