| 109 | } |
| 110 | |
| 111 | bool PathsConnector::FindShortestPath(Graph::Edge const & from, Graph::Edge const & to, |
| 112 | FunctionalRoadClass lowestFrcToNextPoint, uint32_t maxPathLength, |
| 113 | Graph::EdgeVector & path) |
| 114 | { |
| 115 | // TODO(mgsergio): Turn Dijkstra to A*. |
| 116 | uint32_t const kLengthToleranceM = 10; |
| 117 | |
| 118 | struct State |
| 119 | { |
| 120 | State(Graph::Edge const & e, uint32_t const s) : m_edge(e), m_score(s) {} |
| 121 | |
| 122 | bool operator>(State const & o) const { return make_tuple(m_score, m_edge) > make_tuple(o.m_score, o.m_edge); } |
| 123 | |
| 124 | Graph::Edge m_edge; |
| 125 | uint32_t m_score; |
| 126 | }; |
| 127 | |
| 128 | ASSERT(from.HasRealPart() && to.HasRealPart(), ()); |
| 129 | |
| 130 | priority_queue<State, vector<State>, greater<State>> q; |
| 131 | map<Graph::Edge, uint32_t> scores; |
| 132 | map<Graph::Edge, Graph::Edge> links; |
| 133 | |
| 134 | q.emplace(from, 0); |
| 135 | scores[from] = 0; |
| 136 | |
| 137 | while (!q.empty()) |
| 138 | { |
| 139 | auto const state = q.top(); |
| 140 | q.pop(); |
| 141 | |
| 142 | auto const & u = state.m_edge; |
| 143 | // TODO(mgsergio): Unify names: use either score or distance. |
| 144 | auto const us = state.m_score; |
| 145 | |
| 146 | if (us > maxPathLength + kLengthToleranceM) |
| 147 | continue; |
| 148 | |
| 149 | if (us > scores[u]) |
| 150 | continue; |
| 151 | |
| 152 | if (u == to) |
| 153 | { |
| 154 | for (auto e = u; e != from; e = links[e]) |
| 155 | path.push_back(e); |
| 156 | path.push_back(from); |
| 157 | reverse(begin(path), end(path)); |
| 158 | return true; |
| 159 | } |
| 160 | |
| 161 | Graph::EdgeListT edges; |
| 162 | m_graph.GetOutgoingEdges(u.GetEndJunction(), edges); |
| 163 | for (auto const & e : edges) |
| 164 | { |
| 165 | if (!ConformLfrcnp(e, lowestFrcToNextPoint, 2 /* frcThreshold */, m_infoGetter)) |
| 166 | continue; |
| 167 | // TODO(mgsergio): Use frc to filter edges. |
| 168 |
nothing calls this directly
no test coverage detected