Method FindPathBidirectionalEx

libs/routing/base/astar_algorithm.hpp:524–689

template <typename Vertex, typename Edge, typename Weight>
template <class P, class Emitter>
typename AStarAlgorithm<Vertex, Edge, Weight>::Result AStarAlgorithm<Vertex, Edge, Weight>::FindPathBidirectionalEx(
    P & params, Emitter && emitter) const
{
  auto const epsilon = params.m_weightEpsilon;
  auto & graph = params.m_graph;
  auto const & finalVertex = params.m_finalVertex;
  auto const & startVertex = params.m_startVertex;

  BidirectionalStepContext forward(true /* forward */, startVertex, finalVertex, graph);
  BidirectionalStepContext backward(false /* forward */, startVertex, finalVertex, graph);

  auto & forwardParents = forward.GetParents();
  auto & backwardParents = backward.GetParents();

  bool foundAnyPath = false;
  Weight bestPathReducedLength = kZeroDistance;
  Weight bestPathRealLength = kZeroDistance;

  forward.UpdateDistance(State(startVertex, kZeroDistance));
  forward.queue.push(State(startVertex, kZeroDistance, forward.ConsistentHeuristic(startVertex)));

  backward.UpdateDistance(State(finalVertex, kZeroDistance));
  backward.queue.push(State(finalVertex, kZeroDistance, backward.ConsistentHeuristic(finalVertex)));

  // To use the search code both for backward and forward directions
  // we keep the pointers to everything related to the search in the
  // 'current' and 'next' directions. Swapping these pointers indicates
  // changing the end we are searching from.
  BidirectionalStepContext * cur = &forward;
  BidirectionalStepContext * nxt = &backward;

  auto const EmitResult = [cur, nxt, &bestPathRealLength, &emitter]()
  {
    // No problem if length check fails, but we still emit the result.
    // Happens with "transit" route because of length, haven't seen with regular car route.
    // ASSERT(params.m_checkLengthCallback(bestPathRealLength), ());

    RoutingResult<Vertex, Weight> result;
    ReconstructPathBidirectional(cur->bestVertex, nxt->bestVertex, cur->parent, nxt->parent, result.m_path);
    result.m_distance = bestPathRealLength;
    if (!cur->forward)
      reverse(result.m_path.begin(), result.m_path.end());

    return emitter(std::move(result));
  };

  typename Graph::EdgeListT adj;

  // It is not necessary to check emptiness for both queues here
  // because if we have not found a path by the time one of the
  // queues is exhausted, we never will.
  uint32_t steps = 0;
  PeriodicPollCancellable periodicCancellable(params.m_cancellable);

  while (!cur->queue.empty() && !nxt->queue.empty())
  {
    ++steps;