Move files in src/ include/
This commit is contained in:
Patrick Niklaus
@@ -0,0 +1,870 @@
|
||||
/*
|
||||
|
||||
Copyright (c) 2015, Project OSRM contributors
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without modification,
|
||||
are permitted provided that the following conditions are met:
|
||||
|
||||
Redistributions of source code must retain the above copyright notice, this list
|
||||
of conditions and the following disclaimer.
|
||||
Redistributions in binary form must reproduce the above copyright notice, this
|
||||
list of conditions and the following disclaimer in the documentation and/or
|
||||
other materials provided with the distribution.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
|
||||
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
|
||||
ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
*/
|
||||
|
||||
#ifndef ALTERNATIVE_PATH_ROUTING_HPP
|
||||
#define ALTERNATIVE_PATH_ROUTING_HPP
|
||||
|
||||
#include "routing_base.hpp"
|
||||
#include "../data_structures/search_engine_data.hpp"
|
||||
#include "../util/integer_range.hpp"
|
||||
#include "../util/container.hpp"
|
||||
|
||||
#include <boost/assert.hpp>
|
||||
|
||||
#include <unordered_map>
|
||||
#include <unordered_set>
|
||||
|
||||
#include <vector>
|
||||
|
||||
const double VIAPATH_ALPHA = 0.10;
|
||||
const double VIAPATH_EPSILON = 0.15; // alternative at most 15% longer
|
||||
const double VIAPATH_GAMMA = 0.75; // alternative shares at most 75% with the shortest.
|
||||
|
||||
template <class DataFacadeT>
|
||||
class AlternativeRouting final
|
||||
: private BasicRoutingInterface<DataFacadeT, AlternativeRouting<DataFacadeT>>
|
||||
{
|
||||
using super = BasicRoutingInterface<DataFacadeT, AlternativeRouting<DataFacadeT>>;
|
||||
using EdgeData = typename DataFacadeT::EdgeData;
|
||||
using QueryHeap = SearchEngineData::QueryHeap;
|
||||
using SearchSpaceEdge = std::pair<NodeID, NodeID>;
|
||||
|
||||
struct RankedCandidateNode
|
||||
{
|
||||
RankedCandidateNode(const NodeID node, const int length, const int sharing)
|
||||
: node(node), length(length), sharing(sharing)
|
||||
{
|
||||
}
|
||||
|
||||
NodeID node;
|
||||
int length;
|
||||
int sharing;
|
||||
|
||||
bool operator<(const RankedCandidateNode &other) const
|
||||
{
|
||||
return (2 * length + sharing) < (2 * other.length + other.sharing);
|
||||
}
|
||||
};
|
||||
DataFacadeT *facade;
|
||||
SearchEngineData &engine_working_data;
|
||||
|
||||
public:
|
||||
AlternativeRouting(DataFacadeT *facade, SearchEngineData &engine_working_data)
|
||||
: super(facade), facade(facade), engine_working_data(engine_working_data)
|
||||
{
|
||||
}
|
||||
|
||||
virtual ~AlternativeRouting() {}
|
||||
|
||||
void operator()(const PhantomNodes &phantom_node_pair, InternalRouteResult &raw_route_data)
|
||||
{
|
||||
std::vector<NodeID> alternative_path;
|
||||
std::vector<NodeID> via_node_candidate_list;
|
||||
std::vector<SearchSpaceEdge> forward_search_space;
|
||||
std::vector<SearchSpaceEdge> reverse_search_space;
|
||||
|
||||
// Init queues, semi-expensive because access to TSS invokes a sys-call
|
||||
engine_working_data.InitializeOrClearFirstThreadLocalStorage(
|
||||
super::facade->GetNumberOfNodes());
|
||||
engine_working_data.InitializeOrClearSecondThreadLocalStorage(
|
||||
super::facade->GetNumberOfNodes());
|
||||
engine_working_data.InitializeOrClearThirdThreadLocalStorage(
|
||||
super::facade->GetNumberOfNodes());
|
||||
|
||||
QueryHeap &forward_heap1 = *(engine_working_data.forward_heap_1);
|
||||
QueryHeap &reverse_heap1 = *(engine_working_data.reverse_heap_1);
|
||||
QueryHeap &forward_heap2 = *(engine_working_data.forward_heap_2);
|
||||
QueryHeap &reverse_heap2 = *(engine_working_data.reverse_heap_2);
|
||||
|
||||
int upper_bound_to_shortest_path_distance = INVALID_EDGE_WEIGHT;
|
||||
NodeID middle_node = SPECIAL_NODEID;
|
||||
const EdgeWeight min_edge_offset =
|
||||
std::min(-phantom_node_pair.source_phantom.GetForwardWeightPlusOffset(),
|
||||
-phantom_node_pair.source_phantom.GetReverseWeightPlusOffset());
|
||||
|
||||
if (phantom_node_pair.source_phantom.forward_node_id != SPECIAL_NODEID)
|
||||
{
|
||||
// SimpleLogger().Write(logDEBUG) << "fwd-a insert: " <<
|
||||
// phantom_node_pair.source_phantom.forward_node_id << ", w: " <<
|
||||
// -phantom_node_pair.source_phantom.GetForwardWeightPlusOffset();
|
||||
forward_heap1.Insert(phantom_node_pair.source_phantom.forward_node_id,
|
||||
-phantom_node_pair.source_phantom.GetForwardWeightPlusOffset(),
|
||||
phantom_node_pair.source_phantom.forward_node_id);
|
||||
}
|
||||
if (phantom_node_pair.source_phantom.reverse_node_id != SPECIAL_NODEID)
|
||||
{
|
||||
// SimpleLogger().Write(logDEBUG) << "fwd-b insert: " <<
|
||||
// phantom_node_pair.source_phantom.reverse_node_id << ", w: " <<
|
||||
// -phantom_node_pair.source_phantom.GetReverseWeightPlusOffset();
|
||||
forward_heap1.Insert(phantom_node_pair.source_phantom.reverse_node_id,
|
||||
-phantom_node_pair.source_phantom.GetReverseWeightPlusOffset(),
|
||||
phantom_node_pair.source_phantom.reverse_node_id);
|
||||
}
|
||||
|
||||
if (phantom_node_pair.target_phantom.forward_node_id != SPECIAL_NODEID)
|
||||
{
|
||||
// SimpleLogger().Write(logDEBUG) << "rev-a insert: " <<
|
||||
// phantom_node_pair.target_phantom.forward_node_id << ", w: " <<
|
||||
// phantom_node_pair.target_phantom.GetForwardWeightPlusOffset();
|
||||
reverse_heap1.Insert(phantom_node_pair.target_phantom.forward_node_id,
|
||||
phantom_node_pair.target_phantom.GetForwardWeightPlusOffset(),
|
||||
phantom_node_pair.target_phantom.forward_node_id);
|
||||
}
|
||||
if (phantom_node_pair.target_phantom.reverse_node_id != SPECIAL_NODEID)
|
||||
{
|
||||
// SimpleLogger().Write(logDEBUG) << "rev-b insert: " <<
|
||||
// phantom_node_pair.target_phantom.reverse_node_id << ", w: " <<
|
||||
// phantom_node_pair.target_phantom.GetReverseWeightPlusOffset();
|
||||
reverse_heap1.Insert(phantom_node_pair.target_phantom.reverse_node_id,
|
||||
phantom_node_pair.target_phantom.GetReverseWeightPlusOffset(),
|
||||
phantom_node_pair.target_phantom.reverse_node_id);
|
||||
}
|
||||
|
||||
// search from s and t till new_min/(1+epsilon) > length_of_shortest_path
|
||||
while (0 < (forward_heap1.Size() + reverse_heap1.Size()))
|
||||
{
|
||||
if (0 < forward_heap1.Size())
|
||||
{
|
||||
AlternativeRoutingStep<true>(forward_heap1, reverse_heap1, &middle_node,
|
||||
&upper_bound_to_shortest_path_distance,
|
||||
via_node_candidate_list, forward_search_space,
|
||||
min_edge_offset);
|
||||
}
|
||||
if (0 < reverse_heap1.Size())
|
||||
{
|
||||
AlternativeRoutingStep<false>(forward_heap1, reverse_heap1, &middle_node,
|
||||
&upper_bound_to_shortest_path_distance,
|
||||
via_node_candidate_list, reverse_search_space,
|
||||
min_edge_offset);
|
||||
}
|
||||
}
|
||||
|
||||
if (INVALID_EDGE_WEIGHT == upper_bound_to_shortest_path_distance)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
osrm::sort_unique_resize(via_node_candidate_list);
|
||||
|
||||
std::vector<NodeID> packed_forward_path;
|
||||
std::vector<NodeID> packed_reverse_path;
|
||||
|
||||
super::RetrievePackedPathFromSingleHeap(forward_heap1, middle_node, packed_forward_path);
|
||||
super::RetrievePackedPathFromSingleHeap(reverse_heap1, middle_node, packed_reverse_path);
|
||||
|
||||
// this set is is used as an indicator if a node is on the shortest path
|
||||
std::unordered_set<NodeID> nodes_in_path(packed_forward_path.size() +
|
||||
packed_reverse_path.size());
|
||||
nodes_in_path.insert(packed_forward_path.begin(), packed_forward_path.end());
|
||||
nodes_in_path.insert(middle_node);
|
||||
nodes_in_path.insert(packed_reverse_path.begin(), packed_reverse_path.end());
|
||||
|
||||
std::unordered_map<NodeID, int> approximated_forward_sharing;
|
||||
std::unordered_map<NodeID, int> approximated_reverse_sharing;
|
||||
|
||||
// sweep over search space, compute forward sharing for each current edge (u,v)
|
||||
for (const SearchSpaceEdge ¤t_edge : forward_search_space)
|
||||
{
|
||||
const NodeID u = current_edge.first;
|
||||
const NodeID v = current_edge.second;
|
||||
|
||||
if (nodes_in_path.find(v) != nodes_in_path.end())
|
||||
{
|
||||
// current_edge is on shortest path => sharing(v):=queue.GetKey(v);
|
||||
approximated_forward_sharing.emplace(v, forward_heap1.GetKey(v));
|
||||
}
|
||||
else
|
||||
{
|
||||
// current edge is not on shortest path. Check if we know a value for the other
|
||||
// endpoint
|
||||
const auto sharing_of_u_iterator = approximated_forward_sharing.find(u);
|
||||
if (sharing_of_u_iterator != approximated_forward_sharing.end())
|
||||
{
|
||||
approximated_forward_sharing.emplace(v, sharing_of_u_iterator->second);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// sweep over search space, compute backward sharing
|
||||
for (const SearchSpaceEdge ¤t_edge : reverse_search_space)
|
||||
{
|
||||
const NodeID u = current_edge.first;
|
||||
const NodeID v = current_edge.second;
|
||||
if (nodes_in_path.find(v) != nodes_in_path.end())
|
||||
{
|
||||
// current_edge is on shortest path => sharing(u):=queue.GetKey(u);
|
||||
approximated_reverse_sharing.emplace(v, reverse_heap1.GetKey(v));
|
||||
}
|
||||
else
|
||||
{
|
||||
// current edge is not on shortest path. Check if we know a value for the other
|
||||
// endpoint
|
||||
const auto sharing_of_u_iterator = approximated_reverse_sharing.find(u);
|
||||
if (sharing_of_u_iterator != approximated_reverse_sharing.end())
|
||||
{
|
||||
approximated_reverse_sharing.emplace(v, sharing_of_u_iterator->second);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// SimpleLogger().Write(logDEBUG) << "fwd_search_space size: " <<
|
||||
// forward_search_space.size() << ", marked " << approximated_forward_sharing.size() << "
|
||||
// nodes";
|
||||
// SimpleLogger().Write(logDEBUG) << "rev_search_space size: " <<
|
||||
// reverse_search_space.size() << ", marked " << approximated_reverse_sharing.size() << "
|
||||
// nodes";
|
||||
|
||||
std::vector<NodeID> preselected_node_list;
|
||||
for (const NodeID node : via_node_candidate_list)
|
||||
{
|
||||
const auto fwd_iterator = approximated_forward_sharing.find(node);
|
||||
const int fwd_sharing =
|
||||
(fwd_iterator != approximated_forward_sharing.end()) ? fwd_iterator->second : 0;
|
||||
const auto rev_iterator = approximated_reverse_sharing.find(node);
|
||||
const int rev_sharing =
|
||||
(rev_iterator != approximated_reverse_sharing.end()) ? rev_iterator->second : 0;
|
||||
|
||||
const int approximated_sharing = fwd_sharing + rev_sharing;
|
||||
const int approximated_length = forward_heap1.GetKey(node) + reverse_heap1.GetKey(node);
|
||||
const bool length_passes =
|
||||
(approximated_length <
|
||||
upper_bound_to_shortest_path_distance * (1 + VIAPATH_EPSILON));
|
||||
const bool sharing_passes =
|
||||
(approximated_sharing <= upper_bound_to_shortest_path_distance * VIAPATH_GAMMA);
|
||||
const bool stretch_passes =
|
||||
(approximated_length - approximated_sharing) <
|
||||
((1. + VIAPATH_ALPHA) *
|
||||
(upper_bound_to_shortest_path_distance - approximated_sharing));
|
||||
|
||||
if (length_passes && sharing_passes && stretch_passes)
|
||||
{
|
||||
preselected_node_list.emplace_back(node);
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<NodeID> &packed_shortest_path = packed_forward_path;
|
||||
std::reverse(packed_shortest_path.begin(), packed_shortest_path.end());
|
||||
packed_shortest_path.emplace_back(middle_node);
|
||||
packed_shortest_path.insert(packed_shortest_path.end(), packed_reverse_path.begin(),
|
||||
packed_reverse_path.end());
|
||||
std::vector<RankedCandidateNode> ranked_candidates_list;
|
||||
|
||||
// prioritizing via nodes for deep inspection
|
||||
for (const NodeID node : preselected_node_list)
|
||||
{
|
||||
int length_of_via_path = 0, sharing_of_via_path = 0;
|
||||
ComputeLengthAndSharingOfViaPath(node, &length_of_via_path, &sharing_of_via_path,
|
||||
packed_shortest_path, min_edge_offset);
|
||||
const int maximum_allowed_sharing =
|
||||
static_cast<int>(upper_bound_to_shortest_path_distance * VIAPATH_GAMMA);
|
||||
if (sharing_of_via_path <= maximum_allowed_sharing &&
|
||||
length_of_via_path <= upper_bound_to_shortest_path_distance * (1 + VIAPATH_EPSILON))
|
||||
{
|
||||
ranked_candidates_list.emplace_back(node, length_of_via_path, sharing_of_via_path);
|
||||
}
|
||||
}
|
||||
std::sort(ranked_candidates_list.begin(), ranked_candidates_list.end());
|
||||
|
||||
NodeID selected_via_node = SPECIAL_NODEID;
|
||||
int length_of_via_path = INVALID_EDGE_WEIGHT;
|
||||
NodeID s_v_middle = SPECIAL_NODEID, v_t_middle = SPECIAL_NODEID;
|
||||
for (const RankedCandidateNode &candidate : ranked_candidates_list)
|
||||
{
|
||||
if (ViaNodeCandidatePassesTTest(
|
||||
forward_heap1, reverse_heap1, forward_heap2, reverse_heap2, candidate,
|
||||
upper_bound_to_shortest_path_distance, &length_of_via_path, &s_v_middle,
|
||||
&v_t_middle, min_edge_offset))
|
||||
{
|
||||
// select first admissable
|
||||
selected_via_node = candidate.node;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Unpack shortest path and alternative, if they exist
|
||||
if (INVALID_EDGE_WEIGHT != upper_bound_to_shortest_path_distance)
|
||||
{
|
||||
BOOST_ASSERT(!packed_shortest_path.empty());
|
||||
raw_route_data.unpacked_path_segments.resize(1);
|
||||
raw_route_data.source_traversed_in_reverse.push_back(
|
||||
(packed_shortest_path.front() != phantom_node_pair.source_phantom.forward_node_id));
|
||||
raw_route_data.target_traversed_in_reverse.push_back(
|
||||
(packed_shortest_path.back() != phantom_node_pair.target_phantom.forward_node_id));
|
||||
|
||||
super::UnpackPath(
|
||||
// -- packed input
|
||||
packed_shortest_path.begin(), packed_shortest_path.end(),
|
||||
// -- start of route
|
||||
phantom_node_pair,
|
||||
// -- unpacked output
|
||||
raw_route_data.unpacked_path_segments.front());
|
||||
raw_route_data.shortest_path_length = upper_bound_to_shortest_path_distance;
|
||||
}
|
||||
|
||||
if (SPECIAL_NODEID != selected_via_node)
|
||||
{
|
||||
std::vector<NodeID> packed_alternate_path;
|
||||
// retrieve alternate path
|
||||
RetrievePackedAlternatePath(forward_heap1, reverse_heap1, forward_heap2, reverse_heap2,
|
||||
s_v_middle, v_t_middle, packed_alternate_path);
|
||||
|
||||
raw_route_data.alt_source_traversed_in_reverse.push_back((
|
||||
packed_alternate_path.front() != phantom_node_pair.source_phantom.forward_node_id));
|
||||
raw_route_data.alt_target_traversed_in_reverse.push_back(
|
||||
(packed_alternate_path.back() != phantom_node_pair.target_phantom.forward_node_id));
|
||||
|
||||
// unpack the alternate path
|
||||
super::UnpackPath(packed_alternate_path.begin(), packed_alternate_path.end(),
|
||||
phantom_node_pair, raw_route_data.unpacked_alternative);
|
||||
|
||||
raw_route_data.alternative_path_length = length_of_via_path;
|
||||
}
|
||||
else
|
||||
{
|
||||
BOOST_ASSERT(raw_route_data.alternative_path_length == INVALID_EDGE_WEIGHT);
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
// unpack alternate <s,..,v,..,t> by exploring search spaces from v
|
||||
void RetrievePackedAlternatePath(const QueryHeap &forward_heap1,
|
||||
const QueryHeap &reverse_heap1,
|
||||
const QueryHeap &forward_heap2,
|
||||
const QueryHeap &reverse_heap2,
|
||||
const NodeID s_v_middle,
|
||||
const NodeID v_t_middle,
|
||||
std::vector<NodeID> &packed_path) const
|
||||
{
|
||||
// fetch packed path [s,v)
|
||||
std::vector<NodeID> packed_v_t_path;
|
||||
super::RetrievePackedPathFromHeap(forward_heap1, reverse_heap2, s_v_middle, packed_path);
|
||||
packed_path.pop_back(); // remove middle node. It's in both half-paths
|
||||
|
||||
// fetch patched path [v,t]
|
||||
super::RetrievePackedPathFromHeap(forward_heap2, reverse_heap1, v_t_middle,
|
||||
packed_v_t_path);
|
||||
|
||||
packed_path.insert(packed_path.end(), packed_v_t_path.begin(), packed_v_t_path.end());
|
||||
}
|
||||
|
||||
// TODO: reorder parameters
|
||||
// compute and unpack <s,..,v> and <v,..,t> by exploring search spaces
|
||||
// from v and intersecting against queues. only half-searches have to be
|
||||
// done at this stage
|
||||
void ComputeLengthAndSharingOfViaPath(const NodeID via_node,
|
||||
int *real_length_of_via_path,
|
||||
int *sharing_of_via_path,
|
||||
const std::vector<NodeID> &packed_shortest_path,
|
||||
const EdgeWeight min_edge_offset)
|
||||
{
|
||||
engine_working_data.InitializeOrClearSecondThreadLocalStorage(
|
||||
super::facade->GetNumberOfNodes());
|
||||
|
||||
QueryHeap &existing_forward_heap = *engine_working_data.forward_heap_1;
|
||||
QueryHeap &existing_reverse_heap = *engine_working_data.reverse_heap_1;
|
||||
QueryHeap &new_forward_heap = *engine_working_data.forward_heap_2;
|
||||
QueryHeap &new_reverse_heap = *engine_working_data.reverse_heap_2;
|
||||
|
||||
std::vector<NodeID> packed_s_v_path;
|
||||
std::vector<NodeID> packed_v_t_path;
|
||||
|
||||
std::vector<NodeID> partially_unpacked_shortest_path;
|
||||
std::vector<NodeID> partially_unpacked_via_path;
|
||||
|
||||
NodeID s_v_middle = SPECIAL_NODEID;
|
||||
int upper_bound_s_v_path_length = INVALID_EDGE_WEIGHT;
|
||||
new_reverse_heap.Insert(via_node, 0, via_node);
|
||||
// compute path <s,..,v> by reusing forward search from s
|
||||
while (!new_reverse_heap.Empty())
|
||||
{
|
||||
super::RoutingStep(new_reverse_heap, existing_forward_heap, s_v_middle,
|
||||
upper_bound_s_v_path_length, min_edge_offset, false);
|
||||
}
|
||||
// compute path <v,..,t> by reusing backward search from node t
|
||||
NodeID v_t_middle = SPECIAL_NODEID;
|
||||
int upper_bound_of_v_t_path_length = INVALID_EDGE_WEIGHT;
|
||||
new_forward_heap.Insert(via_node, 0, via_node);
|
||||
while (!new_forward_heap.Empty())
|
||||
{
|
||||
super::RoutingStep(new_forward_heap, existing_reverse_heap, v_t_middle,
|
||||
upper_bound_of_v_t_path_length, min_edge_offset, true);
|
||||
}
|
||||
*real_length_of_via_path = upper_bound_s_v_path_length + upper_bound_of_v_t_path_length;
|
||||
|
||||
if (SPECIAL_NODEID == s_v_middle || SPECIAL_NODEID == v_t_middle)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
// retrieve packed paths
|
||||
super::RetrievePackedPathFromHeap(existing_forward_heap, new_reverse_heap, s_v_middle,
|
||||
packed_s_v_path);
|
||||
super::RetrievePackedPathFromHeap(new_forward_heap, existing_reverse_heap, v_t_middle,
|
||||
packed_v_t_path);
|
||||
|
||||
// partial unpacking, compute sharing
|
||||
// First partially unpack s-->v until paths deviate, note length of common path.
|
||||
const int64_t s_v_min_path_size =
|
||||
static_cast<int64_t>(std::min(packed_s_v_path.size(), packed_shortest_path.size())) - 1;
|
||||
for (const int64_t current_node : osrm::irange<int64_t>(0, s_v_min_path_size))
|
||||
{
|
||||
if (packed_s_v_path[current_node] == packed_shortest_path[current_node] &&
|
||||
packed_s_v_path[current_node + 1] == packed_shortest_path[current_node + 1])
|
||||
{
|
||||
EdgeID edgeID = facade->FindEdgeInEitherDirection(
|
||||
packed_s_v_path[current_node], packed_s_v_path[current_node + 1]);
|
||||
*sharing_of_via_path += facade->GetEdgeData(edgeID).distance;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (packed_s_v_path[current_node] == packed_shortest_path[current_node])
|
||||
{
|
||||
super::UnpackEdge(packed_s_v_path[current_node],
|
||||
packed_s_v_path[current_node + 1],
|
||||
partially_unpacked_via_path);
|
||||
super::UnpackEdge(packed_shortest_path[current_node],
|
||||
packed_shortest_path[current_node + 1],
|
||||
partially_unpacked_shortest_path);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
// traverse partially unpacked edge and note common prefix
|
||||
const int64_t packed_path_length =
|
||||
static_cast<int64_t>(std::min(partially_unpacked_via_path.size(),
|
||||
partially_unpacked_shortest_path.size())) -
|
||||
1;
|
||||
for (int64_t current_node = 0; (current_node < packed_path_length) &&
|
||||
(partially_unpacked_via_path[current_node] ==
|
||||
partially_unpacked_shortest_path[current_node] &&
|
||||
partially_unpacked_via_path[current_node + 1] ==
|
||||
partially_unpacked_shortest_path[current_node + 1]);
|
||||
++current_node)
|
||||
{
|
||||
EdgeID selected_edge =
|
||||
facade->FindEdgeInEitherDirection(partially_unpacked_via_path[current_node],
|
||||
partially_unpacked_via_path[current_node + 1]);
|
||||
*sharing_of_via_path += facade->GetEdgeData(selected_edge).distance;
|
||||
}
|
||||
|
||||
// Second, partially unpack v-->t in reverse order until paths deviate and note lengths
|
||||
int64_t via_path_index = static_cast<int64_t>(packed_v_t_path.size()) - 1;
|
||||
int64_t shortest_path_index = static_cast<int64_t>(packed_shortest_path.size()) - 1;
|
||||
for (; via_path_index > 0 && shortest_path_index > 0;
|
||||
--via_path_index, --shortest_path_index)
|
||||
{
|
||||
if (packed_v_t_path[via_path_index - 1] ==
|
||||
packed_shortest_path[shortest_path_index - 1] &&
|
||||
packed_v_t_path[via_path_index] == packed_shortest_path[shortest_path_index])
|
||||
{
|
||||
EdgeID edgeID = facade->FindEdgeInEitherDirection(
|
||||
packed_v_t_path[via_path_index - 1], packed_v_t_path[via_path_index]);
|
||||
*sharing_of_via_path += facade->GetEdgeData(edgeID).distance;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (packed_v_t_path[via_path_index] == packed_shortest_path[shortest_path_index])
|
||||
{
|
||||
super::UnpackEdge(packed_v_t_path[via_path_index - 1],
|
||||
packed_v_t_path[via_path_index], partially_unpacked_via_path);
|
||||
super::UnpackEdge(packed_shortest_path[shortest_path_index - 1],
|
||||
packed_shortest_path[shortest_path_index],
|
||||
partially_unpacked_shortest_path);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
via_path_index = partially_unpacked_via_path.size() - 1;
|
||||
shortest_path_index = partially_unpacked_shortest_path.size() - 1;
|
||||
for (; via_path_index > 0 && shortest_path_index > 0;
|
||||
--via_path_index, --shortest_path_index)
|
||||
{
|
||||
if (partially_unpacked_via_path[via_path_index - 1] ==
|
||||
partially_unpacked_shortest_path[shortest_path_index - 1] &&
|
||||
partially_unpacked_via_path[via_path_index] ==
|
||||
partially_unpacked_shortest_path[shortest_path_index])
|
||||
{
|
||||
EdgeID edgeID = facade->FindEdgeInEitherDirection(
|
||||
partially_unpacked_via_path[via_path_index - 1],
|
||||
partially_unpacked_via_path[via_path_index]);
|
||||
*sharing_of_via_path += facade->GetEdgeData(edgeID).distance;
|
||||
}
|
||||
else
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
// finished partial unpacking spree! Amount of sharing is stored to appropriate pointer
|
||||
// variable
|
||||
}
|
||||
|
||||
// int approximateAmountOfSharing(
|
||||
// const NodeID alternate_path_middle_node_id,
|
||||
// QueryHeap & forward_heap,
|
||||
// QueryHeap & reverse_heap,
|
||||
// const std::vector<NodeID> & packed_shortest_path
|
||||
// ) const {
|
||||
// std::vector<NodeID> packed_alternate_path;
|
||||
// super::RetrievePackedPathFromHeap(
|
||||
// forward_heap,
|
||||
// reverse_heap,
|
||||
// alternate_path_middle_node_id,
|
||||
// packed_alternate_path
|
||||
// );
|
||||
|
||||
// if(packed_shortest_path.size() < 2 || packed_alternate_path.size() < 2) {
|
||||
// return 0;
|
||||
// }
|
||||
|
||||
// int sharing = 0;
|
||||
// int aindex = 0;
|
||||
// //compute forward sharing
|
||||
// while( (packed_alternate_path[aindex] == packed_shortest_path[aindex]) &&
|
||||
// (packed_alternate_path[aindex+1] == packed_shortest_path[aindex+1]) ) {
|
||||
// // SimpleLogger().Write() << "retrieving edge (" <<
|
||||
// packed_alternate_path[aindex] << "," << packed_alternate_path[aindex+1] << ")";
|
||||
// EdgeID edgeID = facade->FindEdgeInEitherDirection(packed_alternate_path[aindex],
|
||||
// packed_alternate_path[aindex+1]);
|
||||
// sharing += facade->GetEdgeData(edgeID).distance;
|
||||
// ++aindex;
|
||||
// }
|
||||
|
||||
// aindex = packed_alternate_path.size()-1;
|
||||
// int bindex = packed_shortest_path.size()-1;
|
||||
// //compute backward sharing
|
||||
// while( aindex > 0 && bindex > 0 && (packed_alternate_path[aindex] ==
|
||||
// packed_shortest_path[bindex]) && (packed_alternate_path[aindex-1] ==
|
||||
// packed_shortest_path[bindex-1]) ) {
|
||||
// EdgeID edgeID = facade->FindEdgeInEitherDirection(packed_alternate_path[aindex],
|
||||
// packed_alternate_path[aindex-1]);
|
||||
// sharing += facade->GetEdgeData(edgeID).distance;
|
||||
// --aindex; --bindex;
|
||||
// }
|
||||
// return sharing;
|
||||
// }
|
||||
|
||||
// todo: reorder parameters
|
||||
template <bool is_forward_directed>
|
||||
void AlternativeRoutingStep(QueryHeap &heap1,
|
||||
QueryHeap &heap2,
|
||||
NodeID *middle_node,
|
||||
int *upper_bound_to_shortest_path_distance,
|
||||
std::vector<NodeID> &search_space_intersection,
|
||||
std::vector<SearchSpaceEdge> &search_space,
|
||||
const EdgeWeight min_edge_offset) const
|
||||
{
|
||||
QueryHeap &forward_heap = (is_forward_directed ? heap1 : heap2);
|
||||
QueryHeap &reverse_heap = (is_forward_directed ? heap2 : heap1);
|
||||
|
||||
const NodeID node = forward_heap.DeleteMin();
|
||||
const int distance = forward_heap.GetKey(node);
|
||||
// const NodeID parentnode = forward_heap.GetData(node).parent;
|
||||
// SimpleLogger().Write() << (is_forward_directed ? "[fwd] " : "[rev] ") << "settled edge ("
|
||||
// << parentnode << "," << node << "), dist: " << distance;
|
||||
|
||||
const int scaled_distance =
|
||||
static_cast<int>((distance + min_edge_offset) / (1. + VIAPATH_EPSILON));
|
||||
if ((INVALID_EDGE_WEIGHT != *upper_bound_to_shortest_path_distance) &&
|
||||
(scaled_distance > *upper_bound_to_shortest_path_distance))
|
||||
{
|
||||
forward_heap.DeleteAll();
|
||||
return;
|
||||
}
|
||||
|
||||
search_space.emplace_back(forward_heap.GetData(node).parent, node);
|
||||
|
||||
if (reverse_heap.WasInserted(node))
|
||||
{
|
||||
search_space_intersection.emplace_back(node);
|
||||
const int new_distance = reverse_heap.GetKey(node) + distance;
|
||||
if (new_distance < *upper_bound_to_shortest_path_distance)
|
||||
{
|
||||
if (new_distance >= 0)
|
||||
{
|
||||
*middle_node = node;
|
||||
*upper_bound_to_shortest_path_distance = new_distance;
|
||||
// SimpleLogger().Write() << "accepted middle_node " << *middle_node << " at
|
||||
// distance " << new_distance;
|
||||
// } else {
|
||||
// SimpleLogger().Write() << "discarded middle_node " << *middle_node << "
|
||||
// at distance " << new_distance;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (auto edge : facade->GetAdjacentEdgeRange(node))
|
||||
{
|
||||
const EdgeData &data = facade->GetEdgeData(edge);
|
||||
const bool edge_is_forward_directed =
|
||||
(is_forward_directed ? data.forward : data.backward);
|
||||
if (edge_is_forward_directed)
|
||||
{
|
||||
|
||||
const NodeID to = facade->GetTarget(edge);
|
||||
const int edge_weight = data.distance;
|
||||
|
||||
BOOST_ASSERT(edge_weight > 0);
|
||||
const int to_distance = distance + edge_weight;
|
||||
|
||||
// New Node discovered -> Add to Heap + Node Info Storage
|
||||
if (!forward_heap.WasInserted(to))
|
||||
{
|
||||
forward_heap.Insert(to, to_distance, node);
|
||||
}
|
||||
// Found a shorter Path -> Update distance
|
||||
else if (to_distance < forward_heap.GetKey(to))
|
||||
{
|
||||
// new parent
|
||||
forward_heap.GetData(to).parent = node;
|
||||
// decreased distance
|
||||
forward_heap.DecreaseKey(to, to_distance);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// conduct T-Test
|
||||
bool ViaNodeCandidatePassesTTest(QueryHeap &existing_forward_heap,
|
||||
QueryHeap &existing_reverse_heap,
|
||||
QueryHeap &new_forward_heap,
|
||||
QueryHeap &new_reverse_heap,
|
||||
const RankedCandidateNode &candidate,
|
||||
const int length_of_shortest_path,
|
||||
int *length_of_via_path,
|
||||
NodeID *s_v_middle,
|
||||
NodeID *v_t_middle,
|
||||
const EdgeWeight min_edge_offset) const
|
||||
{
|
||||
new_forward_heap.Clear();
|
||||
new_reverse_heap.Clear();
|
||||
std::vector<NodeID> packed_s_v_path;
|
||||
std::vector<NodeID> packed_v_t_path;
|
||||
|
||||
*s_v_middle = SPECIAL_NODEID;
|
||||
int upper_bound_s_v_path_length = INVALID_EDGE_WEIGHT;
|
||||
// compute path <s,..,v> by reusing forward search from s
|
||||
new_reverse_heap.Insert(candidate.node, 0, candidate.node);
|
||||
while (new_reverse_heap.Size() > 0)
|
||||
{
|
||||
super::RoutingStep(new_reverse_heap, existing_forward_heap, *s_v_middle,
|
||||
upper_bound_s_v_path_length, min_edge_offset, false);
|
||||
}
|
||||
|
||||
if (INVALID_EDGE_WEIGHT == upper_bound_s_v_path_length)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
// compute path <v,..,t> by reusing backward search from t
|
||||
*v_t_middle = SPECIAL_NODEID;
|
||||
int upper_bound_of_v_t_path_length = INVALID_EDGE_WEIGHT;
|
||||
new_forward_heap.Insert(candidate.node, 0, candidate.node);
|
||||
while (new_forward_heap.Size() > 0)
|
||||
{
|
||||
super::RoutingStep(new_forward_heap, existing_reverse_heap, *v_t_middle,
|
||||
upper_bound_of_v_t_path_length, min_edge_offset, true);
|
||||
}
|
||||
|
||||
if (INVALID_EDGE_WEIGHT == upper_bound_of_v_t_path_length)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
*length_of_via_path = upper_bound_s_v_path_length + upper_bound_of_v_t_path_length;
|
||||
|
||||
// retrieve packed paths
|
||||
super::RetrievePackedPathFromHeap(existing_forward_heap, new_reverse_heap, *s_v_middle,
|
||||
packed_s_v_path);
|
||||
|
||||
super::RetrievePackedPathFromHeap(new_forward_heap, existing_reverse_heap, *v_t_middle,
|
||||
packed_v_t_path);
|
||||
|
||||
NodeID s_P = *s_v_middle, t_P = *v_t_middle;
|
||||
if (SPECIAL_NODEID == s_P)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (SPECIAL_NODEID == t_P)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
const int T_threshold = static_cast<int>(VIAPATH_EPSILON * length_of_shortest_path);
|
||||
int unpacked_until_distance = 0;
|
||||
|
||||
std::stack<SearchSpaceEdge> unpack_stack;
|
||||
// Traverse path s-->v
|
||||
for (std::size_t i = packed_s_v_path.size() - 1; (i > 0) && unpack_stack.empty(); --i)
|
||||
{
|
||||
const EdgeID current_edge_id =
|
||||
facade->FindEdgeInEitherDirection(packed_s_v_path[i - 1], packed_s_v_path[i]);
|
||||
const int length_of_current_edge = facade->GetEdgeData(current_edge_id).distance;
|
||||
if ((length_of_current_edge + unpacked_until_distance) >= T_threshold)
|
||||
{
|
||||
unpack_stack.emplace(packed_s_v_path[i - 1], packed_s_v_path[i]);
|
||||
}
|
||||
else
|
||||
{
|
||||
unpacked_until_distance += length_of_current_edge;
|
||||
s_P = packed_s_v_path[i - 1];
|
||||
}
|
||||
}
|
||||
|
||||
while (!unpack_stack.empty())
|
||||
{
|
||||
const SearchSpaceEdge via_path_edge = unpack_stack.top();
|
||||
unpack_stack.pop();
|
||||
EdgeID edge_in_via_path_id =
|
||||
facade->FindEdgeInEitherDirection(via_path_edge.first, via_path_edge.second);
|
||||
|
||||
if (SPECIAL_EDGEID == edge_in_via_path_id)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
const EdgeData ¤t_edge_data = facade->GetEdgeData(edge_in_via_path_id);
|
||||
const bool current_edge_is_shortcut = current_edge_data.shortcut;
|
||||
if (current_edge_is_shortcut)
|
||||
{
|
||||
const NodeID via_path_middle_node_id = current_edge_data.id;
|
||||
const EdgeID second_segment_edge_id = facade->FindEdgeInEitherDirection(
|
||||
via_path_middle_node_id, via_path_edge.second);
|
||||
const int second_segment_length =
|
||||
facade->GetEdgeData(second_segment_edge_id).distance;
|
||||
// attention: !unpacking in reverse!
|
||||
// Check if second segment is the one to go over treshold? if yes add second segment
|
||||
// to stack, else push first segment to stack and add distance of second one.
|
||||
if (unpacked_until_distance + second_segment_length >= T_threshold)
|
||||
{
|
||||
unpack_stack.emplace(via_path_middle_node_id, via_path_edge.second);
|
||||
}
|
||||
else
|
||||
{
|
||||
unpacked_until_distance += second_segment_length;
|
||||
unpack_stack.emplace(via_path_edge.first, via_path_middle_node_id);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// edge is not a shortcut, set the start node for T-Test to end of edge.
|
||||
unpacked_until_distance += current_edge_data.distance;
|
||||
s_P = via_path_edge.first;
|
||||
}
|
||||
}
|
||||
|
||||
int t_test_path_length = unpacked_until_distance;
|
||||
unpacked_until_distance = 0;
|
||||
// Traverse path s-->v
|
||||
BOOST_ASSERT(!packed_v_t_path.empty());
|
||||
for (unsigned i = 0, packed_path_length = static_cast<unsigned>(packed_v_t_path.size() - 1);
|
||||
(i < packed_path_length) && unpack_stack.empty(); ++i)
|
||||
{
|
||||
const EdgeID edgeID =
|
||||
facade->FindEdgeInEitherDirection(packed_v_t_path[i], packed_v_t_path[i + 1]);
|
||||
int length_of_current_edge = facade->GetEdgeData(edgeID).distance;
|
||||
if (length_of_current_edge + unpacked_until_distance >= T_threshold)
|
||||
{
|
||||
unpack_stack.emplace(packed_v_t_path[i], packed_v_t_path[i + 1]);
|
||||
}
|
||||
else
|
||||
{
|
||||
unpacked_until_distance += length_of_current_edge;
|
||||
t_P = packed_v_t_path[i + 1];
|
||||
}
|
||||
}
|
||||
|
||||
while (!unpack_stack.empty())
|
||||
{
|
||||
const SearchSpaceEdge via_path_edge = unpack_stack.top();
|
||||
unpack_stack.pop();
|
||||
EdgeID edge_in_via_path_id =
|
||||
facade->FindEdgeInEitherDirection(via_path_edge.first, via_path_edge.second);
|
||||
if (SPECIAL_EDGEID == edge_in_via_path_id)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
const EdgeData ¤t_edge_data = facade->GetEdgeData(edge_in_via_path_id);
|
||||
const bool IsViaEdgeShortCut = current_edge_data.shortcut;
|
||||
if (IsViaEdgeShortCut)
|
||||
{
|
||||
const NodeID middleOfViaPath = current_edge_data.id;
|
||||
EdgeID edgeIDOfFirstSegment =
|
||||
facade->FindEdgeInEitherDirection(via_path_edge.first, middleOfViaPath);
|
||||
int lengthOfFirstSegment = facade->GetEdgeData(edgeIDOfFirstSegment).distance;
|
||||
// Check if first segment is the one to go over treshold? if yes first segment to
|
||||
// stack, else push second segment to stack and add distance of first one.
|
||||
if (unpacked_until_distance + lengthOfFirstSegment >= T_threshold)
|
||||
{
|
||||
unpack_stack.emplace(via_path_edge.first, middleOfViaPath);
|
||||
}
|
||||
else
|
||||
{
|
||||
unpacked_until_distance += lengthOfFirstSegment;
|
||||
unpack_stack.emplace(middleOfViaPath, via_path_edge.second);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// edge is not a shortcut, set the start node for T-Test to end of edge.
|
||||
unpacked_until_distance += current_edge_data.distance;
|
||||
t_P = via_path_edge.second;
|
||||
}
|
||||
}
|
||||
|
||||
t_test_path_length += unpacked_until_distance;
|
||||
// Run actual T-Test query and compare if distances equal.
|
||||
engine_working_data.InitializeOrClearThirdThreadLocalStorage(
|
||||
super::facade->GetNumberOfNodes());
|
||||
|
||||
QueryHeap &forward_heap3 = *engine_working_data.forward_heap_3;
|
||||
QueryHeap &reverse_heap3 = *engine_working_data.reverse_heap_3;
|
||||
int upper_bound = INVALID_EDGE_WEIGHT;
|
||||
NodeID middle = SPECIAL_NODEID;
|
||||
|
||||
forward_heap3.Insert(s_P, 0, s_P);
|
||||
reverse_heap3.Insert(t_P, 0, t_P);
|
||||
// exploration from s and t until deletemin/(1+epsilon) > _lengt_oO_sShortest_path
|
||||
while ((forward_heap3.Size() + reverse_heap3.Size()) > 0)
|
||||
{
|
||||
if (!forward_heap3.Empty())
|
||||
{
|
||||
super::RoutingStep(forward_heap3, reverse_heap3, middle, upper_bound,
|
||||
min_edge_offset, true);
|
||||
}
|
||||
if (!reverse_heap3.Empty())
|
||||
{
|
||||
super::RoutingStep(reverse_heap3, forward_heap3, middle, upper_bound,
|
||||
min_edge_offset, false);
|
||||
}
|
||||
}
|
||||
return (upper_bound <= t_test_path_length);
|
||||
}
|
||||
};
|
||||
|
||||
#endif /* ALTERNATIVE_PATH_ROUTING_HPP */
|
||||
@@ -0,0 +1,155 @@
|
||||
/*
|
||||
|
||||
Copyright (c) 2015, Project OSRM contributors
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without modification,
|
||||
are permitted provided that the following conditions are met:
|
||||
|
||||
Redistributions of source code must retain the above copyright notice, this list
|
||||
of conditions and the following disclaimer.
|
||||
Redistributions in binary form must reproduce the above copyright notice, this
|
||||
list of conditions and the following disclaimer in the documentation and/or
|
||||
other materials provided with the distribution.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
|
||||
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
|
||||
ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
*/
|
||||
|
||||
#ifndef DIRECT_SHORTEST_PATH_HPP
|
||||
#define DIRECT_SHORTEST_PATH_HPP
|
||||
|
||||
#include <boost/assert.hpp>
|
||||
#include <iterator>
|
||||
|
||||
#include "routing_base.hpp"
|
||||
#include "../data_structures/search_engine_data.hpp"
|
||||
#include "../util/integer_range.hpp"
|
||||
#include "../util/timing_util.hpp"
|
||||
#include "../typedefs.h"
|
||||
|
||||
/// This is a striped down version of the general shortest path algorithm.
|
||||
/// The general algorithm always computes two queries for each leg. This is only
|
||||
/// necessary in case of vias, where the directions of the start node is constrainted
|
||||
/// by the previous route.
|
||||
/// This variation is only an optimazation for graphs with slow queries, for example
|
||||
/// not fully contracted graphs.
|
||||
template <class DataFacadeT>
|
||||
class DirectShortestPathRouting final
|
||||
: public BasicRoutingInterface<DataFacadeT, DirectShortestPathRouting<DataFacadeT>>
|
||||
{
|
||||
using super = BasicRoutingInterface<DataFacadeT, DirectShortestPathRouting<DataFacadeT>>;
|
||||
using QueryHeap = SearchEngineData::QueryHeap;
|
||||
SearchEngineData &engine_working_data;
|
||||
|
||||
public:
|
||||
DirectShortestPathRouting(DataFacadeT *facade, SearchEngineData &engine_working_data)
|
||||
: super(facade), engine_working_data(engine_working_data)
|
||||
{
|
||||
}
|
||||
|
||||
~DirectShortestPathRouting() {}
|
||||
|
||||
void operator()(const std::vector<PhantomNodes> &phantom_nodes_vector,
|
||||
const std::vector<bool> &uturn_indicators,
|
||||
InternalRouteResult &raw_route_data) const
|
||||
{
|
||||
(void)uturn_indicators; // unused
|
||||
|
||||
// Get distance to next pair of target nodes.
|
||||
BOOST_ASSERT_MSG(1 == phantom_nodes_vector.size(),
|
||||
"Direct Shortest Path Query only accepts a single source and target pair. Multiple ones have been specified.");
|
||||
const auto& phantom_node_pair = phantom_nodes_vector.front();
|
||||
const auto& source_phantom = phantom_node_pair.source_phantom;
|
||||
const auto& target_phantom = phantom_node_pair.target_phantom;
|
||||
|
||||
engine_working_data.InitializeOrClearFirstThreadLocalStorage(
|
||||
super::facade->GetNumberOfNodes());
|
||||
QueryHeap &forward_heap = *(engine_working_data.forward_heap_1);
|
||||
QueryHeap &reverse_heap = *(engine_working_data.reverse_heap_1);
|
||||
forward_heap.Clear();
|
||||
reverse_heap.Clear();
|
||||
|
||||
BOOST_ASSERT(source_phantom.is_valid());
|
||||
BOOST_ASSERT(target_phantom.is_valid());
|
||||
|
||||
if (source_phantom.forward_node_id != SPECIAL_NODEID)
|
||||
{
|
||||
forward_heap.Insert(source_phantom.forward_node_id,
|
||||
-source_phantom.GetForwardWeightPlusOffset(),
|
||||
source_phantom.forward_node_id);
|
||||
}
|
||||
if (source_phantom.reverse_node_id != SPECIAL_NODEID)
|
||||
{
|
||||
forward_heap.Insert(source_phantom.reverse_node_id,
|
||||
-source_phantom.GetReverseWeightPlusOffset(),
|
||||
source_phantom.reverse_node_id);
|
||||
}
|
||||
|
||||
if (target_phantom.forward_node_id != SPECIAL_NODEID)
|
||||
{
|
||||
reverse_heap.Insert(target_phantom.forward_node_id,
|
||||
target_phantom.GetForwardWeightPlusOffset(),
|
||||
target_phantom.forward_node_id);
|
||||
}
|
||||
|
||||
if (target_phantom.reverse_node_id != SPECIAL_NODEID)
|
||||
{
|
||||
reverse_heap.Insert(target_phantom.reverse_node_id,
|
||||
target_phantom.GetReverseWeightPlusOffset(),
|
||||
target_phantom.reverse_node_id);
|
||||
}
|
||||
|
||||
int distance = INVALID_EDGE_WEIGHT;
|
||||
std::vector<NodeID> packed_leg;
|
||||
|
||||
if (super::facade->GetCoreSize() > 0)
|
||||
{
|
||||
engine_working_data.InitializeOrClearSecondThreadLocalStorage(
|
||||
super::facade->GetNumberOfNodes());
|
||||
QueryHeap &forward_core_heap = *(engine_working_data.forward_heap_2);
|
||||
QueryHeap &reverse_core_heap = *(engine_working_data.reverse_heap_2);
|
||||
forward_core_heap.Clear();
|
||||
reverse_core_heap.Clear();
|
||||
|
||||
|
||||
super::SearchWithCore(forward_heap, reverse_heap, forward_core_heap, reverse_core_heap,
|
||||
distance, packed_leg);
|
||||
}
|
||||
else
|
||||
{
|
||||
super::Search(forward_heap, reverse_heap, distance, packed_leg);
|
||||
}
|
||||
|
||||
// No path found for both target nodes?
|
||||
if (INVALID_EDGE_WEIGHT == distance)
|
||||
{
|
||||
raw_route_data.shortest_path_length = INVALID_EDGE_WEIGHT;
|
||||
raw_route_data.alternative_path_length = INVALID_EDGE_WEIGHT;
|
||||
return;
|
||||
}
|
||||
|
||||
BOOST_ASSERT_MSG(!packed_leg.empty(), "packed path empty");
|
||||
|
||||
raw_route_data.shortest_path_length = distance;
|
||||
raw_route_data.unpacked_path_segments.resize(1);
|
||||
raw_route_data.source_traversed_in_reverse.push_back(
|
||||
(packed_leg.front() != phantom_node_pair.source_phantom.forward_node_id));
|
||||
raw_route_data.target_traversed_in_reverse.push_back(
|
||||
(packed_leg.back() != phantom_node_pair.target_phantom.forward_node_id));
|
||||
|
||||
super::UnpackPath(packed_leg.begin(), packed_leg.end(), phantom_node_pair, raw_route_data.unpacked_path_segments.front());
|
||||
|
||||
}
|
||||
};
|
||||
|
||||
#endif /* DIRECT_SHORTEST_PATH_HPP */
|
||||
@@ -0,0 +1,259 @@
|
||||
/*
|
||||
|
||||
Copyright (c) 2014, Project OSRM contributors
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without modification,
|
||||
are permitted provided that the following conditions are met:
|
||||
|
||||
Redistributions of source code must retain the above copyright notice, this list
|
||||
of conditions and the following disclaimer.
|
||||
Redistributions in binary form must reproduce the above copyright notice, this
|
||||
list of conditions and the following disclaimer in the documentation and/or
|
||||
other materials provided with the distribution.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
|
||||
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
|
||||
ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
*/
|
||||
|
||||
#ifndef MANY_TO_MANY_ROUTING_HPP
|
||||
#define MANY_TO_MANY_ROUTING_HPP
|
||||
|
||||
#include "routing_base.hpp"
|
||||
#include "../data_structures/search_engine_data.hpp"
|
||||
#include "../typedefs.h"
|
||||
|
||||
#include <boost/assert.hpp>
|
||||
|
||||
#include <limits>
|
||||
#include <memory>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
template <class DataFacadeT>
|
||||
class ManyToManyRouting final
|
||||
: public BasicRoutingInterface<DataFacadeT, ManyToManyRouting<DataFacadeT>>
|
||||
{
|
||||
using super = BasicRoutingInterface<DataFacadeT, ManyToManyRouting<DataFacadeT>>;
|
||||
using QueryHeap = SearchEngineData::QueryHeap;
|
||||
SearchEngineData &engine_working_data;
|
||||
|
||||
struct NodeBucket
|
||||
{
|
||||
unsigned target_id; // essentially a row in the distance matrix
|
||||
EdgeWeight distance;
|
||||
NodeBucket(const unsigned target_id, const EdgeWeight distance)
|
||||
: target_id(target_id), distance(distance)
|
||||
{
|
||||
}
|
||||
};
|
||||
using SearchSpaceWithBuckets = std::unordered_map<NodeID, std::vector<NodeBucket>>;
|
||||
|
||||
public:
|
||||
ManyToManyRouting(DataFacadeT *facade, SearchEngineData &engine_working_data)
|
||||
: super(facade), engine_working_data(engine_working_data)
|
||||
{
|
||||
}
|
||||
|
||||
~ManyToManyRouting() {}
|
||||
|
||||
std::shared_ptr<std::vector<EdgeWeight>>
|
||||
operator()(const std::vector<PhantomNode> &phantom_sources_array, const std::vector<PhantomNode> &phantom_targets_array) const
|
||||
{
|
||||
const auto number_of_sources = phantom_sources_array.size();
|
||||
const auto number_of_targets = phantom_targets_array.size();
|
||||
std::shared_ptr<std::vector<EdgeWeight>> result_table =
|
||||
std::make_shared<std::vector<EdgeWeight>>(number_of_targets * number_of_sources,
|
||||
std::numeric_limits<EdgeWeight>::max());
|
||||
|
||||
engine_working_data.InitializeOrClearFirstThreadLocalStorage(
|
||||
super::facade->GetNumberOfNodes());
|
||||
|
||||
QueryHeap &query_heap = *(engine_working_data.forward_heap_1);
|
||||
|
||||
SearchSpaceWithBuckets search_space_with_buckets;
|
||||
|
||||
unsigned target_id = 0;
|
||||
for (const auto &phantom : phantom_targets_array)
|
||||
{
|
||||
query_heap.Clear();
|
||||
// insert target(s) at distance 0
|
||||
|
||||
if (SPECIAL_NODEID != phantom.forward_node_id)
|
||||
{
|
||||
query_heap.Insert(phantom.forward_node_id,
|
||||
phantom.GetForwardWeightPlusOffset(),
|
||||
phantom.forward_node_id);
|
||||
}
|
||||
if (SPECIAL_NODEID != phantom.reverse_node_id)
|
||||
{
|
||||
query_heap.Insert(phantom.reverse_node_id,
|
||||
phantom.GetReverseWeightPlusOffset(),
|
||||
phantom.reverse_node_id);
|
||||
}
|
||||
|
||||
// explore search space
|
||||
while (!query_heap.Empty())
|
||||
{
|
||||
BackwardRoutingStep(target_id, query_heap, search_space_with_buckets);
|
||||
}
|
||||
++target_id;
|
||||
}
|
||||
|
||||
// for each source do forward search
|
||||
unsigned source_id = 0;
|
||||
for (const auto &phantom : phantom_sources_array)
|
||||
{
|
||||
query_heap.Clear();
|
||||
// insert target(s) at distance 0
|
||||
|
||||
if (SPECIAL_NODEID != phantom.forward_node_id)
|
||||
{
|
||||
query_heap.Insert(phantom.forward_node_id,
|
||||
-phantom.GetForwardWeightPlusOffset(),
|
||||
phantom.forward_node_id);
|
||||
}
|
||||
if (SPECIAL_NODEID != phantom.reverse_node_id)
|
||||
{
|
||||
query_heap.Insert(phantom.reverse_node_id,
|
||||
-phantom.GetReverseWeightPlusOffset(),
|
||||
phantom.reverse_node_id);
|
||||
}
|
||||
|
||||
// explore search space
|
||||
while (!query_heap.Empty())
|
||||
{
|
||||
ForwardRoutingStep(source_id, number_of_targets, query_heap,
|
||||
search_space_with_buckets, result_table);
|
||||
}
|
||||
|
||||
++source_id;
|
||||
}
|
||||
// BOOST_ASSERT(source_id == target_id);
|
||||
return result_table;
|
||||
}
|
||||
|
||||
void ForwardRoutingStep(const unsigned source_id,
|
||||
const unsigned number_of_targets,
|
||||
QueryHeap &query_heap,
|
||||
const SearchSpaceWithBuckets &search_space_with_buckets,
|
||||
std::shared_ptr<std::vector<EdgeWeight>> result_table) const
|
||||
{
|
||||
const NodeID node = query_heap.DeleteMin();
|
||||
const int source_distance = query_heap.GetKey(node);
|
||||
|
||||
// check if each encountered node has an entry
|
||||
const auto bucket_iterator = search_space_with_buckets.find(node);
|
||||
// iterate bucket if there exists one
|
||||
if (bucket_iterator != search_space_with_buckets.end())
|
||||
{
|
||||
const std::vector<NodeBucket> &bucket_list = bucket_iterator->second;
|
||||
for (const NodeBucket ¤t_bucket : bucket_list)
|
||||
{
|
||||
// get target id from bucket entry
|
||||
const unsigned target_id = current_bucket.target_id;
|
||||
const int target_distance = current_bucket.distance;
|
||||
const EdgeWeight current_distance =
|
||||
(*result_table)[source_id * number_of_targets + target_id];
|
||||
// check if new distance is better
|
||||
const EdgeWeight new_distance = source_distance + target_distance;
|
||||
if (new_distance >= 0 && new_distance < current_distance)
|
||||
{
|
||||
(*result_table)[source_id * number_of_targets + target_id] =
|
||||
(source_distance + target_distance);
|
||||
}
|
||||
}
|
||||
}
|
||||
if (StallAtNode<true>(node, source_distance, query_heap))
|
||||
{
|
||||
return;
|
||||
}
|
||||
RelaxOutgoingEdges<true>(node, source_distance, query_heap);
|
||||
}
|
||||
|
||||
void BackwardRoutingStep(const unsigned target_id,
|
||||
QueryHeap &query_heap,
|
||||
SearchSpaceWithBuckets &search_space_with_buckets) const
|
||||
{
|
||||
const NodeID node = query_heap.DeleteMin();
|
||||
const int target_distance = query_heap.GetKey(node);
|
||||
|
||||
// store settled nodes in search space bucket
|
||||
search_space_with_buckets[node].emplace_back(target_id, target_distance);
|
||||
|
||||
if (StallAtNode<false>(node, target_distance, query_heap))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
RelaxOutgoingEdges<false>(node, target_distance, query_heap);
|
||||
}
|
||||
|
||||
template <bool forward_direction>
|
||||
inline void
|
||||
RelaxOutgoingEdges(const NodeID node, const EdgeWeight distance, QueryHeap &query_heap) const
|
||||
{
|
||||
for (auto edge : super::facade->GetAdjacentEdgeRange(node))
|
||||
{
|
||||
const auto &data = super::facade->GetEdgeData(edge);
|
||||
const bool direction_flag = (forward_direction ? data.forward : data.backward);
|
||||
if (direction_flag)
|
||||
{
|
||||
const NodeID to = super::facade->GetTarget(edge);
|
||||
const int edge_weight = data.distance;
|
||||
|
||||
BOOST_ASSERT_MSG(edge_weight > 0, "edge_weight invalid");
|
||||
const int to_distance = distance + edge_weight;
|
||||
|
||||
// New Node discovered -> Add to Heap + Node Info Storage
|
||||
if (!query_heap.WasInserted(to))
|
||||
{
|
||||
query_heap.Insert(to, to_distance, node);
|
||||
}
|
||||
// Found a shorter Path -> Update distance
|
||||
else if (to_distance < query_heap.GetKey(to))
|
||||
{
|
||||
// new parent
|
||||
query_heap.GetData(to).parent = node;
|
||||
query_heap.DecreaseKey(to, to_distance);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Stalling
|
||||
template <bool forward_direction>
|
||||
inline bool
|
||||
StallAtNode(const NodeID node, const EdgeWeight distance, QueryHeap &query_heap) const
|
||||
{
|
||||
for (auto edge : super::facade->GetAdjacentEdgeRange(node))
|
||||
{
|
||||
const auto &data = super::facade->GetEdgeData(edge);
|
||||
const bool reverse_flag = ((!forward_direction) ? data.forward : data.backward);
|
||||
if (reverse_flag)
|
||||
{
|
||||
const NodeID to = super::facade->GetTarget(edge);
|
||||
const int edge_weight = data.distance;
|
||||
BOOST_ASSERT_MSG(edge_weight > 0, "edge_weight invalid");
|
||||
if (query_heap.WasInserted(to))
|
||||
{
|
||||
if (query_heap.GetKey(to) + edge_weight < distance)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
};
|
||||
#endif
|
||||
@@ -0,0 +1,387 @@
|
||||
/*
|
||||
|
||||
Copyright (c) 2015, Project OSRM contributors
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without modification,
|
||||
are permitted provided that the following conditions are met:
|
||||
|
||||
Redistributions of source code must retain the above copyright notice, this list
|
||||
of conditions and the following disclaimer.
|
||||
Redistributions in binary form must reproduce the above copyright notice, this
|
||||
list of conditions and the following disclaimer in the documentation and/or
|
||||
other materials provided with the distribution.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
|
||||
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
|
||||
ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
*/
|
||||
|
||||
#ifndef MAP_MATCHING_HPP
|
||||
#define MAP_MATCHING_HPP
|
||||
|
||||
#include "routing_base.hpp"
|
||||
|
||||
#include "../algorithms/coordinate_calculation.hpp"
|
||||
#include "../data_structures/hidden_markov_model.hpp"
|
||||
#include "../util/json_logger.hpp"
|
||||
#include "../util/matching_debug_info.hpp"
|
||||
|
||||
#include <cstddef>
|
||||
|
||||
#include <algorithm>
|
||||
#include <deque>
|
||||
#include <iomanip>
|
||||
#include <numeric>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
namespace osrm
|
||||
{
|
||||
namespace matching
|
||||
{
|
||||
|
||||
struct SubMatching
|
||||
{
|
||||
std::vector<PhantomNode> nodes;
|
||||
std::vector<unsigned> indices;
|
||||
double length;
|
||||
double confidence;
|
||||
};
|
||||
|
||||
using CandidateList = std::vector<PhantomNodeWithDistance>;
|
||||
using CandidateLists = std::vector<CandidateList>;
|
||||
using HMM = HiddenMarkovModel<CandidateLists>;
|
||||
using SubMatchingList = std::vector<SubMatching>;
|
||||
|
||||
constexpr static const unsigned MAX_BROKEN_STATES = 10;
|
||||
|
||||
constexpr static const double MAX_SPEED = 180 / 3.6; // 180km -> m/s
|
||||
constexpr static const unsigned SUSPICIOUS_DISTANCE_DELTA = 100;
|
||||
}
|
||||
}
|
||||
|
||||
// implements a hidden markov model map matching algorithm
|
||||
template <class DataFacadeT>
|
||||
class MapMatching final : public BasicRoutingInterface<DataFacadeT, MapMatching<DataFacadeT>>
|
||||
{
|
||||
using super = BasicRoutingInterface<DataFacadeT, MapMatching<DataFacadeT>>;
|
||||
using QueryHeap = SearchEngineData::QueryHeap;
|
||||
SearchEngineData &engine_working_data;
|
||||
|
||||
unsigned GetMedianSampleTime(const std::vector<unsigned>& timestamps) const
|
||||
{
|
||||
BOOST_ASSERT(timestamps.size() > 1);
|
||||
|
||||
std::vector<unsigned> sample_times(timestamps.size());
|
||||
|
||||
std::adjacent_difference(timestamps.begin(), timestamps.end(), sample_times.begin());
|
||||
|
||||
// don't use first element of sample_times -> will not be a difference.
|
||||
auto first_elem = std::next(sample_times.begin());
|
||||
auto median = first_elem + std::distance(first_elem, sample_times.end())/2;
|
||||
std::nth_element(first_elem, median, sample_times.end());
|
||||
return *median;
|
||||
}
|
||||
|
||||
public:
|
||||
MapMatching(DataFacadeT *facade, SearchEngineData &engine_working_data)
|
||||
: super(facade), engine_working_data(engine_working_data)
|
||||
{
|
||||
}
|
||||
|
||||
void operator()(const osrm::matching::CandidateLists &candidates_list,
|
||||
const std::vector<FixedPointCoordinate> &trace_coordinates,
|
||||
const std::vector<unsigned> &trace_timestamps,
|
||||
const double matching_beta,
|
||||
const double gps_precision,
|
||||
osrm::matching::SubMatchingList &sub_matchings) const
|
||||
{
|
||||
BOOST_ASSERT(candidates_list.size() == trace_coordinates.size());
|
||||
BOOST_ASSERT(candidates_list.size() > 1);
|
||||
|
||||
const bool use_timestamps = trace_timestamps.size() > 1;
|
||||
|
||||
const auto median_sample_time = [&]() {
|
||||
if (use_timestamps)
|
||||
{
|
||||
return std::max(1u, GetMedianSampleTime(trace_timestamps));
|
||||
}
|
||||
else
|
||||
{
|
||||
return 1u;
|
||||
}
|
||||
}();
|
||||
const auto max_broken_time = median_sample_time * osrm::matching::MAX_BROKEN_STATES;
|
||||
const auto max_distance_delta = [&]() {
|
||||
if (use_timestamps)
|
||||
{
|
||||
return median_sample_time * osrm::matching::MAX_SPEED;
|
||||
}
|
||||
else
|
||||
{
|
||||
return std::numeric_limits<double>::max();
|
||||
}
|
||||
}();
|
||||
|
||||
// TODO replace default values with table lookup based on sampling frequency
|
||||
EmissionLogProbability emission_log_probability(gps_precision);
|
||||
TransitionLogProbability transition_log_probability(matching_beta);
|
||||
|
||||
osrm::matching::HMM model(candidates_list, emission_log_probability);
|
||||
|
||||
std::size_t initial_timestamp = model.initialize(0);
|
||||
if (initial_timestamp == osrm::matching::INVALID_STATE)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
MatchingDebugInfo matching_debug(osrm::json::Logger::get());
|
||||
matching_debug.initialize(candidates_list);
|
||||
|
||||
engine_working_data.InitializeOrClearFirstThreadLocalStorage(
|
||||
super::facade->GetNumberOfNodes());
|
||||
|
||||
QueryHeap &forward_heap = *(engine_working_data.forward_heap_1);
|
||||
QueryHeap &reverse_heap = *(engine_working_data.reverse_heap_1);
|
||||
|
||||
std::size_t breakage_begin = osrm::matching::INVALID_STATE;
|
||||
std::vector<std::size_t> split_points;
|
||||
std::vector<std::size_t> prev_unbroken_timestamps;
|
||||
prev_unbroken_timestamps.reserve(candidates_list.size());
|
||||
prev_unbroken_timestamps.push_back(initial_timestamp);
|
||||
for (auto t = initial_timestamp + 1; t < candidates_list.size(); ++t)
|
||||
{
|
||||
// breakage recover has removed all previous good points
|
||||
bool trace_split = prev_unbroken_timestamps.empty();
|
||||
|
||||
// use temporal information if available to determine a split
|
||||
if (use_timestamps)
|
||||
{
|
||||
trace_split =
|
||||
trace_split ||
|
||||
(trace_timestamps[t] - trace_timestamps[prev_unbroken_timestamps.back()] >
|
||||
max_broken_time);
|
||||
}
|
||||
else
|
||||
{
|
||||
trace_split = trace_split || (t - prev_unbroken_timestamps.back() >
|
||||
osrm::matching::MAX_BROKEN_STATES);
|
||||
}
|
||||
|
||||
if (trace_split)
|
||||
{
|
||||
std::size_t split_index = t;
|
||||
if (breakage_begin != osrm::matching::INVALID_STATE)
|
||||
{
|
||||
split_index = breakage_begin;
|
||||
breakage_begin = osrm::matching::INVALID_STATE;
|
||||
}
|
||||
split_points.push_back(split_index);
|
||||
|
||||
// note: this preserves everything before split_index
|
||||
model.clear(split_index);
|
||||
std::size_t new_start = model.initialize(split_index);
|
||||
// no new start was found -> stop viterbi calculation
|
||||
if (new_start == osrm::matching::INVALID_STATE)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
prev_unbroken_timestamps.clear();
|
||||
prev_unbroken_timestamps.push_back(new_start);
|
||||
// Important: We potentially go back here!
|
||||
// However since t > new_start >= breakge_begin
|
||||
// we can only reset trace_coordindates.size() times.
|
||||
t = new_start + 1;
|
||||
}
|
||||
|
||||
BOOST_ASSERT(!prev_unbroken_timestamps.empty());
|
||||
const std::size_t prev_unbroken_timestamp = prev_unbroken_timestamps.back();
|
||||
|
||||
const auto &prev_viterbi = model.viterbi[prev_unbroken_timestamp];
|
||||
const auto &prev_pruned = model.pruned[prev_unbroken_timestamp];
|
||||
const auto &prev_unbroken_timestamps_list = candidates_list[prev_unbroken_timestamp];
|
||||
const auto &prev_coordinate = trace_coordinates[prev_unbroken_timestamp];
|
||||
|
||||
auto ¤t_viterbi = model.viterbi[t];
|
||||
auto ¤t_pruned = model.pruned[t];
|
||||
auto ¤t_suspicious = model.suspicious[t];
|
||||
auto ¤t_parents = model.parents[t];
|
||||
auto ¤t_lengths = model.path_lengths[t];
|
||||
const auto ¤t_timestamps_list = candidates_list[t];
|
||||
const auto ¤t_coordinate = trace_coordinates[t];
|
||||
|
||||
const auto haversine_distance = coordinate_calculation::haversine_distance(prev_coordinate, current_coordinate);
|
||||
|
||||
// compute d_t for this timestamp and the next one
|
||||
for (const auto s : osrm::irange<std::size_t>(0u, prev_viterbi.size()))
|
||||
{
|
||||
if (prev_pruned[s])
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
for (const auto s_prime : osrm::irange<std::size_t>(0u, current_viterbi.size()))
|
||||
{
|
||||
// how likely is candidate s_prime at time t to be emitted?
|
||||
// FIXME this can be pre-computed
|
||||
const double emission_pr =
|
||||
emission_log_probability(candidates_list[t][s_prime].distance);
|
||||
double new_value = prev_viterbi[s] + emission_pr;
|
||||
if (current_viterbi[s_prime] > new_value)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
forward_heap.Clear();
|
||||
reverse_heap.Clear();
|
||||
|
||||
// get distance diff between loc1/2 and locs/s_prime
|
||||
const auto network_distance = super::get_network_distance(
|
||||
forward_heap, reverse_heap, prev_unbroken_timestamps_list[s].phantom_node,
|
||||
current_timestamps_list[s_prime].phantom_node);
|
||||
|
||||
const auto d_t = std::abs(network_distance - haversine_distance);
|
||||
|
||||
// very low probability transition -> prune
|
||||
if (d_t >= max_distance_delta)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
const double transition_pr = transition_log_probability(d_t);
|
||||
new_value += transition_pr;
|
||||
|
||||
matching_debug.add_transition_info(prev_unbroken_timestamp, t, s, s_prime,
|
||||
prev_viterbi[s], emission_pr, transition_pr,
|
||||
network_distance, haversine_distance);
|
||||
|
||||
if (new_value > current_viterbi[s_prime])
|
||||
{
|
||||
current_viterbi[s_prime] = new_value;
|
||||
current_parents[s_prime] = std::make_pair(prev_unbroken_timestamp, s);
|
||||
current_lengths[s_prime] = network_distance;
|
||||
current_pruned[s_prime] = false;
|
||||
current_suspicious[s_prime] = d_t > osrm::matching::SUSPICIOUS_DISTANCE_DELTA;
|
||||
model.breakage[t] = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (model.breakage[t])
|
||||
{
|
||||
// save start of breakage -> we need this as split point
|
||||
if (t < breakage_begin)
|
||||
{
|
||||
breakage_begin = t;
|
||||
}
|
||||
|
||||
BOOST_ASSERT(prev_unbroken_timestamps.size() > 0);
|
||||
// remove both ends of the breakage
|
||||
prev_unbroken_timestamps.pop_back();
|
||||
}
|
||||
else
|
||||
{
|
||||
prev_unbroken_timestamps.push_back(t);
|
||||
}
|
||||
}
|
||||
|
||||
matching_debug.set_viterbi(model.viterbi, model.pruned, model.suspicious);
|
||||
|
||||
if (!prev_unbroken_timestamps.empty())
|
||||
{
|
||||
split_points.push_back(prev_unbroken_timestamps.back() + 1);
|
||||
}
|
||||
|
||||
std::size_t sub_matching_begin = initial_timestamp;
|
||||
for (const auto sub_matching_end : split_points)
|
||||
{
|
||||
osrm::matching::SubMatching matching;
|
||||
|
||||
std::size_t parent_timestamp_index = sub_matching_end - 1;
|
||||
while (parent_timestamp_index >= sub_matching_begin &&
|
||||
model.breakage[parent_timestamp_index])
|
||||
{
|
||||
--parent_timestamp_index;
|
||||
}
|
||||
while (sub_matching_begin < sub_matching_end &&
|
||||
model.breakage[sub_matching_begin])
|
||||
{
|
||||
++sub_matching_begin;
|
||||
}
|
||||
|
||||
// matchings that only consist of one candidate are invalid
|
||||
if (parent_timestamp_index - sub_matching_begin + 1 < 2)
|
||||
{
|
||||
sub_matching_begin = sub_matching_end;
|
||||
continue;
|
||||
}
|
||||
|
||||
// loop through the columns, and only compare the last entry
|
||||
const auto max_element_iter =
|
||||
std::max_element(model.viterbi[parent_timestamp_index].begin(),
|
||||
model.viterbi[parent_timestamp_index].end());
|
||||
|
||||
std::size_t parent_candidate_index =
|
||||
std::distance(model.viterbi[parent_timestamp_index].begin(), max_element_iter);
|
||||
|
||||
std::deque<std::pair<std::size_t, std::size_t>> reconstructed_indices;
|
||||
while (parent_timestamp_index > sub_matching_begin)
|
||||
{
|
||||
if (model.breakage[parent_timestamp_index])
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
reconstructed_indices.emplace_front(parent_timestamp_index, parent_candidate_index);
|
||||
const auto &next = model.parents[parent_timestamp_index][parent_candidate_index];
|
||||
// make sure we can never get stuck in this loop
|
||||
if (parent_timestamp_index == next.first)
|
||||
{
|
||||
break;
|
||||
}
|
||||
parent_timestamp_index = next.first;
|
||||
parent_candidate_index = next.second;
|
||||
}
|
||||
reconstructed_indices.emplace_front(parent_timestamp_index, parent_candidate_index);
|
||||
if (reconstructed_indices.size() < 2)
|
||||
{
|
||||
sub_matching_begin = sub_matching_end;
|
||||
continue;
|
||||
}
|
||||
|
||||
matching.length = 0.0f;
|
||||
matching.nodes.resize(reconstructed_indices.size());
|
||||
matching.indices.resize(reconstructed_indices.size());
|
||||
for (const auto i : osrm::irange<std::size_t>(0u, reconstructed_indices.size()))
|
||||
{
|
||||
const auto timestamp_index = reconstructed_indices[i].first;
|
||||
const auto location_index = reconstructed_indices[i].second;
|
||||
|
||||
matching.indices[i] = timestamp_index;
|
||||
matching.nodes[i] = candidates_list[timestamp_index][location_index].phantom_node;
|
||||
matching.length += model.path_lengths[timestamp_index][location_index];
|
||||
|
||||
matching_debug.add_chosen(timestamp_index, location_index);
|
||||
}
|
||||
|
||||
sub_matchings.push_back(matching);
|
||||
sub_matching_begin = sub_matching_end;
|
||||
}
|
||||
matching_debug.add_breakage(model.breakage);
|
||||
}
|
||||
};
|
||||
|
||||
//[1] "Hidden Markov Map Matching Through Noise and Sparseness"; P. Newson and J. Krumm; 2009; ACM
|
||||
// GIS
|
||||
|
||||
#endif /* MAP_MATCHING_HPP */
|
||||
@@ -0,0 +1,687 @@
|
||||
/*
|
||||
|
||||
Copyright (c) 2015, Project OSRM contributors
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without modification,
|
||||
are permitted provided that the following conditions are met:
|
||||
|
||||
Redistributions of source code must retain the above copyright notice, this list
|
||||
of conditions and the following disclaimer.
|
||||
Redistributions in binary form must reproduce the above copyright notice, this
|
||||
list of conditions and the following disclaimer in the documentation and/or
|
||||
other materials provided with the distribution.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
|
||||
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
|
||||
ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
*/
|
||||
|
||||
#ifndef ROUTING_BASE_HPP
|
||||
#define ROUTING_BASE_HPP
|
||||
|
||||
#include "../algorithms/coordinate_calculation.hpp"
|
||||
#include "../data_structures/internal_route_result.hpp"
|
||||
#include "../data_structures/search_engine_data.hpp"
|
||||
#include "../data_structures/turn_instructions.hpp"
|
||||
|
||||
#include <boost/assert.hpp>
|
||||
|
||||
#include <stack>
|
||||
|
||||
SearchEngineData::SearchEngineHeapPtr SearchEngineData::forward_heap_1;
|
||||
SearchEngineData::SearchEngineHeapPtr SearchEngineData::reverse_heap_1;
|
||||
SearchEngineData::SearchEngineHeapPtr SearchEngineData::forward_heap_2;
|
||||
SearchEngineData::SearchEngineHeapPtr SearchEngineData::reverse_heap_2;
|
||||
SearchEngineData::SearchEngineHeapPtr SearchEngineData::forward_heap_3;
|
||||
SearchEngineData::SearchEngineHeapPtr SearchEngineData::reverse_heap_3;
|
||||
|
||||
template <class DataFacadeT, class Derived> class BasicRoutingInterface
|
||||
{
|
||||
private:
|
||||
using EdgeData = typename DataFacadeT::EdgeData;
|
||||
|
||||
protected:
|
||||
DataFacadeT *facade;
|
||||
|
||||
public:
|
||||
BasicRoutingInterface() = delete;
|
||||
BasicRoutingInterface(const BasicRoutingInterface &) = delete;
|
||||
explicit BasicRoutingInterface(DataFacadeT *facade) : facade(facade) {}
|
||||
~BasicRoutingInterface() {}
|
||||
|
||||
// min_edge_offset is needed in case we use multiple
|
||||
// nodes as start/target nodes with different (even negative) offsets.
|
||||
// In that case the termination criterion is not correct
|
||||
// anymore.
|
||||
//
|
||||
// Example:
|
||||
// forward heap: a(-100), b(0),
|
||||
// reverse heap: c(0), d(100)
|
||||
//
|
||||
// a --- d
|
||||
// \ /
|
||||
// / \
|
||||
// b --- c
|
||||
//
|
||||
// This is equivalent to running a bi-directional Dijkstra on the following graph:
|
||||
//
|
||||
// a --- d
|
||||
// / \ / \
|
||||
// y x z
|
||||
// \ / \ /
|
||||
// b --- c
|
||||
//
|
||||
// The graph is constructed by inserting nodes y and z that are connected to the initial nodes
|
||||
// using edges (y, a) with weight -100, (y, b) with weight 0 and,
|
||||
// (d, z) with weight 100, (c, z) with weight 0 corresponding.
|
||||
// Since we are dealing with a graph that contains _negative_ edges,
|
||||
// we need to add an offset to the termination criterion.
|
||||
void RoutingStep(SearchEngineData::QueryHeap &forward_heap,
|
||||
SearchEngineData::QueryHeap &reverse_heap,
|
||||
NodeID &middle_node_id,
|
||||
int &upper_bound,
|
||||
int min_edge_offset,
|
||||
const bool forward_direction,
|
||||
const bool stalling = true) const
|
||||
{
|
||||
const NodeID node = forward_heap.DeleteMin();
|
||||
const int distance = forward_heap.GetKey(node);
|
||||
|
||||
if (reverse_heap.WasInserted(node))
|
||||
{
|
||||
const int new_distance = reverse_heap.GetKey(node) + distance;
|
||||
if (new_distance < upper_bound)
|
||||
{
|
||||
if (new_distance >= 0)
|
||||
{
|
||||
middle_node_id = node;
|
||||
upper_bound = new_distance;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// make sure we don't terminate too early if we initialize the distance
|
||||
// for the nodes in the forward heap with the forward/reverse offset
|
||||
BOOST_ASSERT(min_edge_offset <= 0);
|
||||
if (distance + min_edge_offset > upper_bound)
|
||||
{
|
||||
forward_heap.DeleteAll();
|
||||
return;
|
||||
}
|
||||
|
||||
// Stalling
|
||||
if (stalling)
|
||||
{
|
||||
for (const auto edge : facade->GetAdjacentEdgeRange(node))
|
||||
{
|
||||
const EdgeData &data = facade->GetEdgeData(edge);
|
||||
const bool reverse_flag = ((!forward_direction) ? data.forward : data.backward);
|
||||
if (reverse_flag)
|
||||
{
|
||||
const NodeID to = facade->GetTarget(edge);
|
||||
const int edge_weight = data.distance;
|
||||
|
||||
BOOST_ASSERT_MSG(edge_weight > 0, "edge_weight invalid");
|
||||
|
||||
if (forward_heap.WasInserted(to))
|
||||
{
|
||||
if (forward_heap.GetKey(to) + edge_weight < distance)
|
||||
{
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (const auto edge : facade->GetAdjacentEdgeRange(node))
|
||||
{
|
||||
const EdgeData &data = facade->GetEdgeData(edge);
|
||||
bool forward_directionFlag = (forward_direction ? data.forward : data.backward);
|
||||
if (forward_directionFlag)
|
||||
{
|
||||
|
||||
const NodeID to = facade->GetTarget(edge);
|
||||
const int edge_weight = data.distance;
|
||||
|
||||
BOOST_ASSERT_MSG(edge_weight > 0, "edge_weight invalid");
|
||||
const int to_distance = distance + edge_weight;
|
||||
|
||||
// New Node discovered -> Add to Heap + Node Info Storage
|
||||
if (!forward_heap.WasInserted(to))
|
||||
{
|
||||
forward_heap.Insert(to, to_distance, node);
|
||||
}
|
||||
// Found a shorter Path -> Update distance
|
||||
else if (to_distance < forward_heap.GetKey(to))
|
||||
{
|
||||
// new parent
|
||||
forward_heap.GetData(to).parent = node;
|
||||
forward_heap.DecreaseKey(to, to_distance);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
template <typename RandomIter>
|
||||
void UnpackPath(RandomIter packed_path_begin,
|
||||
RandomIter packed_path_end,
|
||||
const PhantomNodes &phantom_node_pair,
|
||||
std::vector<PathData> &unpacked_path) const
|
||||
{
|
||||
const bool start_traversed_in_reverse =
|
||||
(*packed_path_begin != phantom_node_pair.source_phantom.forward_node_id);
|
||||
const bool target_traversed_in_reverse =
|
||||
(*std::prev(packed_path_end) != phantom_node_pair.target_phantom.forward_node_id);
|
||||
|
||||
BOOST_ASSERT(std::distance(packed_path_begin, packed_path_end) > 0);
|
||||
std::stack<std::pair<NodeID, NodeID>> recursion_stack;
|
||||
|
||||
// We have to push the path in reverse order onto the stack because it's LIFO.
|
||||
for (auto current = std::prev(packed_path_end); current != packed_path_begin;
|
||||
current = std::prev(current))
|
||||
{
|
||||
recursion_stack.emplace(*std::prev(current), *current);
|
||||
}
|
||||
|
||||
std::pair<NodeID, NodeID> edge;
|
||||
while (!recursion_stack.empty())
|
||||
{
|
||||
// edge.first edge.second
|
||||
// *------------------>*
|
||||
// edge_id
|
||||
edge = recursion_stack.top();
|
||||
recursion_stack.pop();
|
||||
|
||||
// facade->FindEdge does not suffice here in case of shortcuts.
|
||||
// The above explanation unclear? Think!
|
||||
EdgeID smaller_edge_id = SPECIAL_EDGEID;
|
||||
int edge_weight = std::numeric_limits<EdgeWeight>::max();
|
||||
for (const auto edge_id : facade->GetAdjacentEdgeRange(edge.first))
|
||||
{
|
||||
const int weight = facade->GetEdgeData(edge_id).distance;
|
||||
if ((facade->GetTarget(edge_id) == edge.second) && (weight < edge_weight) &&
|
||||
facade->GetEdgeData(edge_id).forward)
|
||||
{
|
||||
smaller_edge_id = edge_id;
|
||||
edge_weight = weight;
|
||||
}
|
||||
}
|
||||
|
||||
// edge.first edge.second
|
||||
// *<------------------*
|
||||
// edge_id
|
||||
if (SPECIAL_EDGEID == smaller_edge_id)
|
||||
{
|
||||
for (const auto edge_id : facade->GetAdjacentEdgeRange(edge.second))
|
||||
{
|
||||
const int weight = facade->GetEdgeData(edge_id).distance;
|
||||
if ((facade->GetTarget(edge_id) == edge.first) && (weight < edge_weight) &&
|
||||
facade->GetEdgeData(edge_id).backward)
|
||||
{
|
||||
smaller_edge_id = edge_id;
|
||||
edge_weight = weight;
|
||||
}
|
||||
}
|
||||
}
|
||||
BOOST_ASSERT_MSG(edge_weight != INVALID_EDGE_WEIGHT, "edge id invalid");
|
||||
|
||||
const EdgeData &ed = facade->GetEdgeData(smaller_edge_id);
|
||||
if (ed.shortcut)
|
||||
{ // unpack
|
||||
const NodeID middle_node_id = ed.id;
|
||||
// again, we need to this in reversed order
|
||||
recursion_stack.emplace(middle_node_id, edge.second);
|
||||
recursion_stack.emplace(edge.first, middle_node_id);
|
||||
}
|
||||
else
|
||||
{
|
||||
BOOST_ASSERT_MSG(!ed.shortcut, "original edge flagged as shortcut");
|
||||
unsigned name_index = facade->GetNameIndexFromEdgeID(ed.id);
|
||||
const TurnInstruction turn_instruction = facade->GetTurnInstructionForEdgeID(ed.id);
|
||||
const TravelMode travel_mode = facade->GetTravelModeForEdgeID(ed.id);
|
||||
|
||||
if (!facade->EdgeIsCompressed(ed.id))
|
||||
{
|
||||
BOOST_ASSERT(!facade->EdgeIsCompressed(ed.id));
|
||||
unpacked_path.emplace_back(facade->GetGeometryIndexForEdgeID(ed.id), name_index,
|
||||
turn_instruction, ed.distance, travel_mode);
|
||||
}
|
||||
else
|
||||
{
|
||||
std::vector<unsigned> id_vector;
|
||||
facade->GetUncompressedGeometry(facade->GetGeometryIndexForEdgeID(ed.id),
|
||||
id_vector);
|
||||
|
||||
const std::size_t start_index =
|
||||
(unpacked_path.empty()
|
||||
? ((start_traversed_in_reverse)
|
||||
? id_vector.size() -
|
||||
phantom_node_pair.source_phantom.fwd_segment_position - 1
|
||||
: phantom_node_pair.source_phantom.fwd_segment_position)
|
||||
: 0);
|
||||
const std::size_t end_index = id_vector.size();
|
||||
|
||||
BOOST_ASSERT(start_index >= 0);
|
||||
BOOST_ASSERT(start_index <= end_index);
|
||||
for (std::size_t i = start_index; i < end_index; ++i)
|
||||
{
|
||||
unpacked_path.emplace_back(id_vector[i], name_index,
|
||||
TurnInstruction::NoTurn, 0, travel_mode);
|
||||
}
|
||||
unpacked_path.back().turn_instruction = turn_instruction;
|
||||
unpacked_path.back().segment_duration = ed.distance;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (SPECIAL_EDGEID != phantom_node_pair.target_phantom.packed_geometry_id)
|
||||
{
|
||||
std::vector<unsigned> id_vector;
|
||||
facade->GetUncompressedGeometry(phantom_node_pair.target_phantom.packed_geometry_id,
|
||||
id_vector);
|
||||
const bool is_local_path = (phantom_node_pair.source_phantom.packed_geometry_id ==
|
||||
phantom_node_pair.target_phantom.packed_geometry_id) &&
|
||||
unpacked_path.empty();
|
||||
|
||||
std::size_t start_index = 0;
|
||||
if (is_local_path)
|
||||
{
|
||||
start_index = phantom_node_pair.source_phantom.fwd_segment_position;
|
||||
if (target_traversed_in_reverse)
|
||||
{
|
||||
start_index =
|
||||
id_vector.size() - phantom_node_pair.source_phantom.fwd_segment_position;
|
||||
}
|
||||
}
|
||||
|
||||
std::size_t end_index = phantom_node_pair.target_phantom.fwd_segment_position;
|
||||
if (target_traversed_in_reverse)
|
||||
{
|
||||
std::reverse(id_vector.begin(), id_vector.end());
|
||||
end_index =
|
||||
id_vector.size() - phantom_node_pair.target_phantom.fwd_segment_position;
|
||||
}
|
||||
|
||||
if (start_index > end_index)
|
||||
{
|
||||
start_index = std::min(start_index, id_vector.size() - 1);
|
||||
}
|
||||
|
||||
for (std::size_t i = start_index; i != end_index; (start_index < end_index ? ++i : --i))
|
||||
{
|
||||
BOOST_ASSERT(i < id_vector.size());
|
||||
BOOST_ASSERT(phantom_node_pair.target_phantom.forward_travel_mode > 0);
|
||||
unpacked_path.emplace_back(PathData{
|
||||
id_vector[i], phantom_node_pair.target_phantom.name_id, TurnInstruction::NoTurn,
|
||||
0, phantom_node_pair.target_phantom.forward_travel_mode});
|
||||
}
|
||||
}
|
||||
|
||||
// there is no equivalent to a node-based node in an edge-expanded graph.
|
||||
// two equivalent routes may start (or end) at different node-based edges
|
||||
// as they are added with the offset how much "distance" on the edge
|
||||
// has already been traversed. Depending on offset one needs to remove
|
||||
// the last node.
|
||||
if (unpacked_path.size() > 1)
|
||||
{
|
||||
const std::size_t last_index = unpacked_path.size() - 1;
|
||||
const std::size_t second_to_last_index = last_index - 1;
|
||||
|
||||
// looks like a trivially true check but tests for underflow
|
||||
BOOST_ASSERT(last_index > second_to_last_index);
|
||||
|
||||
if (unpacked_path[last_index].node == unpacked_path[second_to_last_index].node)
|
||||
{
|
||||
unpacked_path.pop_back();
|
||||
}
|
||||
BOOST_ASSERT(!unpacked_path.empty());
|
||||
}
|
||||
}
|
||||
|
||||
void UnpackEdge(const NodeID s, const NodeID t, std::vector<NodeID> &unpacked_path) const
|
||||
{
|
||||
std::stack<std::pair<NodeID, NodeID>> recursion_stack;
|
||||
recursion_stack.emplace(s, t);
|
||||
|
||||
std::pair<NodeID, NodeID> edge;
|
||||
while (!recursion_stack.empty())
|
||||
{
|
||||
edge = recursion_stack.top();
|
||||
recursion_stack.pop();
|
||||
|
||||
EdgeID smaller_edge_id = SPECIAL_EDGEID;
|
||||
int edge_weight = std::numeric_limits<EdgeWeight>::max();
|
||||
for (const auto edge_id : facade->GetAdjacentEdgeRange(edge.first))
|
||||
{
|
||||
const int weight = facade->GetEdgeData(edge_id).distance;
|
||||
if ((facade->GetTarget(edge_id) == edge.second) && (weight < edge_weight) &&
|
||||
facade->GetEdgeData(edge_id).forward)
|
||||
{
|
||||
smaller_edge_id = edge_id;
|
||||
edge_weight = weight;
|
||||
}
|
||||
}
|
||||
|
||||
if (SPECIAL_EDGEID == smaller_edge_id)
|
||||
{
|
||||
for (const auto edge_id : facade->GetAdjacentEdgeRange(edge.second))
|
||||
{
|
||||
const int weight = facade->GetEdgeData(edge_id).distance;
|
||||
if ((facade->GetTarget(edge_id) == edge.first) && (weight < edge_weight) &&
|
||||
facade->GetEdgeData(edge_id).backward)
|
||||
{
|
||||
smaller_edge_id = edge_id;
|
||||
edge_weight = weight;
|
||||
}
|
||||
}
|
||||
}
|
||||
BOOST_ASSERT_MSG(edge_weight != std::numeric_limits<EdgeWeight>::max(),
|
||||
"edge weight invalid");
|
||||
|
||||
const EdgeData &ed = facade->GetEdgeData(smaller_edge_id);
|
||||
if (ed.shortcut)
|
||||
{ // unpack
|
||||
const NodeID middle_node_id = ed.id;
|
||||
// again, we need to this in reversed order
|
||||
recursion_stack.emplace(middle_node_id, edge.second);
|
||||
recursion_stack.emplace(edge.first, middle_node_id);
|
||||
}
|
||||
else
|
||||
{
|
||||
BOOST_ASSERT_MSG(!ed.shortcut, "edge must be shortcut");
|
||||
unpacked_path.emplace_back(edge.first);
|
||||
}
|
||||
}
|
||||
unpacked_path.emplace_back(t);
|
||||
}
|
||||
|
||||
void RetrievePackedPathFromHeap(const SearchEngineData::QueryHeap &forward_heap,
|
||||
const SearchEngineData::QueryHeap &reverse_heap,
|
||||
const NodeID middle_node_id,
|
||||
std::vector<NodeID> &packed_path) const
|
||||
{
|
||||
RetrievePackedPathFromSingleHeap(forward_heap, middle_node_id, packed_path);
|
||||
std::reverse(packed_path.begin(), packed_path.end());
|
||||
packed_path.emplace_back(middle_node_id);
|
||||
RetrievePackedPathFromSingleHeap(reverse_heap, middle_node_id, packed_path);
|
||||
}
|
||||
|
||||
void RetrievePackedPathFromSingleHeap(const SearchEngineData::QueryHeap &search_heap,
|
||||
const NodeID middle_node_id,
|
||||
std::vector<NodeID> &packed_path) const
|
||||
{
|
||||
NodeID current_node_id = middle_node_id;
|
||||
while (current_node_id != search_heap.GetData(current_node_id).parent)
|
||||
{
|
||||
current_node_id = search_heap.GetData(current_node_id).parent;
|
||||
packed_path.emplace_back(current_node_id);
|
||||
}
|
||||
}
|
||||
|
||||
// assumes that heaps are already setup correctly.
|
||||
void Search(SearchEngineData::QueryHeap &forward_heap,
|
||||
SearchEngineData::QueryHeap &reverse_heap,
|
||||
int &distance,
|
||||
std::vector<NodeID> &packed_leg) const
|
||||
{
|
||||
NodeID middle = SPECIAL_NODEID;
|
||||
|
||||
// get offset to account for offsets on phantom nodes on compressed edges
|
||||
const auto min_edge_offset = std::min(0, forward_heap.MinKey());
|
||||
BOOST_ASSERT(min_edge_offset <= 0);
|
||||
// we only every insert negative offsets for nodes in the forward heap
|
||||
BOOST_ASSERT(reverse_heap.MinKey() >= 0);
|
||||
|
||||
// run two-Target Dijkstra routing step.
|
||||
while (0 < (forward_heap.Size() + reverse_heap.Size()))
|
||||
{
|
||||
if (!forward_heap.Empty())
|
||||
{
|
||||
RoutingStep(forward_heap, reverse_heap, middle, distance, min_edge_offset, true);
|
||||
}
|
||||
if (!reverse_heap.Empty())
|
||||
{
|
||||
RoutingStep(reverse_heap, forward_heap, middle, distance, min_edge_offset, false);
|
||||
}
|
||||
}
|
||||
|
||||
// No path found for both target nodes?
|
||||
if (INVALID_EDGE_WEIGHT == distance || SPECIAL_NODEID == middle)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
// Was a paths over one of the forward/reverse nodes not found?
|
||||
BOOST_ASSERT_MSG((SPECIAL_NODEID != middle && INVALID_EDGE_WEIGHT != distance),
|
||||
"no path found");
|
||||
|
||||
RetrievePackedPathFromHeap(forward_heap, reverse_heap, middle, packed_leg);
|
||||
}
|
||||
|
||||
// assumes that heaps are already setup correctly.
|
||||
void SearchWithCore(SearchEngineData::QueryHeap &forward_heap,
|
||||
SearchEngineData::QueryHeap &reverse_heap,
|
||||
SearchEngineData::QueryHeap &forward_core_heap,
|
||||
SearchEngineData::QueryHeap &reverse_core_heap,
|
||||
int &distance,
|
||||
std::vector<NodeID> &packed_leg) const
|
||||
{
|
||||
NodeID middle = SPECIAL_NODEID;
|
||||
|
||||
std::vector<std::pair<NodeID, EdgeWeight>> forward_entry_points;
|
||||
std::vector<std::pair<NodeID, EdgeWeight>> reverse_entry_points;
|
||||
|
||||
// get offset to account for offsets on phantom nodes on compressed edges
|
||||
const auto min_edge_offset = std::min(0, forward_heap.MinKey());
|
||||
// we only every insert negative offsets for nodes in the forward heap
|
||||
BOOST_ASSERT(reverse_heap.MinKey() >= 0);
|
||||
|
||||
// run two-Target Dijkstra routing step.
|
||||
while (0 < (forward_heap.Size() + reverse_heap.Size()))
|
||||
{
|
||||
if (!forward_heap.Empty())
|
||||
{
|
||||
if (facade->IsCoreNode(forward_heap.Min()))
|
||||
{
|
||||
const NodeID node = forward_heap.DeleteMin();
|
||||
const int key = forward_heap.GetKey(node);
|
||||
forward_entry_points.emplace_back(node, key);
|
||||
}
|
||||
else
|
||||
{
|
||||
RoutingStep(forward_heap, reverse_heap, middle, distance, min_edge_offset,
|
||||
true);
|
||||
}
|
||||
}
|
||||
if (!reverse_heap.Empty())
|
||||
{
|
||||
if (facade->IsCoreNode(reverse_heap.Min()))
|
||||
{
|
||||
const NodeID node = reverse_heap.DeleteMin();
|
||||
const int key = reverse_heap.GetKey(node);
|
||||
reverse_entry_points.emplace_back(node, key);
|
||||
}
|
||||
else
|
||||
{
|
||||
RoutingStep(reverse_heap, forward_heap, middle, distance, min_edge_offset,
|
||||
false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// TODO check if unordered_set might be faster
|
||||
// sort by id and increasing by distance
|
||||
auto entry_point_comparator = [](const std::pair<NodeID, EdgeWeight> &lhs,
|
||||
const std::pair<NodeID, EdgeWeight> &rhs)
|
||||
{
|
||||
return lhs.first < rhs.first || (lhs.first == rhs.first && lhs.second < rhs.second);
|
||||
};
|
||||
std::sort(forward_entry_points.begin(), forward_entry_points.end(), entry_point_comparator);
|
||||
std::sort(reverse_entry_points.begin(), reverse_entry_points.end(), entry_point_comparator);
|
||||
|
||||
NodeID last_id = SPECIAL_NODEID;
|
||||
for (const auto p : forward_entry_points)
|
||||
{
|
||||
if (p.first == last_id)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
forward_core_heap.Insert(p.first, p.second, p.first);
|
||||
last_id = p.first;
|
||||
}
|
||||
last_id = SPECIAL_NODEID;
|
||||
for (const auto p : reverse_entry_points)
|
||||
{
|
||||
if (p.first == last_id)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
reverse_core_heap.Insert(p.first, p.second, p.first);
|
||||
last_id = p.first;
|
||||
}
|
||||
|
||||
// get offset to account for offsets on phantom nodes on compressed edges
|
||||
int min_core_edge_offset = 0;
|
||||
if (forward_core_heap.Size() > 0)
|
||||
{
|
||||
min_core_edge_offset = std::min(min_core_edge_offset, forward_core_heap.MinKey());
|
||||
}
|
||||
if (reverse_core_heap.Size() > 0 && reverse_core_heap.MinKey() < 0)
|
||||
{
|
||||
min_core_edge_offset = std::min(min_core_edge_offset, reverse_core_heap.MinKey());
|
||||
}
|
||||
BOOST_ASSERT(min_core_edge_offset <= 0);
|
||||
|
||||
// run two-target Dijkstra routing step on core with termination criterion
|
||||
while (0 < (forward_core_heap.Size() + reverse_core_heap.Size()) &&
|
||||
distance > (forward_core_heap.MinKey() + reverse_core_heap.MinKey()))
|
||||
{
|
||||
if (!forward_core_heap.Empty())
|
||||
{
|
||||
RoutingStep(forward_core_heap, reverse_core_heap, middle, distance,
|
||||
min_core_edge_offset, true, false);
|
||||
}
|
||||
if (!reverse_core_heap.Empty())
|
||||
{
|
||||
RoutingStep(reverse_core_heap, forward_core_heap, middle, distance,
|
||||
min_core_edge_offset, false, false);
|
||||
}
|
||||
}
|
||||
|
||||
// No path found for both target nodes?
|
||||
if (INVALID_EDGE_WEIGHT == distance || SPECIAL_NODEID == middle)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
// Was a paths over one of the forward/reverse nodes not found?
|
||||
BOOST_ASSERT_MSG((SPECIAL_NODEID != middle && INVALID_EDGE_WEIGHT != distance),
|
||||
"no path found");
|
||||
|
||||
// we need to unpack sub path from core heaps
|
||||
if (facade->IsCoreNode(middle))
|
||||
{
|
||||
std::vector<NodeID> packed_core_leg;
|
||||
RetrievePackedPathFromHeap(forward_core_heap, reverse_core_heap, middle,
|
||||
packed_core_leg);
|
||||
BOOST_ASSERT(packed_core_leg.size() > 0);
|
||||
RetrievePackedPathFromSingleHeap(forward_heap, packed_core_leg.front(), packed_leg);
|
||||
std::reverse(packed_leg.begin(), packed_leg.end());
|
||||
packed_leg.insert(packed_leg.end(), packed_core_leg.begin(), packed_core_leg.end());
|
||||
RetrievePackedPathFromSingleHeap(reverse_heap, packed_core_leg.back(), packed_leg);
|
||||
}
|
||||
else
|
||||
{
|
||||
RetrievePackedPathFromHeap(forward_heap, reverse_heap, middle, packed_leg);
|
||||
}
|
||||
}
|
||||
|
||||
double get_network_distance(SearchEngineData::QueryHeap &forward_heap,
|
||||
SearchEngineData::QueryHeap &reverse_heap,
|
||||
const PhantomNode &source_phantom,
|
||||
const PhantomNode &target_phantom) const
|
||||
{
|
||||
EdgeWeight upper_bound = INVALID_EDGE_WEIGHT;
|
||||
NodeID middle_node = SPECIAL_NODEID;
|
||||
EdgeWeight edge_offset = std::min(0, -source_phantom.GetForwardWeightPlusOffset());
|
||||
edge_offset = std::min(edge_offset, -source_phantom.GetReverseWeightPlusOffset());
|
||||
|
||||
if (source_phantom.forward_node_id != SPECIAL_NODEID)
|
||||
{
|
||||
forward_heap.Insert(source_phantom.forward_node_id,
|
||||
-source_phantom.GetForwardWeightPlusOffset(),
|
||||
source_phantom.forward_node_id);
|
||||
}
|
||||
if (source_phantom.reverse_node_id != SPECIAL_NODEID)
|
||||
{
|
||||
forward_heap.Insert(source_phantom.reverse_node_id,
|
||||
-source_phantom.GetReverseWeightPlusOffset(),
|
||||
source_phantom.reverse_node_id);
|
||||
}
|
||||
|
||||
if (target_phantom.forward_node_id != SPECIAL_NODEID)
|
||||
{
|
||||
reverse_heap.Insert(target_phantom.forward_node_id,
|
||||
target_phantom.GetForwardWeightPlusOffset(),
|
||||
target_phantom.forward_node_id);
|
||||
}
|
||||
if (target_phantom.reverse_node_id != SPECIAL_NODEID)
|
||||
{
|
||||
reverse_heap.Insert(target_phantom.reverse_node_id,
|
||||
target_phantom.GetReverseWeightPlusOffset(),
|
||||
target_phantom.reverse_node_id);
|
||||
}
|
||||
|
||||
// search from s and t till new_min/(1+epsilon) > length_of_shortest_path
|
||||
while (0 < (forward_heap.Size() + reverse_heap.Size()))
|
||||
{
|
||||
if (0 < forward_heap.Size())
|
||||
{
|
||||
RoutingStep(forward_heap, reverse_heap, middle_node, upper_bound, edge_offset,
|
||||
true);
|
||||
}
|
||||
if (0 < reverse_heap.Size())
|
||||
{
|
||||
RoutingStep(reverse_heap, forward_heap, middle_node, upper_bound, edge_offset,
|
||||
false);
|
||||
}
|
||||
}
|
||||
|
||||
double distance = std::numeric_limits<double>::max();
|
||||
if (upper_bound != INVALID_EDGE_WEIGHT)
|
||||
{
|
||||
std::vector<NodeID> packed_leg;
|
||||
RetrievePackedPathFromHeap(forward_heap, reverse_heap, middle_node, packed_leg);
|
||||
std::vector<PathData> unpacked_path;
|
||||
PhantomNodes nodes;
|
||||
nodes.source_phantom = source_phantom;
|
||||
nodes.target_phantom = target_phantom;
|
||||
UnpackPath(packed_leg.begin(), packed_leg.end(), nodes, unpacked_path);
|
||||
|
||||
FixedPointCoordinate previous_coordinate = source_phantom.location;
|
||||
FixedPointCoordinate current_coordinate;
|
||||
distance = 0;
|
||||
for (const auto &p : unpacked_path)
|
||||
{
|
||||
current_coordinate = facade->GetCoordinateOfNode(p.node);
|
||||
distance += coordinate_calculation::haversine_distance(previous_coordinate,
|
||||
current_coordinate);
|
||||
previous_coordinate = current_coordinate;
|
||||
}
|
||||
distance += coordinate_calculation::haversine_distance(previous_coordinate,
|
||||
target_phantom.location);
|
||||
}
|
||||
return distance;
|
||||
}
|
||||
};
|
||||
|
||||
#endif // ROUTING_BASE_HPP
|
||||
@@ -0,0 +1,536 @@
|
||||
/*
|
||||
|
||||
Copyright (c) 2015, Project OSRM contributors
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without modification,
|
||||
are permitted provided that the following conditions are met:
|
||||
|
||||
Redistributions of source code must retain the above copyright notice, this list
|
||||
of conditions and the following disclaimer.
|
||||
Redistributions in binary form must reproduce the above copyright notice, this
|
||||
list of conditions and the following disclaimer in the documentation and/or
|
||||
other materials provided with the distribution.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
|
||||
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
|
||||
ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
*/
|
||||
|
||||
#ifndef SHORTEST_PATH_HPP
|
||||
#define SHORTEST_PATH_HPP
|
||||
|
||||
#include "../typedefs.h"
|
||||
|
||||
#include "routing_base.hpp"
|
||||
|
||||
#include "../data_structures/search_engine_data.hpp"
|
||||
#include "../util/integer_range.hpp"
|
||||
|
||||
#include <boost/assert.hpp>
|
||||
|
||||
template <class DataFacadeT>
|
||||
class ShortestPathRouting final
|
||||
: public BasicRoutingInterface<DataFacadeT, ShortestPathRouting<DataFacadeT>>
|
||||
{
|
||||
using super = BasicRoutingInterface<DataFacadeT, ShortestPathRouting<DataFacadeT>>;
|
||||
using QueryHeap = SearchEngineData::QueryHeap;
|
||||
SearchEngineData &engine_working_data;
|
||||
|
||||
public:
|
||||
ShortestPathRouting(DataFacadeT *facade, SearchEngineData &engine_working_data)
|
||||
: super(facade), engine_working_data(engine_working_data)
|
||||
{
|
||||
}
|
||||
|
||||
~ShortestPathRouting() {}
|
||||
|
||||
// allows a uturn at the target_phantom
|
||||
// searches source forward/reverse -> target forward/reverse
|
||||
void SearchWithUTurn(QueryHeap &forward_heap,
|
||||
QueryHeap &reverse_heap,
|
||||
const bool search_from_forward_node,
|
||||
const bool search_from_reverse_node,
|
||||
const bool search_to_forward_node,
|
||||
const bool search_to_reverse_node,
|
||||
const PhantomNode &source_phantom,
|
||||
const PhantomNode &target_phantom,
|
||||
const int total_distance_to_forward,
|
||||
const int total_distance_to_reverse,
|
||||
int &new_total_distance,
|
||||
std::vector<NodeID> &leg_packed_path) const
|
||||
{
|
||||
forward_heap.Clear();
|
||||
reverse_heap.Clear();
|
||||
if (search_from_forward_node)
|
||||
{
|
||||
forward_heap.Insert(source_phantom.forward_node_id,
|
||||
total_distance_to_forward -
|
||||
source_phantom.GetForwardWeightPlusOffset(),
|
||||
source_phantom.forward_node_id);
|
||||
}
|
||||
if (search_from_reverse_node)
|
||||
{
|
||||
forward_heap.Insert(source_phantom.reverse_node_id,
|
||||
total_distance_to_reverse -
|
||||
source_phantom.GetReverseWeightPlusOffset(),
|
||||
source_phantom.reverse_node_id);
|
||||
}
|
||||
if (search_to_forward_node)
|
||||
{
|
||||
reverse_heap.Insert(target_phantom.forward_node_id,
|
||||
target_phantom.GetForwardWeightPlusOffset(),
|
||||
target_phantom.forward_node_id);
|
||||
}
|
||||
if (search_to_reverse_node)
|
||||
{
|
||||
reverse_heap.Insert(target_phantom.reverse_node_id,
|
||||
target_phantom.GetReverseWeightPlusOffset(),
|
||||
target_phantom.reverse_node_id);
|
||||
}
|
||||
BOOST_ASSERT(forward_heap.Size() > 0);
|
||||
BOOST_ASSERT(reverse_heap.Size() > 0);
|
||||
super::Search(forward_heap, reverse_heap, new_total_distance, leg_packed_path);
|
||||
}
|
||||
|
||||
// If source and target are reverse on a oneway we need to find a path
|
||||
// that connects the two. This is _not_ the shortest path in our model,
|
||||
// as source and target are on the same edge based node.
|
||||
// We force a detour by inserting "virtaul vias", which means we search a path
|
||||
// from all nodes that are connected by outgoing edges to all nodes that are connected by
|
||||
// incoming edges.
|
||||
// ------^
|
||||
// | ^source
|
||||
// | ^
|
||||
// | ^target
|
||||
// ------^
|
||||
void SearchLoop(QueryHeap &forward_heap,
|
||||
QueryHeap &reverse_heap,
|
||||
const bool search_forward_node,
|
||||
const bool search_reverse_node,
|
||||
const PhantomNode &source_phantom,
|
||||
const PhantomNode &target_phantom,
|
||||
const int total_distance_to_forward,
|
||||
const int total_distance_to_reverse,
|
||||
int &new_total_distance_to_forward,
|
||||
int &new_total_distance_to_reverse,
|
||||
std::vector<NodeID> &leg_packed_path_forward,
|
||||
std::vector<NodeID> &leg_packed_path_reverse) const
|
||||
{
|
||||
BOOST_ASSERT(source_phantom.forward_node_id == target_phantom.forward_node_id);
|
||||
BOOST_ASSERT(source_phantom.reverse_node_id == target_phantom.reverse_node_id);
|
||||
|
||||
if (search_forward_node)
|
||||
{
|
||||
forward_heap.Clear();
|
||||
reverse_heap.Clear();
|
||||
|
||||
auto node_id = source_phantom.forward_node_id;
|
||||
|
||||
for (const auto edge : super::facade->GetAdjacentEdgeRange(node_id))
|
||||
{
|
||||
const auto& data = super::facade->GetEdgeData(edge);
|
||||
if (data.forward)
|
||||
{
|
||||
auto target = super::facade->GetTarget(edge);
|
||||
auto offset = total_distance_to_forward + data.distance - source_phantom.GetForwardWeightPlusOffset();
|
||||
forward_heap.Insert(target, offset, target);
|
||||
}
|
||||
|
||||
if (data.backward)
|
||||
{
|
||||
auto target = super::facade->GetTarget(edge);
|
||||
auto offset = data.distance + target_phantom.GetForwardWeightPlusOffset();
|
||||
reverse_heap.Insert(target, offset, target);
|
||||
}
|
||||
}
|
||||
|
||||
BOOST_ASSERT(forward_heap.Size() > 0);
|
||||
BOOST_ASSERT(reverse_heap.Size() > 0);
|
||||
super::Search(forward_heap, reverse_heap, new_total_distance_to_forward,
|
||||
leg_packed_path_forward);
|
||||
|
||||
// insert node to both endpoints to close the leg
|
||||
leg_packed_path_forward.push_back(node_id);
|
||||
std::reverse(leg_packed_path_forward.begin(), leg_packed_path_forward.end());
|
||||
leg_packed_path_forward.push_back(node_id);
|
||||
std::reverse(leg_packed_path_forward.begin(), leg_packed_path_forward.end());
|
||||
}
|
||||
|
||||
if (search_reverse_node)
|
||||
{
|
||||
forward_heap.Clear();
|
||||
reverse_heap.Clear();
|
||||
|
||||
auto node_id = source_phantom.reverse_node_id;
|
||||
|
||||
for (const auto edge : super::facade->GetAdjacentEdgeRange(node_id))
|
||||
{
|
||||
const auto& data = super::facade->GetEdgeData(edge);
|
||||
if (data.forward)
|
||||
{
|
||||
auto target = super::facade->GetTarget(edge);
|
||||
auto offset = total_distance_to_reverse + data.distance - source_phantom.GetReverseWeightPlusOffset();
|
||||
forward_heap.Insert(target, offset, target);
|
||||
}
|
||||
|
||||
if (data.backward)
|
||||
{
|
||||
auto target = super::facade->GetTarget(edge);
|
||||
auto offset = data.distance + target_phantom.GetReverseWeightPlusOffset();
|
||||
reverse_heap.Insert(target, offset, target);
|
||||
}
|
||||
}
|
||||
|
||||
BOOST_ASSERT(forward_heap.Size() > 0);
|
||||
BOOST_ASSERT(reverse_heap.Size() > 0);
|
||||
super::Search(forward_heap, reverse_heap, new_total_distance_to_reverse,
|
||||
leg_packed_path_reverse);
|
||||
|
||||
// insert node to both endpoints to close the leg
|
||||
leg_packed_path_reverse.push_back(node_id);
|
||||
std::reverse(leg_packed_path_reverse.begin(), leg_packed_path_reverse.end());
|
||||
leg_packed_path_reverse.push_back(node_id);
|
||||
std::reverse(leg_packed_path_reverse.begin(), leg_packed_path_reverse.end());
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// searches shortest path between:
|
||||
// source forward/reverse -> target forward
|
||||
// source forward/reverse -> target reverse
|
||||
void Search(QueryHeap &forward_heap,
|
||||
QueryHeap &reverse_heap,
|
||||
const bool search_from_forward_node,
|
||||
const bool search_from_reverse_node,
|
||||
const bool search_to_forward_node,
|
||||
const bool search_to_reverse_node,
|
||||
const PhantomNode &source_phantom,
|
||||
const PhantomNode &target_phantom,
|
||||
const int total_distance_to_forward,
|
||||
const int total_distance_to_reverse,
|
||||
int &new_total_distance_to_forward,
|
||||
int &new_total_distance_to_reverse,
|
||||
std::vector<NodeID> &leg_packed_path_forward,
|
||||
std::vector<NodeID> &leg_packed_path_reverse) const
|
||||
{
|
||||
if (search_to_forward_node)
|
||||
{
|
||||
forward_heap.Clear();
|
||||
reverse_heap.Clear();
|
||||
reverse_heap.Insert(target_phantom.forward_node_id,
|
||||
target_phantom.GetForwardWeightPlusOffset(),
|
||||
target_phantom.forward_node_id);
|
||||
|
||||
if (search_from_forward_node)
|
||||
{
|
||||
forward_heap.Insert(source_phantom.forward_node_id,
|
||||
total_distance_to_forward -
|
||||
source_phantom.GetForwardWeightPlusOffset(),
|
||||
source_phantom.forward_node_id);
|
||||
}
|
||||
if (search_from_reverse_node)
|
||||
{
|
||||
forward_heap.Insert(source_phantom.reverse_node_id,
|
||||
total_distance_to_reverse -
|
||||
source_phantom.GetReverseWeightPlusOffset(),
|
||||
source_phantom.reverse_node_id);
|
||||
}
|
||||
BOOST_ASSERT(forward_heap.Size() > 0);
|
||||
BOOST_ASSERT(reverse_heap.Size() > 0);
|
||||
super::Search(forward_heap, reverse_heap, new_total_distance_to_forward,
|
||||
leg_packed_path_forward);
|
||||
}
|
||||
|
||||
if (search_to_reverse_node)
|
||||
{
|
||||
forward_heap.Clear();
|
||||
reverse_heap.Clear();
|
||||
reverse_heap.Insert(target_phantom.reverse_node_id,
|
||||
target_phantom.GetReverseWeightPlusOffset(),
|
||||
target_phantom.reverse_node_id);
|
||||
if (search_from_forward_node)
|
||||
{
|
||||
forward_heap.Insert(source_phantom.forward_node_id,
|
||||
total_distance_to_forward -
|
||||
source_phantom.GetForwardWeightPlusOffset(),
|
||||
source_phantom.forward_node_id);
|
||||
}
|
||||
if (search_from_reverse_node)
|
||||
{
|
||||
forward_heap.Insert(source_phantom.reverse_node_id,
|
||||
total_distance_to_reverse -
|
||||
source_phantom.GetReverseWeightPlusOffset(),
|
||||
source_phantom.reverse_node_id);
|
||||
}
|
||||
BOOST_ASSERT(forward_heap.Size() > 0);
|
||||
BOOST_ASSERT(reverse_heap.Size() > 0);
|
||||
super::Search(forward_heap, reverse_heap, new_total_distance_to_reverse,
|
||||
leg_packed_path_reverse);
|
||||
}
|
||||
}
|
||||
|
||||
void UnpackLegs(const std::vector<PhantomNodes> &phantom_nodes_vector,
|
||||
const std::vector<NodeID> &total_packed_path,
|
||||
const std::vector<std::size_t> &packed_leg_begin,
|
||||
const int shortest_path_length,
|
||||
InternalRouteResult &raw_route_data) const
|
||||
{
|
||||
raw_route_data.unpacked_path_segments.resize(packed_leg_begin.size() - 1);
|
||||
|
||||
raw_route_data.shortest_path_length = shortest_path_length;
|
||||
|
||||
for (const auto current_leg : osrm::irange<std::size_t>(0, packed_leg_begin.size() - 1))
|
||||
{
|
||||
auto leg_begin = total_packed_path.begin() + packed_leg_begin[current_leg];
|
||||
auto leg_end = total_packed_path.begin() + packed_leg_begin[current_leg + 1];
|
||||
const auto &unpack_phantom_node_pair = phantom_nodes_vector[current_leg];
|
||||
super::UnpackPath(leg_begin, leg_end, unpack_phantom_node_pair,
|
||||
raw_route_data.unpacked_path_segments[current_leg]);
|
||||
|
||||
raw_route_data.source_traversed_in_reverse.push_back(
|
||||
(*leg_begin != phantom_nodes_vector[current_leg].source_phantom.forward_node_id));
|
||||
raw_route_data.target_traversed_in_reverse.push_back(
|
||||
(*std::prev(leg_end) !=
|
||||
phantom_nodes_vector[current_leg].target_phantom.forward_node_id));
|
||||
}
|
||||
}
|
||||
|
||||
void operator()(const std::vector<PhantomNodes> &phantom_nodes_vector,
|
||||
const std::vector<bool> &uturn_indicators,
|
||||
InternalRouteResult &raw_route_data) const
|
||||
{
|
||||
BOOST_ASSERT(uturn_indicators.size() == phantom_nodes_vector.size() + 1);
|
||||
engine_working_data.InitializeOrClearFirstThreadLocalStorage(
|
||||
super::facade->GetNumberOfNodes());
|
||||
|
||||
QueryHeap &forward_heap = *(engine_working_data.forward_heap_1);
|
||||
QueryHeap &reverse_heap = *(engine_working_data.reverse_heap_1);
|
||||
|
||||
int total_distance_to_forward = 0;
|
||||
int total_distance_to_reverse = 0;
|
||||
bool search_from_forward_node = phantom_nodes_vector.front().source_phantom.forward_node_id != SPECIAL_NODEID;
|
||||
bool search_from_reverse_node = phantom_nodes_vector.front().source_phantom.reverse_node_id != SPECIAL_NODEID;
|
||||
|
||||
std::vector<NodeID> prev_packed_leg_to_forward;
|
||||
std::vector<NodeID> prev_packed_leg_to_reverse;
|
||||
|
||||
std::vector<NodeID> total_packed_path_to_forward;
|
||||
std::vector<std::size_t> packed_leg_to_forward_begin;
|
||||
std::vector<NodeID> total_packed_path_to_reverse;
|
||||
std::vector<std::size_t> packed_leg_to_reverse_begin;
|
||||
|
||||
std::size_t current_leg = 0;
|
||||
// this implements a dynamic program that finds the shortest route through
|
||||
// a list of vias
|
||||
for (const auto &phantom_node_pair : phantom_nodes_vector)
|
||||
{
|
||||
int new_total_distance_to_forward = INVALID_EDGE_WEIGHT;
|
||||
int new_total_distance_to_reverse = INVALID_EDGE_WEIGHT;
|
||||
|
||||
std::vector<NodeID> packed_leg_to_forward;
|
||||
std::vector<NodeID> packed_leg_to_reverse;
|
||||
|
||||
const auto &source_phantom = phantom_node_pair.source_phantom;
|
||||
const auto &target_phantom = phantom_node_pair.target_phantom;
|
||||
|
||||
|
||||
BOOST_ASSERT(current_leg + 1 < uturn_indicators.size());
|
||||
const bool allow_u_turn_at_via = uturn_indicators[current_leg + 1];
|
||||
|
||||
bool search_to_forward_node = target_phantom.forward_node_id != SPECIAL_NODEID;
|
||||
bool search_to_reverse_node = target_phantom.reverse_node_id != SPECIAL_NODEID;
|
||||
|
||||
BOOST_ASSERT(!search_from_forward_node || source_phantom.forward_node_id != SPECIAL_NODEID);
|
||||
BOOST_ASSERT(!search_from_reverse_node || source_phantom.reverse_node_id != SPECIAL_NODEID);
|
||||
|
||||
if (source_phantom.forward_node_id == target_phantom.forward_node_id &&
|
||||
source_phantom.GetForwardWeightPlusOffset() > target_phantom.GetForwardWeightPlusOffset())
|
||||
{
|
||||
search_to_forward_node = search_from_reverse_node;
|
||||
}
|
||||
if (source_phantom.reverse_node_id == target_phantom.reverse_node_id &&
|
||||
source_phantom.GetReverseWeightPlusOffset() > target_phantom.GetReverseWeightPlusOffset())
|
||||
{
|
||||
search_to_reverse_node = search_from_forward_node;
|
||||
}
|
||||
|
||||
BOOST_ASSERT(search_from_forward_node || search_from_reverse_node);
|
||||
|
||||
if(search_to_reverse_node || search_to_forward_node)
|
||||
{
|
||||
if (allow_u_turn_at_via)
|
||||
{
|
||||
SearchWithUTurn(forward_heap, reverse_heap, search_from_forward_node,
|
||||
search_from_reverse_node, search_to_forward_node,
|
||||
search_to_reverse_node, source_phantom, target_phantom,
|
||||
total_distance_to_forward, total_distance_to_reverse,
|
||||
new_total_distance_to_forward, packed_leg_to_forward);
|
||||
// if only the reverse node is valid (e.g. when using the match plugin) we actually need to move
|
||||
if (target_phantom.forward_node_id == SPECIAL_NODEID)
|
||||
{
|
||||
BOOST_ASSERT(target_phantom.reverse_node_id != SPECIAL_NODEID);
|
||||
new_total_distance_to_reverse = new_total_distance_to_forward;
|
||||
packed_leg_to_reverse = std::move(packed_leg_to_forward);
|
||||
new_total_distance_to_forward = INVALID_EDGE_WEIGHT;
|
||||
}
|
||||
else if (target_phantom.reverse_node_id != SPECIAL_NODEID)
|
||||
{
|
||||
new_total_distance_to_reverse = new_total_distance_to_forward;
|
||||
packed_leg_to_reverse = packed_leg_to_forward;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
Search(forward_heap, reverse_heap, search_from_forward_node,
|
||||
search_from_reverse_node, search_to_forward_node, search_to_reverse_node,
|
||||
source_phantom, target_phantom, total_distance_to_forward,
|
||||
total_distance_to_reverse, new_total_distance_to_forward,
|
||||
new_total_distance_to_reverse, packed_leg_to_forward, packed_leg_to_reverse);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
search_to_forward_node = target_phantom.forward_node_id != SPECIAL_NODEID;
|
||||
search_to_reverse_node = target_phantom.reverse_node_id != SPECIAL_NODEID;
|
||||
BOOST_ASSERT(search_from_reverse_node == search_to_reverse_node);
|
||||
BOOST_ASSERT(search_from_forward_node == search_to_forward_node);
|
||||
SearchLoop(forward_heap, reverse_heap, search_from_forward_node,
|
||||
search_from_reverse_node, source_phantom, target_phantom, total_distance_to_forward,
|
||||
total_distance_to_reverse, new_total_distance_to_forward,
|
||||
new_total_distance_to_reverse, packed_leg_to_forward, packed_leg_to_reverse);
|
||||
}
|
||||
|
||||
// No path found for both target nodes?
|
||||
if ((INVALID_EDGE_WEIGHT == new_total_distance_to_forward) &&
|
||||
(INVALID_EDGE_WEIGHT == new_total_distance_to_reverse))
|
||||
{
|
||||
raw_route_data.shortest_path_length = INVALID_EDGE_WEIGHT;
|
||||
raw_route_data.alternative_path_length = INVALID_EDGE_WEIGHT;
|
||||
return;
|
||||
}
|
||||
|
||||
// we need to figure out how the new legs connect to the previous ones
|
||||
if (current_leg > 0)
|
||||
{
|
||||
bool forward_to_forward =
|
||||
(new_total_distance_to_forward != INVALID_EDGE_WEIGHT) &&
|
||||
packed_leg_to_forward.front() == source_phantom.forward_node_id;
|
||||
bool reverse_to_forward =
|
||||
(new_total_distance_to_forward != INVALID_EDGE_WEIGHT) &&
|
||||
packed_leg_to_forward.front() == source_phantom.reverse_node_id;
|
||||
bool forward_to_reverse =
|
||||
(new_total_distance_to_reverse != INVALID_EDGE_WEIGHT) &&
|
||||
packed_leg_to_reverse.front() == source_phantom.forward_node_id;
|
||||
bool reverse_to_reverse =
|
||||
(new_total_distance_to_reverse != INVALID_EDGE_WEIGHT) &&
|
||||
packed_leg_to_reverse.front() == source_phantom.reverse_node_id;
|
||||
|
||||
BOOST_ASSERT(!forward_to_forward || !reverse_to_forward);
|
||||
BOOST_ASSERT(!forward_to_reverse || !reverse_to_reverse);
|
||||
|
||||
// in this case we always need to copy
|
||||
if (forward_to_forward && forward_to_reverse)
|
||||
{
|
||||
// in this case we copy the path leading to the source forward node
|
||||
// and change the case
|
||||
total_packed_path_to_reverse = total_packed_path_to_forward;
|
||||
packed_leg_to_reverse_begin = packed_leg_to_forward_begin;
|
||||
forward_to_reverse = false;
|
||||
reverse_to_reverse = true;
|
||||
}
|
||||
else if (reverse_to_forward && reverse_to_reverse)
|
||||
{
|
||||
total_packed_path_to_forward = total_packed_path_to_reverse;
|
||||
packed_leg_to_forward_begin = packed_leg_to_reverse_begin;
|
||||
reverse_to_forward = false;
|
||||
forward_to_forward = true;
|
||||
}
|
||||
BOOST_ASSERT(!forward_to_forward || !forward_to_reverse);
|
||||
BOOST_ASSERT(!reverse_to_forward || !reverse_to_reverse);
|
||||
|
||||
// in this case we just need to swap to regain the correct mapping
|
||||
if (reverse_to_forward || forward_to_reverse)
|
||||
{
|
||||
total_packed_path_to_forward.swap(total_packed_path_to_reverse);
|
||||
packed_leg_to_forward_begin.swap(packed_leg_to_reverse_begin);
|
||||
}
|
||||
}
|
||||
|
||||
if (new_total_distance_to_forward != INVALID_EDGE_WEIGHT)
|
||||
{
|
||||
BOOST_ASSERT(target_phantom.forward_node_id != SPECIAL_NODEID);
|
||||
|
||||
packed_leg_to_forward_begin.push_back(total_packed_path_to_forward.size());
|
||||
total_packed_path_to_forward.insert(total_packed_path_to_forward.end(),
|
||||
packed_leg_to_forward.begin(),
|
||||
packed_leg_to_forward.end());
|
||||
search_from_forward_node = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
total_packed_path_to_forward.clear();
|
||||
packed_leg_to_forward_begin.clear();
|
||||
search_from_forward_node = false;
|
||||
}
|
||||
|
||||
if (new_total_distance_to_reverse != INVALID_EDGE_WEIGHT)
|
||||
{
|
||||
BOOST_ASSERT(target_phantom.reverse_node_id != SPECIAL_NODEID);
|
||||
|
||||
packed_leg_to_reverse_begin.push_back(total_packed_path_to_reverse.size());
|
||||
total_packed_path_to_reverse.insert(total_packed_path_to_reverse.end(),
|
||||
packed_leg_to_reverse.begin(),
|
||||
packed_leg_to_reverse.end());
|
||||
search_from_reverse_node = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
total_packed_path_to_reverse.clear();
|
||||
packed_leg_to_reverse_begin.clear();
|
||||
search_from_reverse_node = false;
|
||||
}
|
||||
|
||||
prev_packed_leg_to_forward = std::move(packed_leg_to_forward);
|
||||
prev_packed_leg_to_reverse = std::move(packed_leg_to_reverse);
|
||||
|
||||
total_distance_to_forward = new_total_distance_to_forward;
|
||||
total_distance_to_reverse = new_total_distance_to_reverse;
|
||||
|
||||
++current_leg;
|
||||
}
|
||||
|
||||
BOOST_ASSERT(total_distance_to_forward != INVALID_EDGE_WEIGHT ||
|
||||
total_distance_to_reverse != INVALID_EDGE_WEIGHT);
|
||||
|
||||
// We make sure the fastest route is always in packed_legs_to_forward
|
||||
if (total_distance_to_forward > total_distance_to_reverse)
|
||||
{
|
||||
// insert sentinel
|
||||
packed_leg_to_reverse_begin.push_back(total_packed_path_to_reverse.size());
|
||||
BOOST_ASSERT(packed_leg_to_reverse_begin.size() == phantom_nodes_vector.size() + 1);
|
||||
|
||||
UnpackLegs(phantom_nodes_vector, total_packed_path_to_reverse,
|
||||
packed_leg_to_reverse_begin, total_distance_to_reverse, raw_route_data);
|
||||
}
|
||||
else
|
||||
{
|
||||
// insert sentinel
|
||||
packed_leg_to_forward_begin.push_back(total_packed_path_to_forward.size());
|
||||
BOOST_ASSERT(packed_leg_to_forward_begin.size() == phantom_nodes_vector.size() + 1);
|
||||
|
||||
UnpackLegs(phantom_nodes_vector, total_packed_path_to_forward,
|
||||
packed_leg_to_forward_begin, total_distance_to_forward, raw_route_data);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
#endif /* SHORTEST_PATH_HPP */
|
||||
Reference in New Issue
Block a user