1 #ifndef OSRM_ENGINE_ROUTING_BASE_HPP
2 #define OSRM_ENGINE_ROUTING_BASE_HPP
3
4 #include "guidance/turn_bearing.hpp"
5 #include "guidance/turn_instruction.hpp"
6
7 #include "engine/algorithm.hpp"
8 #include "engine/datafacade.hpp"
9 #include "engine/internal_route_result.hpp"
10 #include "engine/phantom_node.hpp"
11 #include "engine/search_engine_data.hpp"
12
13 #include "util/coordinate_calculation.hpp"
14 #include "util/typedefs.hpp"
15
16 #include <boost/assert.hpp>
17
18 #include <cstddef>
19 #include <cstdint>
20
21 #include <algorithm>
22 #include <functional>
23 #include <iterator>
24 #include <memory>
25 #include <numeric>
26 #include <stack>
27 #include <utility>
28 #include <vector>
29
30 namespace osrm
31 {
32 namespace engine
33 {
34
35 namespace routing_algorithms
36 {
37 static constexpr bool FORWARD_DIRECTION = true;
38 static constexpr bool REVERSE_DIRECTION = false;
39 static constexpr bool DO_NOT_FORCE_LOOPS = false;
40
41 bool needsLoopForward(const PhantomNode &source_phantom, const PhantomNode &target_phantom);
42 bool needsLoopBackwards(const PhantomNode &source_phantom, const PhantomNode &target_phantom);
43
44 bool needsLoopForward(const PhantomNodes &phantoms);
45 bool needsLoopBackwards(const PhantomNodes &phantoms);
46
47 template <typename Heap>
insertNodesInHeaps(Heap & forward_heap,Heap & reverse_heap,const PhantomNodes & nodes)48 void insertNodesInHeaps(Heap &forward_heap, Heap &reverse_heap, const PhantomNodes &nodes)
49 {
50 const auto &source = nodes.source_phantom;
51 if (source.IsValidForwardSource())
52 {
53 forward_heap.Insert(source.forward_segment_id.id,
54 -source.GetForwardWeightPlusOffset(),
55 source.forward_segment_id.id);
56 }
57
58 if (source.IsValidReverseSource())
59 {
60 forward_heap.Insert(source.reverse_segment_id.id,
61 -source.GetReverseWeightPlusOffset(),
62 source.reverse_segment_id.id);
63 }
64
65 const auto &target = nodes.target_phantom;
66 if (target.IsValidForwardTarget())
67 {
68 reverse_heap.Insert(target.forward_segment_id.id,
69 target.GetForwardWeightPlusOffset(),
70 target.forward_segment_id.id);
71 }
72
73 if (target.IsValidReverseTarget())
74 {
75 reverse_heap.Insert(target.reverse_segment_id.id,
76 target.GetReverseWeightPlusOffset(),
77 target.reverse_segment_id.id);
78 }
79 }
80
81 template <typename ManyToManyQueryHeap>
insertSourceInHeap(ManyToManyQueryHeap & heap,const PhantomNode & phantom_node)82 void insertSourceInHeap(ManyToManyQueryHeap &heap, const PhantomNode &phantom_node)
83 {
84 if (phantom_node.IsValidForwardSource())
85 {
86 heap.Insert(phantom_node.forward_segment_id.id,
87 -phantom_node.GetForwardWeightPlusOffset(),
88 {phantom_node.forward_segment_id.id,
89 -phantom_node.GetForwardDuration(),
90 -phantom_node.GetForwardDistance()});
91 }
92 if (phantom_node.IsValidReverseSource())
93 {
94 heap.Insert(phantom_node.reverse_segment_id.id,
95 -phantom_node.GetReverseWeightPlusOffset(),
96 {phantom_node.reverse_segment_id.id,
97 -phantom_node.GetReverseDuration(),
98 -phantom_node.GetReverseDistance()});
99 }
100 }
101
102 template <typename ManyToManyQueryHeap>
insertTargetInHeap(ManyToManyQueryHeap & heap,const PhantomNode & phantom_node)103 void insertTargetInHeap(ManyToManyQueryHeap &heap, const PhantomNode &phantom_node)
104 {
105 if (phantom_node.IsValidForwardTarget())
106 {
107 heap.Insert(phantom_node.forward_segment_id.id,
108 phantom_node.GetForwardWeightPlusOffset(),
109 {phantom_node.forward_segment_id.id,
110 phantom_node.GetForwardDuration(),
111 phantom_node.GetForwardDistance()});
112 }
113 if (phantom_node.IsValidReverseTarget())
114 {
115 heap.Insert(phantom_node.reverse_segment_id.id,
116 phantom_node.GetReverseWeightPlusOffset(),
117 {phantom_node.reverse_segment_id.id,
118 phantom_node.GetReverseDuration(),
119 phantom_node.GetReverseDistance()});
120 }
121 }
122
123 template <typename FacadeT>
annotatePath(const FacadeT & facade,const PhantomNodes & phantom_node_pair,const std::vector<NodeID> & unpacked_nodes,const std::vector<EdgeID> & unpacked_edges,std::vector<PathData> & unpacked_path)124 void annotatePath(const FacadeT &facade,
125 const PhantomNodes &phantom_node_pair,
126 const std::vector<NodeID> &unpacked_nodes,
127 const std::vector<EdgeID> &unpacked_edges,
128 std::vector<PathData> &unpacked_path)
129 {
130 BOOST_ASSERT(!unpacked_nodes.empty());
131 BOOST_ASSERT(unpacked_nodes.size() == unpacked_edges.size() + 1);
132
133 const auto source_node_id = unpacked_nodes.front();
134 const auto target_node_id = unpacked_nodes.back();
135 const bool start_traversed_in_reverse =
136 phantom_node_pair.source_phantom.forward_segment_id.id != source_node_id;
137 const bool target_traversed_in_reverse =
138 phantom_node_pair.target_phantom.forward_segment_id.id != target_node_id;
139
140 BOOST_ASSERT(phantom_node_pair.source_phantom.forward_segment_id.id == source_node_id ||
141 phantom_node_pair.source_phantom.reverse_segment_id.id == source_node_id);
142 BOOST_ASSERT(phantom_node_pair.target_phantom.forward_segment_id.id == target_node_id ||
143 phantom_node_pair.target_phantom.reverse_segment_id.id == target_node_id);
144
145 // datastructures to hold extracted data from geometry
146 std::vector<NodeID> id_vector;
147 std::vector<SegmentWeight> weight_vector;
148 std::vector<SegmentDuration> duration_vector;
149 std::vector<DatasourceID> datasource_vector;
150
151 const auto get_segment_geometry = [&](const auto geometry_index) {
152 const auto copy = [](auto &vector, const auto range) {
153 vector.resize(range.size());
154 std::copy(range.begin(), range.end(), vector.begin());
155 };
156
157 if (geometry_index.forward)
158 {
159 copy(id_vector, facade.GetUncompressedForwardGeometry(geometry_index.id));
160 copy(weight_vector, facade.GetUncompressedForwardWeights(geometry_index.id));
161 copy(duration_vector, facade.GetUncompressedForwardDurations(geometry_index.id));
162 copy(datasource_vector, facade.GetUncompressedForwardDatasources(geometry_index.id));
163 }
164 else
165 {
166 copy(id_vector, facade.GetUncompressedReverseGeometry(geometry_index.id));
167 copy(weight_vector, facade.GetUncompressedReverseWeights(geometry_index.id));
168 copy(duration_vector, facade.GetUncompressedReverseDurations(geometry_index.id));
169 copy(datasource_vector, facade.GetUncompressedReverseDatasources(geometry_index.id));
170 }
171 };
172
173 auto node_from = unpacked_nodes.begin(), node_last = std::prev(unpacked_nodes.end());
174 for (auto edge = unpacked_edges.begin(); node_from != node_last; ++node_from, ++edge)
175 {
176 const auto &edge_data = facade.GetEdgeData(*edge);
177 const auto turn_id = edge_data.turn_id; // edge-based graph edge index
178 const auto node_id = *node_from; // edge-based graph node index
179 const auto name_index = facade.GetNameIndex(node_id);
180 const bool is_segregated = facade.IsSegregated(node_id);
181 const auto turn_instruction = facade.GetTurnInstructionForEdgeID(turn_id);
182 const extractor::TravelMode travel_mode = facade.GetTravelMode(node_id);
183 const auto classes = facade.GetClassData(node_id);
184
185 const auto geometry_index = facade.GetGeometryIndex(node_id);
186 get_segment_geometry(geometry_index);
187
188 BOOST_ASSERT(id_vector.size() > 0);
189 BOOST_ASSERT(datasource_vector.size() > 0);
190 BOOST_ASSERT(weight_vector.size() + 1 == id_vector.size());
191 BOOST_ASSERT(duration_vector.size() + 1 == id_vector.size());
192
193 const bool is_first_segment = unpacked_path.empty();
194
195 std::size_t start_index = 0;
196 if (is_first_segment)
197 {
198 unsigned short segment_position = phantom_node_pair.source_phantom.fwd_segment_position;
199 if (start_traversed_in_reverse)
200 {
201 segment_position = weight_vector.size() -
202 phantom_node_pair.source_phantom.fwd_segment_position - 1;
203 }
204 BOOST_ASSERT(segment_position >= 0);
205 start_index = static_cast<std::size_t>(segment_position);
206 }
207 const std::size_t end_index = weight_vector.size();
208
209 bool is_left_hand_driving = facade.IsLeftHandDriving(node_id);
210
211 BOOST_ASSERT(start_index < end_index);
212 for (std::size_t segment_idx = start_index; segment_idx < end_index; ++segment_idx)
213 {
214 unpacked_path.push_back(
215 PathData{*node_from,
216 id_vector[segment_idx + 1],
217 name_index,
218 is_segregated,
219 static_cast<EdgeWeight>(weight_vector[segment_idx]),
220 0,
221 static_cast<EdgeDuration>(duration_vector[segment_idx]),
222 0,
223 guidance::TurnInstruction::NO_TURN(),
224 {{0, INVALID_LANEID}, INVALID_LANE_DESCRIPTIONID},
225 travel_mode,
226 classes,
227 EMPTY_ENTRY_CLASS,
228 datasource_vector[segment_idx],
229 osrm::guidance::TurnBearing(0),
230 osrm::guidance::TurnBearing(0),
231 is_left_hand_driving});
232 }
233 BOOST_ASSERT(unpacked_path.size() > 0);
234 if (facade.HasLaneData(turn_id))
235 unpacked_path.back().lane_data = facade.GetLaneData(turn_id);
236
237 const auto turn_duration = facade.GetDurationPenaltyForEdgeID(turn_id);
238 const auto turn_weight = facade.GetWeightPenaltyForEdgeID(turn_id);
239
240 unpacked_path.back().entry_class = facade.GetEntryClass(turn_id);
241 unpacked_path.back().turn_instruction = turn_instruction;
242 unpacked_path.back().duration_until_turn += turn_duration;
243 unpacked_path.back().duration_of_turn = turn_duration;
244 unpacked_path.back().weight_until_turn += turn_weight;
245 unpacked_path.back().weight_of_turn = turn_weight;
246 unpacked_path.back().pre_turn_bearing = facade.PreTurnBearing(turn_id);
247 unpacked_path.back().post_turn_bearing = facade.PostTurnBearing(turn_id);
248 }
249
250 std::size_t start_index = 0, end_index = 0;
251 const auto source_geometry_id = facade.GetGeometryIndex(source_node_id).id;
252 const auto target_geometry = facade.GetGeometryIndex(target_node_id);
253 const auto is_local_path = source_geometry_id == target_geometry.id && unpacked_path.empty();
254
255 get_segment_geometry(target_geometry);
256
257 if (target_traversed_in_reverse)
258 {
259 if (is_local_path)
260 {
261 start_index =
262 weight_vector.size() - phantom_node_pair.source_phantom.fwd_segment_position - 1;
263 }
264 end_index =
265 weight_vector.size() - phantom_node_pair.target_phantom.fwd_segment_position - 1;
266 }
267 else
268 {
269 if (is_local_path)
270 {
271 start_index = phantom_node_pair.source_phantom.fwd_segment_position;
272 }
273 end_index = phantom_node_pair.target_phantom.fwd_segment_position;
274 }
275
276 // Given the following compressed geometry:
277 // U---v---w---x---y---Z
278 // s t
279 // s: fwd_segment 0
280 // t: fwd_segment 3
281 // -> (U, v), (v, w), (w, x)
282 // note that (x, t) is _not_ included but needs to be added later.
283 bool is_target_left_hand_driving = facade.IsLeftHandDriving(target_node_id);
284 for (std::size_t segment_idx = start_index; segment_idx != end_index;
285 (start_index < end_index ? ++segment_idx : --segment_idx))
286 {
287 BOOST_ASSERT(segment_idx < static_cast<std::size_t>(id_vector.size() - 1));
288 BOOST_ASSERT(facade.GetTravelMode(target_node_id) > 0);
289 unpacked_path.push_back(
290 PathData{target_node_id,
291 id_vector[start_index < end_index ? segment_idx + 1 : segment_idx - 1],
292 facade.GetNameIndex(target_node_id),
293 facade.IsSegregated(target_node_id),
294 static_cast<EdgeWeight>(weight_vector[segment_idx]),
295 0,
296 static_cast<EdgeDuration>(duration_vector[segment_idx]),
297 0,
298 guidance::TurnInstruction::NO_TURN(),
299 {{0, INVALID_LANEID}, INVALID_LANE_DESCRIPTIONID},
300 facade.GetTravelMode(target_node_id),
301 facade.GetClassData(target_node_id),
302 EMPTY_ENTRY_CLASS,
303 datasource_vector[segment_idx],
304 guidance::TurnBearing(0),
305 guidance::TurnBearing(0),
306 is_target_left_hand_driving});
307 }
308
309 if (unpacked_path.size() > 0)
310 {
311 const auto source_weight = start_traversed_in_reverse
312 ? phantom_node_pair.source_phantom.reverse_weight
313 : phantom_node_pair.source_phantom.forward_weight;
314 const auto source_duration = start_traversed_in_reverse
315 ? phantom_node_pair.source_phantom.reverse_duration
316 : phantom_node_pair.source_phantom.forward_duration;
317 // The above code will create segments for (v, w), (w,x), (x, y) and (y, Z).
318 // However the first segment duration needs to be adjusted to the fact that the source
319 // phantom is in the middle of the segment. We do this by subtracting v--s from the
320 // duration.
321
322 // Since it's possible duration_until_turn can be less than source_weight here if
323 // a negative enough turn penalty is used to modify this edge weight during
324 // osrm-contract, we clamp to 0 here so as not to return a negative duration
325 // for this segment.
326
327 // TODO this creates a scenario where it's possible the duration from a phantom
328 // node to the first turn would be the same as from end to end of a segment,
329 // which is obviously incorrect and not ideal...
330 unpacked_path.front().weight_until_turn =
331 std::max(unpacked_path.front().weight_until_turn - source_weight, 0);
332 unpacked_path.front().duration_until_turn =
333 std::max(unpacked_path.front().duration_until_turn - source_duration, 0);
334 }
335 }
336
337 template <typename Algorithm>
getPathDistance(const DataFacade<Algorithm> & facade,const std::vector<PathData> unpacked_path,const PhantomNode & source_phantom,const PhantomNode & target_phantom)338 double getPathDistance(const DataFacade<Algorithm> &facade,
339 const std::vector<PathData> unpacked_path,
340 const PhantomNode &source_phantom,
341 const PhantomNode &target_phantom)
342 {
343 using util::coordinate_calculation::detail::DEGREE_TO_RAD;
344 using util::coordinate_calculation::detail::EARTH_RADIUS;
345
346 double distance = 0;
347 double prev_lat =
348 static_cast<double>(util::toFloating(source_phantom.location.lat)) * DEGREE_TO_RAD;
349 double prev_lon =
350 static_cast<double>(util::toFloating(source_phantom.location.lon)) * DEGREE_TO_RAD;
351 double prev_cos = std::cos(prev_lat);
352 for (const auto &p : unpacked_path)
353 {
354 const auto current_coordinate = facade.GetCoordinateOfNode(p.turn_via_node);
355
356 const double current_lat =
357 static_cast<double>(util::toFloating(current_coordinate.lat)) * DEGREE_TO_RAD;
358 const double current_lon =
359 static_cast<double>(util::toFloating(current_coordinate.lon)) * DEGREE_TO_RAD;
360 const double current_cos = std::cos(current_lat);
361
362 const double sin_dlon = std::sin((prev_lon - current_lon) / 2.0);
363 const double sin_dlat = std::sin((prev_lat - current_lat) / 2.0);
364
365 const double aharv = sin_dlat * sin_dlat + prev_cos * current_cos * sin_dlon * sin_dlon;
366 const double charv = 2. * std::atan2(std::sqrt(aharv), std::sqrt(1.0 - aharv));
367 distance += EARTH_RADIUS * charv;
368
369 prev_lat = current_lat;
370 prev_lon = current_lon;
371 prev_cos = current_cos;
372 }
373
374 const double current_lat =
375 static_cast<double>(util::toFloating(target_phantom.location.lat)) * DEGREE_TO_RAD;
376 const double current_lon =
377 static_cast<double>(util::toFloating(target_phantom.location.lon)) * DEGREE_TO_RAD;
378 const double current_cos = std::cos(current_lat);
379
380 const double sin_dlon = std::sin((prev_lon - current_lon) / 2.0);
381 const double sin_dlat = std::sin((prev_lat - current_lat) / 2.0);
382
383 const double aharv = sin_dlat * sin_dlat + prev_cos * current_cos * sin_dlon * sin_dlon;
384 const double charv = 2. * std::atan2(std::sqrt(aharv), std::sqrt(1.0 - aharv));
385 distance += EARTH_RADIUS * charv;
386
387 return distance;
388 }
389
390 template <typename AlgorithmT>
extractRoute(const DataFacade<AlgorithmT> & facade,const EdgeWeight weight,const PhantomNodes & phantom_nodes,const std::vector<NodeID> & unpacked_nodes,const std::vector<EdgeID> & unpacked_edges)391 InternalRouteResult extractRoute(const DataFacade<AlgorithmT> &facade,
392 const EdgeWeight weight,
393 const PhantomNodes &phantom_nodes,
394 const std::vector<NodeID> &unpacked_nodes,
395 const std::vector<EdgeID> &unpacked_edges)
396 {
397 InternalRouteResult raw_route_data;
398 raw_route_data.segment_end_coordinates = {phantom_nodes};
399
400 // No path found for both target nodes?
401 if (INVALID_EDGE_WEIGHT == weight)
402 {
403 return raw_route_data;
404 }
405
406 raw_route_data.shortest_path_weight = weight;
407 raw_route_data.unpacked_path_segments.resize(1);
408 raw_route_data.source_traversed_in_reverse.push_back(
409 (unpacked_nodes.front() != phantom_nodes.source_phantom.forward_segment_id.id));
410 raw_route_data.target_traversed_in_reverse.push_back(
411 (unpacked_nodes.back() != phantom_nodes.target_phantom.forward_segment_id.id));
412
413 annotatePath(facade,
414 phantom_nodes,
415 unpacked_nodes,
416 unpacked_edges,
417 raw_route_data.unpacked_path_segments.front());
418
419 return raw_route_data;
420 }
421
computeEdgeDistance(const FacadeT & facade,NodeID node_id)422 template <typename FacadeT> EdgeDistance computeEdgeDistance(const FacadeT &facade, NodeID node_id)
423 {
424 const auto geometry_index = facade.GetGeometryIndex(node_id);
425
426 EdgeDistance total_distance = 0.0;
427
428 auto geometry_range = facade.GetUncompressedForwardGeometry(geometry_index.id);
429 for (auto current = geometry_range.begin(); current < geometry_range.end() - 1; ++current)
430 {
431 total_distance += util::coordinate_calculation::fccApproximateDistance(
432 facade.GetCoordinateOfNode(*current), facade.GetCoordinateOfNode(*std::next(current)));
433 }
434
435 return total_distance;
436 }
437
438 } // namespace routing_algorithms
439 } // namespace engine
440 } // namespace osrm
441
442 #endif // OSRM_ENGINE_ROUTING_BASE_HPP
443