diff --git a/3party/osrm/osrm-backend/RoutingAlgorithms/NToMManyToManyRouting.h b/3party/osrm/osrm-backend/RoutingAlgorithms/NToMManyToManyRouting.h new file mode 100644 index 0000000000..76363866c4 --- /dev/null +++ b/3party/osrm/osrm-backend/RoutingAlgorithms/NToMManyToManyRouting.h @@ -0,0 +1,267 @@ +/* + +Copyright (c) 2014, Project OSRM, Dennis Luxen, others +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 NMMANY_TO_MANY_ROUTING_H +#define NMMANY_TO_MANY_ROUTING_H + +#include "BasicRoutingInterface.h" +#include "../DataStructures/SearchEngineData.h" +#include "../typedefs.h" + +#include + +#include +#include +#include +#include + +template class NMManyToManyRouting final : public BasicRoutingInterface +{ + using super = BasicRoutingInterface; + 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>; + + public: + NMManyToManyRouting(DataFacadeT *facade, SearchEngineData &engine_working_data) + : super(facade), engine_working_data(engine_working_data) + { + } + + ~NMManyToManyRouting() {} + + std::shared_ptr> operator()(const PhantomNodeArray &phantom_sources_nodes_array, const PhantomNodeArray &phantom_targets_nodes_array) + const + { + const unsigned number_of_sources = static_cast(phantom_sources_nodes_array.size()); + const unsigned number_of_targets = static_cast(phantom_targets_nodes_array.size()); + std::shared_ptr> result_table = + std::make_shared>(number_of_sources * number_of_targets, + std::numeric_limits::max()); + + engine_working_data.InitializeOrClearFirstThreadLocalStorage( + super::facade->GetNumberOfNodes()); + + QueryHeap &query_heap = *(engine_working_data.forwardHeap); + + SearchSpaceWithBuckets search_space_with_buckets; + + unsigned target_id = 0; + for (const std::vector &phantom_node_vector : phantom_targets_nodes_array) + { + query_heap.Clear(); + // insert target(s) at distance 0 + + for (const PhantomNode &phantom_node : phantom_node_vector) + { + if (SPECIAL_NODEID != phantom_node.forward_node_id) + { + query_heap.Insert(phantom_node.forward_node_id, + phantom_node.GetForwardWeightPlusOffset(), + phantom_node.forward_node_id); + } + if (SPECIAL_NODEID != phantom_node.reverse_node_id) + { + query_heap.Insert(phantom_node.reverse_node_id, + phantom_node.GetReverseWeightPlusOffset(), + phantom_node.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 std::vector &phantom_node_vector : phantom_sources_nodes_array) + { + query_heap.Clear(); + for (const PhantomNode &phantom_node : phantom_node_vector) + { + // insert sources at distance 0 + if (SPECIAL_NODEID != phantom_node.forward_node_id) + { + query_heap.Insert(phantom_node.forward_node_id, + -phantom_node.GetForwardWeightPlusOffset(), + phantom_node.forward_node_id); + } + if (SPECIAL_NODEID != phantom_node.reverse_node_id) + { + query_heap.Insert(phantom_node.reverse_node_id, + -phantom_node.GetReverseWeightPlusOffset(), + phantom_node.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_locations, + QueryHeap &query_heap, + const SearchSpaceWithBuckets &search_space_with_buckets, + std::shared_ptr> 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 &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_locations + 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_locations + target_id] = + (source_distance + target_distance); + } + } + } + if (StallAtNode(node, source_distance, query_heap)) + { + return; + } + RelaxOutgoingEdges(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(node, target_distance, query_heap)) + { + return; + } + + RelaxOutgoingEdges(node, target_distance, query_heap); + } + + template + 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, node); + 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 + 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, node); + 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 diff --git a/routing/osrm_router.cpp b/routing/osrm_router.cpp index d96ad9a025..a3db9e9525 100644 --- a/routing/osrm_router.cpp +++ b/routing/osrm_router.cpp @@ -20,11 +20,12 @@ #include "../coding/reader_wrapper.hpp" #include "../base/logging.hpp" +#include "../base/timer.hpp" #include "../3party/osrm/osrm-backend/DataStructures/SearchEngineData.h" #include "../3party/osrm/osrm-backend/Descriptors/DescriptionFactory.h" #include "../3party/osrm/osrm-backend/RoutingAlgorithms/ShortestPathRouting.h" -#include "../3party/osrm/osrm-backend/RoutingAlgorithms/ManyToManyRouting.h" +#include "../3party/osrm/osrm-backend/RoutingAlgorithms/NToMManyToManyRouting.h" namespace routing @@ -514,48 +515,36 @@ bool IsRouteExist(RawRouteData const & r) } -MultiroutingTaskPointT::const_iterator OsrmRouter::FilterWeightsMatrix(MultiroutingTaskPointT const & sources, MultiroutingTaskPointT const & targets, - std::vector &weightMatrix) -{ - for (size_t i = 0; i < sources.size(); ++i) - for (size_t j = 0; j < targets.size(); ++j) - if (weightMatrix[i*sources.size() + j] < INVALID_EDGE_WEIGHT) - { - weightMatrix.erase(weightMatrix.begin(), weightMatrix.begin() + i*targets.size()); - weightMatrix.erase(weightMatrix.begin() + targets.size(), weightMatrix.end()); - ASSERT(weightMatrix.size() == targets.size(), ()); - return sources.cbegin() + i; - } - - return sources.cend(); -} - void OsrmRouter::FindWeightsMatrix(MultiroutingTaskPointT const & sources, MultiroutingTaskPointT const & targets, RawDataFacadeT &facade, std::vector &result) { SearchEngineData engineData; - ManyToManyRouting pathFinder(&facade, engineData); + NMManyToManyRouting pathFinder(&facade, engineData); - size_t const row_size = sources.size() + targets.size(); - size_t const result_row_size = targets.size(); - PhantomNodeArray taskVector(row_size); + PhantomNodeArray sourcesTaskVector(sources.size()); + PhantomNodeArray targetsTaskVector(targets.size()); for (int i = 0; i < sources.size(); ++i) { - taskVector[i].push_back(sources[i].m_node); + sourcesTaskVector[i].push_back(sources[i].m_node); } for (int i = 0; i < targets.size(); ++i) { - taskVector[sources.size() + i].push_back(targets[i].m_node); + targetsTaskVector[i].push_back(targets[i].m_node); } - std::shared_ptr> resultTable = pathFinder(taskVector); + my::HighResTimer timer1(true); + std::shared_ptr> resultTable = pathFinder(sourcesTaskVector, targetsTaskVector); + LOG(LINFO, ("Pure one-to many", timer1.ElapsedNano())); + timer1.Reset(); - result.resize(sources.size() * result_row_size); + /*result.resize(sources.size() * result_row_size); auto const start_iter = resultTable->begin()+sources.size(); for (int i = 0; i < sources.size(); ++i) { std::copy_n(start_iter + i*row_size, result_row_size, result.begin() + result_row_size*i); - } + }*/ + ASSERT_EQUAL(resultTable->size(), sources.size() * targets.size(), ()); + result.swap(*resultTable); } bool OsrmRouter::FindSingleRoute(FeatureGraphNodeVecT const & source, FeatureGraphNodeVecT const & target, DataFacadeT &facade, @@ -758,31 +747,28 @@ public: LastCrossFinder(RoutingMappingPtrT & mapping, FeatureGraphNodeVecT const & targetTask):m_targetContext(mapping->m_dataFacade.getRoutingContext()), m_mwmName(mapping->GetName()) { auto income_iterators = m_targetContext.GetIngoingIterators(); - MultiroutingTaskPointT targets; + MultiroutingTaskPointT targets(1); m_sources.resize(distance(income_iterators.first, income_iterators.second)); for (auto i = income_iterators.first; i < income_iterators.second; ++i) OsrmRouter::GenerateRoutingTaskFromNodeId(*i, m_sources[distance(income_iterators.first, i)]); - for (auto t : targetTask) - targets.push_back(t); vector weights; - OsrmRouter::FindWeightsMatrix(m_sources, targets, mapping->m_dataFacade, weights); - - for (size_t i = 0; i < targets.size(); ++i) - for (size_t j = 0; j < m_sources.size(); ++j) - if (weights[j * m_sources.size() + i] < INVALID_EDGE_WEIGHT) + for (auto t : targetTask) + { + targets[0] = t; + OsrmRouter::FindWeightsMatrix(m_sources, targets, mapping->m_dataFacade, weights); + for (auto weight : weights) + { + if (weight < INVALID_EDGE_WEIGHT) { - for (int k = m_sources.size() - 1; k > -1; --k) - { - weights.erase(weights.begin() + k*targets.size(), weights.begin() + k*targets.size() + i); - weights.erase(weights.begin() + k*targets.size() + i + 1, weights.begin() + (k + 1) * targets.size()); - } - ASSERT(weights.size() == m_sources.size(), ()); - m_target = targets[i]; + ASSERT_EQUAL(weights.size(), m_sources.size(), ()); + m_target = t; m_weights.swap(weights); return; } + } + } throw OsrmRouter::EndPointNotFound; } @@ -810,6 +796,7 @@ public: OsrmRouter::ResultCode OsrmRouter::CalculateRouteImpl(m2::PointD const & startPt, m2::PointD const & startDr, m2::PointD const & finalPt, Route & route) { + my::HighResTimer timer(true); RoutingMappingPtrT startMapping = m_indexManager.GetMappingByPoint(startPt, m_pIndex); RoutingMappingPtrT targetMapping = m_indexManager.GetMappingByPoint(finalPt, m_pIndex); @@ -822,6 +809,8 @@ OsrmRouter::ResultCode OsrmRouter::CalculateRouteImpl(m2::PointD const & startPt MappingGuard finalMappingGuard(targetMapping); UNUSED_VALUE(startMappingGuard); UNUSED_VALUE(finalMappingGuard); + LOG(LINFO, ("Loading first MWM", timer.ElapsedNano())); + timer.Reset(); // 3. Find start/end nodes. FeatureGraphNodeVecT startTask; @@ -843,6 +832,9 @@ OsrmRouter::ResultCode OsrmRouter::CalculateRouteImpl(m2::PointD const & startPt if (m_requestCancel) return Cancelled; + LOG(LINFO, ("Find start/stop points", timer.ElapsedNano())); + timer.Reset(); + // 4. Find route. RawRoutingResultT routingResult; @@ -878,24 +870,38 @@ OsrmRouter::ResultCode OsrmRouter::CalculateRouteImpl(m2::PointD const & startPt else //4.2 Different mwm case { LOG(LINFO, ("Different mwm routing case")); - if (!m_additionalFeatures) - return PointsInDifferentMWM; + //if (!m_additionalFeatures) + // return PointsInDifferentMWM; // Load source data CrossRoutingContext const & startContext = startMapping->m_dataFacade.getRoutingContext(); auto out_iterators = startContext.GetOutgoingIterators(); - MultiroutingTaskPointT sources, targets(distance(out_iterators.first, out_iterators.second)); + MultiroutingTaskPointT sources(1), targets(distance(out_iterators.first, out_iterators.second)); for (auto i = out_iterators.first; i < out_iterators.second; ++i) OsrmRouter::GenerateRoutingTaskFromNodeId(i->first, targets[distance(out_iterators.first, i)]); - for (auto t : startTask) - sources.push_back(t); vector weights; - FindWeightsMatrix(sources, targets, startMapping->m_dataFacade, weights); + for (auto t : startTask) + { + sources[0] = t; + bool found = false; + LOG(LINFO, ("Start case")); + FindWeightsMatrix(sources, targets, startMapping->m_dataFacade, weights); + for (auto weight : weights) + { + if (weight < INVALID_EDGE_WEIGHT) + { + found = true; + break; + } + } + if (found) + break; + else + weights.clear(); + } - auto sourceNodeIter = FilterWeightsMatrix(sources, targets,weights); - - if (sourceNodeIter == sources.cend()) + if (!weights.size()) return StartPointNotFound; if (m_requestCancel) @@ -910,6 +916,9 @@ OsrmRouter::ResultCode OsrmRouter::CalculateRouteImpl(m2::PointD const & startPt RoutingTaskQueueT crossTasks; CheckedOutsT checkedOuts; + LOG(LINFO, ("Make one-to-many crosses", timer.ElapsedNano())); + timer.Reset(); + //Submit tasks from source for (size_t j = 0; j < targets.size(); ++j) { @@ -931,7 +940,7 @@ OsrmRouter::ResultCode OsrmRouter::CalculateRouteImpl(m2::PointD const & startPt continue; checkedOuts.insert(make_pair(tNodeId, startMapping->GetName())); CheckedPathT tmpPath; - tmpPath.push_back({startMapping->GetName(), *sourceNodeIter, targets[j], weights[j]}); + tmpPath.push_back({startMapping->GetName(), sources[0], targets[j], weights[j]}); if (nextMwm != targetMapping->GetName()) { @@ -1043,9 +1052,17 @@ OsrmRouter::ResultCode OsrmRouter::CalculateRouteImpl(m2::PointD const & startPt } } + LOG(LINFO, ("Finding cross path", timer.ElapsedNano())); + timer.Reset(); + // 5. Make generate answer if (finalWeight < INVALID_EDGE_WEIGHT) - return MakeRouteFromCrossesPath(finalPath, route); + { + auto code = MakeRouteFromCrossesPath(finalPath, route); + LOG(LINFO, ("Make final route", timer.ElapsedNano())); + timer.Reset(); + return code; + } else return OsrmRouter::RouteNotFound; } diff --git a/routing/osrm_router.hpp b/routing/osrm_router.hpp index 4a4d9ad185..123c889c27 100644 --- a/routing/osrm_router.hpp +++ b/routing/osrm_router.hpp @@ -292,9 +292,6 @@ private: typedef priority_queue, pathChecker> RoutingTaskQueueT; - MultiroutingTaskPointT::const_iterator FilterWeightsMatrix(MultiroutingTaskPointT const & sources, MultiroutingTaskPointT const & targets, - std::vector & weightMatrix); - /*! * \brief Makes route (points turns and other annotations) and submits it to @route class * \param path vector of pathes through mwms