| 691 | template <typename Vertex, typename Edge, typename Weight> |
| 692 | template <typename P> |
| 693 | typename AStarAlgorithm<Vertex, Edge, Weight>::Result AStarAlgorithm<Vertex, Edge, Weight>::AdjustRoute( |
| 694 | P & params, std::vector<Edge> const & prevRoute, RoutingResult<Vertex, Weight> & result) const |
| 695 | { |
| 696 | auto & graph = params.m_graph; |
| 697 | auto const & startVertex = params.m_startVertex; |
| 698 | CHECK(!prevRoute.empty(), ()); |
| 699 | |
| 700 | result.Clear(); |
| 701 | |
| 702 | bool wasCancelled = false; |
| 703 | auto minDistance = kInfiniteDistance; |
| 704 | Vertex returnVertex; |
| 705 | |
| 706 | std::map<Vertex, Weight> remainingDistances; |
| 707 | auto remainingDistance = kZeroDistance; |
| 708 | |
| 709 | for (auto it = prevRoute.crbegin(); it != prevRoute.crend(); ++it) |
| 710 | { |
| 711 | remainingDistances[it->GetTarget()] = remainingDistance; |
| 712 | remainingDistance += it->GetWeight(); |
| 713 | } |
| 714 | |
| 715 | Context context(graph); |
| 716 | PeriodicPollCancellable periodicCancellable(params.m_cancellable); |
| 717 | |
| 718 | auto visitVertex = [&](Vertex const & vertex) |
| 719 | { |
| 720 | if (periodicCancellable.IsCancelled()) |
| 721 | { |
| 722 | wasCancelled = true; |
| 723 | return false; |
| 724 | } |
| 725 | |
| 726 | params.m_onVisitedVertexCallback(startVertex, vertex); |
| 727 | |
| 728 | auto it = remainingDistances.find(vertex); |
| 729 | if (it != remainingDistances.cend()) |
| 730 | { |
| 731 | auto const fullDistance = context.GetDistance(vertex) + it->second; |
| 732 | if (fullDistance < minDistance) |
| 733 | { |
| 734 | minDistance = fullDistance; |
| 735 | returnVertex = vertex; |
| 736 | } |
| 737 | } |
| 738 | |
| 739 | return true; |
| 740 | }; |
| 741 | |
| 742 | auto const adjustEdgeWeight = [](Vertex const & /* vertex */, Edge const & edge) { return edge.GetWeight(); }; |
| 743 | |
| 744 | auto const filterStates = [&](State const & state) { return params.m_checkLengthCallback(state.distance); }; |
| 745 | |
| 746 | auto const reducedToRealLength = [&](State const & state) { return state.distance; }; |
| 747 | |
| 748 | PropagateWave(graph, startVertex, visitVertex, adjustEdgeWeight, filterStates, reducedToRealLength, context); |
| 749 | if (wasCancelled) |
| 750 | return Result::Cancelled; |