diff --git a/pedestrian_routing_benchmarks/pedestrian_routing_benchmarks.cpp b/pedestrian_routing_benchmarks/pedestrian_routing_benchmarks.cpp index a3e49edd83..ef98e8d498 100644 --- a/pedestrian_routing_benchmarks/pedestrian_routing_benchmarks.cpp +++ b/pedestrian_routing_benchmarks/pedestrian_routing_benchmarks.cpp @@ -5,6 +5,7 @@ #include "routing/astar_router.hpp" #include "routing/features_road_graph.hpp" +#include "routing/route.hpp" #include "base/logging.hpp" #include "base/macros.hpp" @@ -14,6 +15,12 @@ #include "std/utility.hpp" #include "std/vector.hpp" +namespace +{ + +string const MAP_NAME = "UK_England"; +string const MAP_FILE = MAP_NAME + DATA_FILE_EXTENSION; + pair GetPointsAroundSeg(Index & index, MwmSet::MwmId id, uint32_t featureId, uint32_t segId) { @@ -26,44 +33,62 @@ pair GetPointsAroundSeg(Index & index, MwmSet::MwmId id, return make_pair(ft.GetPoint(segId), ft.GetPoint(segId + 1)); } +void TestTwoPoints(routing::IRouter & router, m2::PointD const & startPos, m2::PointD const & finalPos) +{ + LOG(LINFO, ("Calculating routing...")); + routing::Route route(""); + my::Timer timer; + routing::IRouter::ResultCode const resultCode = router.CalculateRoute(startPos, m2::PointD::Zero() /* startDirection */, + finalPos, route); + double const elapsedSec = timer.ElapsedSeconds(); + TEST_EQUAL(routing::IRouter::NoError, resultCode, ()); + LOG(LINFO, ("Route polyline size:", route.GetPoly().GetSize())); + LOG(LINFO, ("Route distance, meters:", route.GetDistance())); + LOG(LINFO, ("Elapsed, seconds:", elapsedSec)); +} + +void TestTwoPoints(Index & index, m2::PointD const & startPos, m2::PointD const & finalPos) +{ + routing::AStarRouter router([](m2::PointD const & /* point */) { return MAP_FILE; }, + &index); + TestTwoPoints(router, startPos, finalPos); +} + void TestTwoPoints(uint32_t featureIdStart, uint32_t segIdStart, uint32_t featureIdFinal, uint32_t segIdFinal) { - string const kMapName = "UK_England"; classificator::Load(); - Index index; - routing::AStarRouter router([&kMapName](m2::PointD const & /*point*/) - { - return kMapName; - }, - &index); - UNUSED_VALUE(index.RegisterMap(kMapName + DATA_FILE_EXTENSION)); - TEST(index.IsLoaded(kMapName), ()); - MwmSet::MwmId id = index.GetMwmIdByFileName(kMapName + DATA_FILE_EXTENSION); + Index index; + UNUSED_VALUE(index.RegisterMap(MAP_FILE)); + TEST(index.IsLoaded(MAP_NAME), ()); + MwmSet::MwmId const id = index.GetMwmIdByFileName(MAP_FILE); TEST(id.IsAlive(), ()); - router.SetRoadGraph(make_unique(&index, id)); + pair const startBounds = GetPointsAroundSeg(index, id, featureIdStart, segIdStart); + pair const finalBounds = GetPointsAroundSeg(index, id, featureIdFinal, segIdFinal); - pair startBounds = - GetPointsAroundSeg(index, id, featureIdStart, segIdStart); + m2::PointD const startPos = startBounds.first; + m2::PointD const finalPos = finalBounds.first; - routing::RoadPos startPos(featureIdStart, true, segIdStart, startBounds.first); - - pair finalBounds = - GetPointsAroundSeg(index, id, featureIdFinal, segIdFinal); - routing::RoadPos finalPos(featureIdFinal, true, segIdFinal, finalBounds.first); - - vector route; - - my::Timer timer; - LOG(LINFO, ("Calculating routing...")); - routing::IRouter::ResultCode resultCode = router.CalculateRoute(startPos, finalPos, route); - TEST_EQUAL(routing::IRouter::NoError, resultCode, ()); - LOG(LINFO, ("Route length:", route.size())); - LOG(LINFO, ("Elapsed:", timer.ElapsedSeconds(), "seconds")); + TestTwoPoints(index, startPos, finalPos); } +void TestTwoPoints(m2::PointD const & startPos, m2::PointD const & finalPos) +{ + classificator::Load(); + + Index index; + UNUSED_VALUE(index.RegisterMap(MAP_FILE)); + TEST(index.IsLoaded(MAP_NAME), ()); + + TestTwoPoints(index, startPos, finalPos); +} + +} // namespace + +// Tests on features + UNIT_TEST(PedestrianRouting_UK_Long1) { TestTwoPoints(59231052, 8, 49334376, 0); } UNIT_TEST(PedestrianRouting_UK_Long2) { TestTwoPoints(2909201, 1, 86420951, 1); } @@ -91,3 +116,191 @@ UNIT_TEST(PedestrianRouting_UK_Short1) { TestTwoPoints(3038057, 0, 3032688, 3); UNIT_TEST(PedestrianRouting_UK_Short2) { TestTwoPoints(2947484, 2, 44889742, 0); } UNIT_TEST(PedestrianRouting_UK_Short3) { TestTwoPoints(2931545, 0, 2969395, 0); } + +// Tests on points + +UNIT_TEST(PedestrianRouting_UK_Test1) +{ + TestTwoPoints(m2::PointD(-0.23371357519479166176, 60.188217037454911917), + m2::PointD(-0.27958780433409546884, 60.251555957343796877)); +} + +UNIT_TEST(PedestrianRouting_UK_Test2) +{ + TestTwoPoints(m2::PointD(-0.23204233496629894651, 60.220733702351964212), + m2::PointD(-0.25325265780566397211, 60.343129850040341466)); +} + +UNIT_TEST(PedestrianRouting_UK_Test3) +{ + TestTwoPoints(m2::PointD(-0.13493810466972872009, 60.213290963151536062), + m2::PointD(-0.075021485248326899575, 60.386990007024301974)); +} + +UNIT_TEST(PedestrianRouting_UK_Test4) +{ + TestTwoPoints(m2::PointD(0.073624712333011516074, 60.249651023717902376), + m2::PointD(0.062623007653576381881, 60.305363026945343563)); +} + +UNIT_TEST(PedestrianRouting_UK_Test5) +{ + TestTwoPoints(m2::PointD(0.073624712333011516074, 60.249651023717902376), + m2::PointD(0.062623007653576381881, 60.305363026945343563)); +} + +UNIT_TEST(PedestrianRouting_UK_Test6) +{ + TestTwoPoints(m2::PointD(0.12973003099584515252, 60.286986176872751741), + m2::PointD(0.16166505598152342005, 60.329896866615413842)); +} + +UNIT_TEST(PedestrianRouting_UK_Test7) +{ + TestTwoPoints(m2::PointD(0.24339008846246840134, 60.221936300171577727), + m2::PointD(0.30297476080828561473, 60.472352430858123284)); +} + +UNIT_TEST(PedestrianRouting_UK_Test8) +{ + TestTwoPoints(m2::PointD(-0.090078418419228770131, 59.938877740935481597), + m2::PointD(-0.36591832729336593033, 60.383060825937320715)); +} + +UNIT_TEST(PedestrianRouting_UK_Test9) +{ + TestTwoPoints(m2::PointD(0.013909241828589231221, 60.248524891536746395), + m2::PointD(-0.011025824403098606619, 60.293190853881299063)); +} + +UNIT_TEST(PedestrianRouting_UK_Test10) +{ + TestTwoPoints(m2::PointD(-1.2608451895615837568, 60.688400774771103841), + m2::PointD(-1.3402756985196870865, 60.378654240852370094)); +} + +UNIT_TEST(PedestrianRouting_UK_Test11) +{ + TestTwoPoints(m2::PointD(-1.2608451895615837568, 60.688400774771103841), + m2::PointD(-1.3402756985196870865, 60.378654240852370094)); +} + +UNIT_TEST(PedestrianRouting_UK_Test12) +{ + TestTwoPoints(m2::PointD(-0.41581758334591578663, 60.055074253917027249), + m2::PointD(-0.0049981648490620023823, 60.559216972985538519)); +} + +UNIT_TEST(PedestrianRouting_UK_Test13) +{ + TestTwoPoints(m2::PointD(-0.0084726671967171318656, 60.175004410845247094), + m2::PointD(-0.38290269805087756572, 60.484353263782054455)); +} + +UNIT_TEST(PedestrianRouting_UK_Test14) +{ + TestTwoPoints(m2::PointD(-0.49920524882713435133, 60.500939921180034275), + m2::PointD(-0.4253928261485126483, 60.460210120242891207)); +} + +UNIT_TEST(PedestrianRouting_UK_Test15) +{ + TestTwoPoints(m2::PointD(-0.35293312317744285345, 60.38324360888867659), + m2::PointD(-0.27232356277650499043, 60.485946731323195991)); +} + +UNIT_TEST(PedestrianRouting_UK_Test16) +{ + TestTwoPoints(m2::PointD(-0.2452144979288601867, 60.417717669797063706), + m2::PointD(0.052673435877072988243, 60.48102832828819686)); +} + +UNIT_TEST(PedestrianRouting_UK_Test17) +{ + TestTwoPoints(m2::PointD(0.60492089858687592141, 60.365652323287299907), + m2::PointD(0.59411588825676053816, 60.315295907170423106)); +} + +UNIT_TEST(PedestrianRouting_UK_Test18) +{ + TestTwoPoints(m2::PointD(0.57712031437541466694, 60.311563537302561144), + m2::PointD(-1.0991154539409491164, 59.24340383025300838)); +} + +UNIT_TEST(PedestrianRouting_UK_Test19) +{ + TestTwoPoints(m2::PointD(-0.42410951183471667925, 60.225100494175073607), + m2::PointD(-0.4417841749541066565, 60.377963804987665242)); +} + +UNIT_TEST(PedestrianRouting_UK_Test20) +{ + TestTwoPoints(m2::PointD(0.087765601941695303712, 60.054331215788280929), + m2::PointD(0.19336133919110590207, 60.383987006527995334)); +} + +UNIT_TEST(PedestrianRouting_UK_Test21) +{ + TestTwoPoints(m2::PointD(0.23038165654281794748, 60.438464644310201379), + m2::PointD(0.18335075596080072091, 60.466925517864886785)); +} + +UNIT_TEST(PedestrianRouting_UK_Test22) +{ + TestTwoPoints(m2::PointD(-0.33907208409976324903, 60.691735528482595896), + m2::PointD(-0.17824228031321673327, 60.478512208248780269)); +} + +UNIT_TEST(PedestrianRouting_UK_Test23) +{ + TestTwoPoints(m2::PointD(-0.0255750943822493082, 60.413717422909641641), + m2::PointD(0.059727476276875829386, 60.314137951796560344)); +} + +UNIT_TEST(PedestrianRouting_UK_Test24) +{ + TestTwoPoints(m2::PointD(-0.1251022759281295027, 60.238139790774681614), + m2::PointD(-0.27656544081449146999, 60.05896703409919013)); +} + +UNIT_TEST(PedestrianRouting_UK_Test25) +{ + TestTwoPoints(m2::PointD(-0.1251022759281295027, 60.238139790774681614), + m2::PointD(-0.27656544081449146999, 60.05896703409919013)); +} + +UNIT_TEST(PedestrianRouting_UK_Test26) +{ + TestTwoPoints(m2::PointD(-3.0453848423988452154, 63.444289157178360483), + m2::PointD(-2.9888705955481791321, 63.475820316921343078)); +} + +UNIT_TEST(PedestrianRouting_UK_Test27) +{ + TestTwoPoints(m2::PointD(-2.9465302867103040363, 63.61187786025546842), + m2::PointD(-2.8321504085699609199, 63.515257402251123153)); +} + +UNIT_TEST(PedestrianRouting_UK_Test28) +{ + TestTwoPoints(m2::PointD(-2.8527592513387749484, 63.424788250610269813), + m2::PointD(-2.8824557029010167142, 63.389320899559180589)); +} + +UNIT_TEST(PedestrianRouting_UK_Test29) +{ + TestTwoPoints(m2::PointD(-2.3526647292757063568, 63.599798938870364395), + m2::PointD(-2.29857574370878881, 63.546779173754892156)); +} + +UNIT_TEST(PedestrianRouting_UK_Test30) +{ + TestTwoPoints(m2::PointD(-2.2204371931102926396, 63.410664672405502529), + m2::PointD(-2.2961918002218593138, 63.653059523966334154)); +} + +UNIT_TEST(PedestrianRouting_UK_Test31) +{ + TestTwoPoints(m2::PointD(-2.2807869696804585757, 63.6673587499283542), + m2::PointD(-2.2537861498623481538, 63.627449392852028609)); +} diff --git a/routing/astar_router.cpp b/routing/astar_router.cpp index 83b9d588e3..cc271f8443 100644 --- a/routing/astar_router.cpp +++ b/routing/astar_router.cpp @@ -6,32 +6,27 @@ namespace routing { -AStarRouter::AStarRouter(TMwmFileByPointFn const & fn, Index const * pIndex, - RoutingVisualizerFn routingVisualizer) - : RoadGraphRouter(pIndex, unique_ptr(new PedestrianModel()), fn), - m_routingVisualizer(routingVisualizer) + +AStarRouter::AStarRouter(TMwmFileByPointFn const & fn, Index const * pIndex, RoutingVisualizerFn routingVisualizer) + : RoadGraphRouter(pIndex, unique_ptr(new PedestrianModel()), fn) + , m_routingVisualizer(routingVisualizer) { } -IRouter::ResultCode AStarRouter::CalculateRoute(RoadPos const & startPos, RoadPos const & finalPos, - vector & route) +IRouter::ResultCode AStarRouter::CalculateRoute(Junction const & startPos, Junction const & finalPos, + vector & route) { - RoadGraph graph(*m_roadGraph); - m_algo.SetGraph(graph); + RoadGraph const roadGraph(*GetGraph()); + m_algo.SetGraph(roadGraph); TAlgorithm::OnVisitedVertexCallback onVisitedVertexCallback = nullptr; if (nullptr != m_routingVisualizer) - onVisitedVertexCallback = [this](RoadPos const & roadPos) { m_routingVisualizer(roadPos.GetSegEndpoint()); }; + onVisitedVertexCallback = [this](Junction const & junction) { m_routingVisualizer(junction.GetPoint()); }; TAlgorithm::Result const result = m_algo.FindPathBidirectional(startPos, finalPos, route, onVisitedVertexCallback); switch (result) { case TAlgorithm::Result::OK: - // Following hack is used because operator== checks for - // equivalience, not identity, since it doesn't test - // RoadPos::m_segEndpoint. Thus, start and final positions - // returned by algorithm should be replaced by correct start and - // final positions. ASSERT_EQUAL(route.front(), startPos, ()); ASSERT_EQUAL(route.back(), finalPos, ()); return IRouter::NoError; @@ -42,4 +37,5 @@ IRouter::ResultCode AStarRouter::CalculateRoute(RoadPos const & startPos, RoadPo } return IRouter::RouteNotFound; } + } // namespace routing diff --git a/routing/astar_router.hpp b/routing/astar_router.hpp index 58b28b1b6a..831bb86c98 100644 --- a/routing/astar_router.hpp +++ b/routing/astar_router.hpp @@ -7,6 +7,7 @@ namespace routing { + class AStarRouter : public RoadGraphRouter { public: @@ -18,8 +19,8 @@ public: void ClearState() override { Reset(); } // RoadGraphRouter overrides: - ResultCode CalculateRoute(RoadPos const & startPos, RoadPos const & finalPos, - vector & route) override; + ResultCode CalculateRoute(Junction const & startPos, Junction const & finalPos, + vector & route) override; // my::Cancellable overrides: void Reset() override { m_algo.Reset(); } @@ -29,6 +30,7 @@ public: private: using TAlgorithm = AStarAlgorithm; TAlgorithm m_algo; - RoutingVisualizerFn m_routingVisualizer; + RoutingVisualizerFn const m_routingVisualizer; }; + } // namespace routing diff --git a/routing/async_router.cpp b/routing/async_router.cpp index a5cc034461..7f3f037dff 100644 --- a/routing/async_router.cpp +++ b/routing/async_router.cpp @@ -6,6 +6,7 @@ namespace routing { + AsyncRouter::AsyncRouter(unique_ptr && router) : m_router(move(router)) { m_isReadyThread.clear(); @@ -97,4 +98,5 @@ void AsyncRouter::CalculateRouteImpl(ReadyCallback const & callback) GetPlatform().RunOnGuiThread(bind(callback, route, code)); } -} + +} // namespace routing diff --git a/routing/async_router.hpp b/routing/async_router.hpp index d62251f95f..6607621f9b 100644 --- a/routing/async_router.hpp +++ b/routing/async_router.hpp @@ -9,6 +9,7 @@ namespace routing { + /// Callback takes ownership of passed route. typedef function ReadyCallback; @@ -47,6 +48,7 @@ private: m2::PointD m_finalPoint; m2::PointD m_startDirection; - unique_ptr m_router; + unique_ptr const m_router; }; -} + +} // namespace routing diff --git a/routing/base/astar_algorithm.hpp b/routing/base/astar_algorithm.hpp index 26922fd5be..fd28a5ebb3 100644 --- a/routing/base/astar_algorithm.hpp +++ b/routing/base/astar_algorithm.hpp @@ -13,6 +13,7 @@ namespace routing { + template class AStarAlgorithm : public my::Cancellable { @@ -119,6 +120,14 @@ private: } } + void GetAdjacencyList(TVertexType const & v, vector & adj) + { + if (forward) + graph.GetOutgoingEdgesList(v, adj); + else + graph.GetIngoingEdgesList(v, adj); + } + bool const forward; TVertexType const & startVertex; TVertexType const & finalVertex; @@ -188,8 +197,9 @@ typename AStarAlgorithm::Result AStarAlgorithm::FindPath( bestDistance[finalVertex] = 0.0; queue.push(State(finalVertex, 0.0)); - uint32_t steps = 0; + vector adj; + uint32_t steps = 0; while (!queue.empty()) { ++steps; @@ -212,8 +222,7 @@ typename AStarAlgorithm::Result AStarAlgorithm::FindPath( return Result::OK; } - vector adj; - m_graph->GetAdjacencyList(stateV.vertex, adj); + m_graph->GetIngoingEdgesList(stateV.vertex, adj); for (auto const & edge : adj) { State stateW(edge.GetTarget(), 0.0); @@ -272,6 +281,8 @@ typename AStarAlgorithm::Result AStarAlgorithm::FindPathBidirect BidirectionalStepContext * cur = &forward; BidirectionalStepContext * nxt = &backward; + vector adj; + // It is not necessary to check emptiness for both queues here // because if we have not found a path by the time one of the // queues is exhausted, we never will. @@ -315,8 +326,7 @@ typename AStarAlgorithm::Result AStarAlgorithm::FindPathBidirect if (steps % kVisitedVerticesPeriod == 0) onVisitedVertexCallback(stateV.vertex); - vector adj; - m_graph->GetAdjacencyList(stateV.vertex, adj); + cur->GetAdjacencyList(stateV.vertex, adj); for (auto const & edge : adj) { State stateW(edge.GetTarget(), 0.0); @@ -331,13 +341,14 @@ typename AStarAlgorithm::Result AStarAlgorithm::FindPathBidirect CHECK(reducedLen >= -kEpsilon, ("Invariant violated:", reducedLen, "<", -kEpsilon)); double const newReducedDist = stateV.distance + max(reducedLen, 0.0); - auto const it = cur->bestDistance.find(stateW.vertex); - if (it != cur->bestDistance.end() && newReducedDist >= it->second - kEpsilon) + auto const itCur = cur->bestDistance.find(stateW.vertex); + if (itCur != cur->bestDistance.end() && newReducedDist >= itCur->second - kEpsilon) continue; - if (nxt->bestDistance.find(stateW.vertex) != nxt->bestDistance.end()) + auto const itNxt = nxt->bestDistance.find(stateW.vertex); + if (itNxt != nxt->bestDistance.end()) { - double const distW = nxt->bestDistance[stateW.vertex]; + double const distW = itNxt->second; // Length that the path we've just found has in the original graph: // find the length of the path's parts in the reduced forward and backward // graphs and remove the heuristic adjustments. @@ -390,7 +401,10 @@ void AStarAlgorithm::ReconstructPathBidirectional( ReconstructPath(v, parentV, pathV); vector pathW; ReconstructPath(w, parentW, pathW); + path.clear(); + path.reserve(pathV.size() + pathW.size()); path.insert(path.end(), pathV.rbegin(), pathV.rend()); path.insert(path.end(), pathW.begin(), pathW.end()); } + } // namespace routing diff --git a/routing/base/graph.hpp b/routing/base/graph.hpp index 666f332f78..af97fa4d9f 100644 --- a/routing/base/graph.hpp +++ b/routing/base/graph.hpp @@ -5,6 +5,7 @@ namespace routing { + template class Graph { @@ -12,16 +13,14 @@ public: using TVertexType = TVertex; using TEdgeType = TEdge; - /// TODO (@gorshenin, @pimenov, @ldragunov): for bidirectional - /// algorithms this method should be replaced by two: - /// GetOutgoingEdges() and GetIngoingEdges(). They should be - /// identical for undirected graphs, but may differ on directed. The - /// reason is that forward pass of routing algorithms should use - /// only GetOutgoingEdges() while backward pass should use only - /// GetIngoingEdges(). - void GetAdjacencyList(TVertexType const & v, vector & adj) const + void GetOutgoingEdgesList(TVertexType const & v, vector & adj) const { - return GetImpl().GetAdjacencyListImpl(v, adj); + return GetImpl().GetOutgoingEdgesListImpl(v, adj); + } + + void GetIngoingEdgesList(TVertexType const & v, vector & adj) const + { + return GetImpl().GetIngoingEdgesListImpl(v, adj); } double HeuristicCostEstimate(TVertexType const & v, TVertexType const & w) const @@ -31,7 +30,7 @@ public: private: TImpl & GetImpl() { return static_cast(*this); } - TImpl const & GetImpl() const { return static_cast(*this); } }; + } // namespace routing diff --git a/routing/features_road_graph.cpp b/routing/features_road_graph.cpp index 649ae677f1..43743ce7b0 100644 --- a/routing/features_road_graph.cpp +++ b/routing/features_road_graph.cpp @@ -13,17 +13,17 @@ namespace routing { + namespace { uint32_t const FEATURE_CACHE_SIZE = 10; double const READ_CROSS_EPSILON = 1.0E-4; -} // namespace +} // namespace -/// @todo Factor out vehicle model as a parameter for the features graph. -FeaturesRoadGraph::FeaturesRoadGraph(Index const * pIndex, MwmSet::MwmId const & mwmID) +FeaturesRoadGraph::FeaturesRoadGraph(IVehicleModel const * vehicleModel, Index const * pIndex, MwmSet::MwmId const & mwmID) : m_pIndex(pIndex), m_mwmID(mwmID), - m_vehicleModel(new PedestrianModel()), + m_vehicleModel(vehicleModel), m_cache(FEATURE_CACHE_SIZE), m_cacheMiss(0), m_cacheAccess(0) @@ -35,11 +35,10 @@ uint32_t FeaturesRoadGraph::GetStreetReadScale() { return scales::GetUpperScale( class CrossFeaturesLoader { public: - CrossFeaturesLoader(FeaturesRoadGraph & graph, m2::PointD const & point, - IRoadGraph::CrossTurnsLoader & turnsLoader) - : m_graph(graph), m_point(point), m_turnsLoader(turnsLoader) - { - } + CrossFeaturesLoader(FeaturesRoadGraph & graph, + IRoadGraph::CrossEdgesLoader & edgesLoader) + : m_graph(graph), m_edgesLoader(edgesLoader) + {} void operator()(FeatureType & ft) { @@ -62,13 +61,12 @@ public: IRoadGraph::RoadInfo const & ri = m_graph.GetCachedRoadInfo(fID.m_offset, ft, false); ASSERT_EQUAL(speed, ri.m_speedKMPH, ()); - m_turnsLoader(fID.m_offset, ri); + m_edgesLoader(fID.m_offset, ri); } private: FeaturesRoadGraph & m_graph; - m2::PointD m_point; - IRoadGraph::CrossTurnsLoader & m_turnsLoader; + IRoadGraph::CrossEdgesLoader & m_edgesLoader; }; void FeaturesRoadGraph::LoadFeature(uint32_t featureId, FeatureType & ft) @@ -90,14 +88,18 @@ IRoadGraph::RoadInfo FeaturesRoadGraph::GetRoadInfo(uint32_t featureId) double FeaturesRoadGraph::GetSpeedKMPH(uint32_t featureId) { FeatureType ft; - LoadFeature(featureId, ft); - return GetSpeedKMPHFromFt(ft); + return GetCachedRoadInfo(featureId, ft, true).m_speedKMPH; +} + +double FeaturesRoadGraph::GetMaxSpeedKMPH() +{ + return m_vehicleModel->GetMaxSpeed(); } void FeaturesRoadGraph::ForEachFeatureClosestToCross(m2::PointD const & cross, - CrossTurnsLoader & turnsLoader) + CrossEdgesLoader & edgesLoader) { - CrossFeaturesLoader featuresLoader(*this, cross, turnsLoader); + CrossFeaturesLoader featuresLoader(*this, edgesLoader); m_pIndex->ForEachInRect(featuresLoader, m2::RectD(cross.x - READ_CROSS_EPSILON, cross.y - READ_CROSS_EPSILON, cross.x + READ_CROSS_EPSILON, cross.y + READ_CROSS_EPSILON), @@ -136,4 +138,5 @@ IRoadGraph::RoadInfo const & FeaturesRoadGraph::GetCachedRoadInfo(uint32_t const return ri; } + } // namespace routing diff --git a/routing/features_road_graph.hpp b/routing/features_road_graph.hpp index fc0d2c5a8b..ea55a0f744 100644 --- a/routing/features_road_graph.hpp +++ b/routing/features_road_graph.hpp @@ -17,6 +17,7 @@ class FeatureType; namespace routing { + class FeaturesRoadGraph : public IRoadGraph { // TODO (@gorshenin): ft is not set when feature is not loaded from @@ -24,7 +25,7 @@ class FeaturesRoadGraph : public IRoadGraph RoadInfo const & GetCachedRoadInfo(uint32_t const ftId, FeatureType & ft, bool fullLoad); public: - FeaturesRoadGraph(Index const * pIndex, MwmSet::MwmId const & mwmID); + FeaturesRoadGraph(IVehicleModel const * vehicleModel, Index const * pIndex, MwmSet::MwmId const & mwmID); static uint32_t GetStreetReadScale(); @@ -41,24 +42,24 @@ protected: // IRoadGraph overrides: RoadInfo GetRoadInfo(uint32_t featureId) override; double GetSpeedKMPH(uint32_t featureId) override; + double GetMaxSpeedKMPH() override; void ForEachFeatureClosestToCross(m2::PointD const & cross, - CrossTurnsLoader & turnsLoader) override; + CrossEdgesLoader & edgesLoader) override; private: friend class CrossFeaturesLoader; bool IsOneWay(FeatureType const & ft) const; - double GetSpeedKMPHFromFt(FeatureType const & ft) const; - void LoadFeature(uint32_t featureId, FeatureType & ft); - Index const * m_pIndex; - MwmSet::MwmId m_mwmID; - unique_ptr m_vehicleModel; + Index const * const m_pIndex; + MwmSet::MwmId const m_mwmID; + IVehicleModel const * const m_vehicleModel; + my::Cache m_cache; - uint32_t m_cacheMiss; uint32_t m_cacheAccess; }; + } // namespace routing diff --git a/routing/nearest_road_pos_finder.cpp b/routing/nearest_edge_finder.cpp similarity index 53% rename from routing/nearest_road_pos_finder.cpp rename to routing/nearest_edge_finder.cpp index 28cba18ea1..8b7faa00ad 100644 --- a/routing/nearest_road_pos_finder.cpp +++ b/routing/nearest_edge_finder.cpp @@ -1,4 +1,4 @@ -#include "routing/nearest_road_pos_finder.hpp" +#include "routing/nearest_edge_finder.hpp" #include "geometry/distance.hpp" @@ -8,7 +8,23 @@ namespace routing { -void NearestRoadPosFinder::AddInformationSource(uint32_t featureId) + +NearestEdgeFinder::Candidate::Candidate() + : m_dist(numeric_limits::max()), + m_segId(0), + m_segStart(m2::PointD::Zero()), + m_segEnd(m2::PointD::Zero()), + m_point(m2::PointD::Zero()), + m_fid(INVALID_FID) +{ +} + +NearestEdgeFinder::NearestEdgeFinder(m2::PointD const & point, IRoadGraph & roadGraph) + : m_point(point), m_roadGraph(roadGraph) +{ +} + +void NearestEdgeFinder::AddInformationSource(uint32_t featureId) { Candidate res; @@ -28,8 +44,9 @@ void NearestRoadPosFinder::AddInformationSource(uint32_t featureId) res.m_dist = d; res.m_fid = featureId; res.m_segId = i - 1; + res.m_segStart = info.m_points[i - 1]; + res.m_segEnd = info.m_points[i]; res.m_point = pt; - res.m_isOneway = !info.m_bidirectional; } } @@ -37,7 +54,7 @@ void NearestRoadPosFinder::AddInformationSource(uint32_t featureId) m_candidates.push_back(res); } -void NearestRoadPosFinder::MakeResult(vector & res, const size_t maxCount) +void NearestEdgeFinder::MakeResult(vector> & res, size_t const maxCountFeatures) { sort(m_candidates.begin(), m_candidates.end(), [](Candidate const & r1, Candidate const & r2) { @@ -45,18 +62,15 @@ void NearestRoadPosFinder::MakeResult(vector & res, const size_t maxCou }); res.clear(); - res.reserve(maxCount); + res.reserve(maxCountFeatures); + size_t i = 0; for (Candidate const & candidate : m_candidates) { - if (res.size() == maxCount) + res.emplace_back(Edge(candidate.m_fid, true /* forward */, candidate.m_segId, candidate.m_segStart, candidate.m_segEnd), candidate.m_point); + ++i; + if (i == maxCountFeatures) break; - res.push_back(RoadPos(candidate.m_fid, true, candidate.m_segId, candidate.m_point)); - if (res.size() == maxCount) - break; - if (candidate.m_isOneway) - continue; - res.push_back(RoadPos(candidate.m_fid, false, candidate.m_segId, candidate.m_point)); } } diff --git a/routing/nearest_road_pos_finder.hpp b/routing/nearest_edge_finder.hpp similarity index 52% rename from routing/nearest_road_pos_finder.hpp rename to routing/nearest_edge_finder.hpp index b81085b7e5..f415de6a93 100644 --- a/routing/nearest_road_pos_finder.hpp +++ b/routing/nearest_edge_finder.hpp @@ -10,14 +10,15 @@ #include "std/limits.hpp" #include "std/unique_ptr.hpp" +#include "std/utility.hpp" #include "std/vector.hpp" -class FeatureType; - namespace routing { -/// Helper functor class to filter nearest RoadPos'es to the given starting point. -class NearestRoadPosFinder + +/// Helper functor class to filter nearest roads to the given starting point. +/// Class returns pairs of outgoing edge and projection point on the edge +class NearestEdgeFinder { static constexpr uint32_t INVALID_FID = numeric_limits::max(); @@ -25,38 +26,28 @@ class NearestRoadPosFinder { double m_dist; uint32_t m_segId; + m2::PointD m_segStart; + m2::PointD m_segEnd; m2::PointD m_point; uint32_t m_fid; - bool m_isOneway; - Candidate() - : m_dist(numeric_limits::max()), - m_segId(0), - m_point(m2::PointD::Zero()), - m_fid(INVALID_FID), - m_isOneway(false) - { - } + Candidate(); inline bool IsValid() const { return m_fid != INVALID_FID; } }; - m2::PointD m_point; - m2::PointD m_direction; - vector m_candidates; + m2::PointD const m_point; IRoadGraph & m_roadGraph; + vector m_candidates; public: - NearestRoadPosFinder(m2::PointD const & point, m2::PointD const & direction, - IRoadGraph & roadGraph) - : m_point(point), m_direction(direction), m_roadGraph(roadGraph) - { - } + NearestEdgeFinder(m2::PointD const & point, IRoadGraph & roadGraph); inline bool HasCandidates() const { return !m_candidates.empty(); } void AddInformationSource(uint32_t featureId); - void MakeResult(vector & res, size_t const maxCount); + void MakeResult(vector> & res, size_t const maxCountFeatures); }; -} + +} // namespace routing diff --git a/routing/road_graph.cpp b/routing/road_graph.cpp index 95600a1cb1..99175ca015 100644 --- a/routing/road_graph.cpp +++ b/routing/road_graph.cpp @@ -14,97 +14,188 @@ namespace routing { + namespace { -double const KMPH2MPS = 1000.0 / (60 * 60); -double const MAX_SPEED_MPS = 5000.0 / (60 * 60); +double constexpr KMPH2MPS = 1000.0 / (60 * 60); -double CalcDistanceMeters(m2::PointD const & p1, m2::PointD const & p2) +inline double CalcDistanceMeters(m2::PointD const & p1, m2::PointD const & p2) { return MercatorBounds::DistanceOnEarth(p1, p2); } -double TimeBetweenSec(m2::PointD const & p1, m2::PointD const & p2) +inline double TimeBetweenSec(m2::PointD const & p1, m2::PointD const & p2, double speedMPS) { - return CalcDistanceMeters(p1, p2) / MAX_SPEED_MPS; -} -} // namespace - -RoadPos::RoadPos(uint32_t featureId, bool forward, size_t segId, m2::PointD const & p) - : m_featureId((featureId << 1) + (forward ? 1 : 0)), m_segId(segId), m_segEndpoint(p) -{ - ASSERT_LESS(featureId, 1U << 31, ()); - ASSERT_LESS(segId, numeric_limits::max(), ()); + ASSERT(speedMPS > 0.0, ()); + return CalcDistanceMeters(p1, p2) / speedMPS; } -// static -bool RoadPos::IsFakeFeatureId(uint32_t featureId) +inline double TimeBetweenSec(Junction const & j1, Junction const & j2, double speedMPS) { - return featureId == kFakeStartFeatureId || featureId == kFakeFinalFeatureId; + return TimeBetweenSec(j1.GetPoint(), j2.GetPoint(), speedMPS); } -string DebugPrint(RoadPos const & r) +inline bool PointsAlmostEqual(const m2::PointD & pt1, const m2::PointD & pt2) +{ + double constexpr EPSILON = 1e-6; + if ((pt2.x < (pt1.x - EPSILON)) || (pt2.x > (pt1.x + EPSILON))) + return false; + if ((pt2.y < (pt1.y - EPSILON)) || (pt2.y > (pt1.y + EPSILON))) + return false; + return true; +} + +} // namespace + +// Junction -------------------------------------------------------------------- + +Junction::Junction() + : m_point(m2::PointD::Zero()) +{} + +Junction::Junction(m2::PointD const & point) + : m_point(point) +{} + +string DebugPrint(Junction const & r) { ostringstream ss; - ss << "{ featureId: " << r.GetFeatureId() << ", isForward: " << r.IsForward() - << ", segId:" << r.m_segId << ", segEndpoint:" << DebugPrint(r.m_segEndpoint) << "}"; + ss << "Junction{point:" << DebugPrint(r.m_point) << "}"; return ss.str(); } -// IRoadGraph ------------------------------------------------------------------ +// Edge ------------------------------------------------------------------------ -IRoadGraph::CrossTurnsLoader::CrossTurnsLoader(m2::PointD const & cross, TurnsVectorT & turns) - : m_cross(cross), m_turns(turns) +Edge Edge::MakeFake(Junction const & startJunction, Junction const & endJunction) +{ + return Edge(kFakeFeatureId, true /* forward */, 0 /* segId */, startJunction, endJunction); +} + +Edge::Edge(uint32_t featureId, bool forward, size_t segId, Junction const & startJunction, Junction const & endJunction) + : m_featureId(featureId), m_forward(forward), m_segId(segId), m_startJunction(startJunction), m_endJunction(endJunction) +{ + ASSERT_LESS(segId, numeric_limits::max(), ()); +} + +Edge Edge::GetReverseEdge() const +{ + return Edge(m_featureId, !m_forward, m_segId, m_endJunction, m_startJunction); +} + +bool Edge::SameRoadSegmentAndDirection(Edge const & r) const +{ + return m_featureId == r.m_featureId && + m_forward == r.m_forward && + m_segId == r.m_segId; +} + +bool Edge::operator==(Edge const & r) const +{ + return m_featureId == r.m_featureId && + m_forward == r.m_forward && + m_segId == r.m_segId && + m_startJunction == r.m_startJunction && + m_endJunction == r.m_endJunction; +} + +bool Edge::operator<(Edge const & r) const +{ + if (m_featureId != r.m_featureId) + return m_featureId < r.m_featureId; + if (m_forward != r.m_forward) + return (m_forward == false); + if (m_segId != r.m_segId) + return m_segId < r.m_segId; + if (!(m_startJunction == r.m_startJunction)) + return m_startJunction < r.m_startJunction; + if (!(m_endJunction == r.m_endJunction)) + return m_endJunction < r.m_endJunction; + return false; +} + +string DebugPrint(Edge const & r) +{ + ostringstream ss; + ss << "Edge{featureId: " << r.GetFeatureId() << ", isForward:" << r.IsForward() + << ", segId:" << r.m_segId << ", startJunction:" << DebugPrint(r.m_startJunction) + << ", endJunction:" << DebugPrint(r.m_endJunction) << "}"; + return ss.str(); +} + +// IRoadGraph::RoadInfo -------------------------------------------------------- + +IRoadGraph::RoadInfo::RoadInfo() + : m_speedKMPH(0.0), m_bidirectional(false) +{} + +IRoadGraph::RoadInfo::RoadInfo(RoadInfo && ri) + : m_points(move(ri.m_points)), + m_speedKMPH(ri.m_speedKMPH), + m_bidirectional(ri.m_bidirectional) +{} + +IRoadGraph::RoadInfo::RoadInfo(bool bidirectional, double speedKMPH, initializer_list const & points) + : m_points(points), m_speedKMPH(speedKMPH), m_bidirectional(bidirectional) +{} + +// IRoadGraph::CrossEdgesLoader ------------------------------------------------ + +IRoadGraph::CrossEdgesLoader::CrossEdgesLoader(m2::PointD const & cross, TEdgeVector & outgoingEdges) + : m_cross(cross), m_outgoingEdges(outgoingEdges) { } -void IRoadGraph::CrossTurnsLoader::operator()(uint32_t featureId, RoadInfo const & roadInfo) +void IRoadGraph::CrossEdgesLoader::operator()(uint32_t featureId, RoadInfo const & roadInfo) { if (roadInfo.m_points.empty()) return; size_t const numPoints = roadInfo.m_points.size(); - PossibleTurn turn; - turn.m_speedKMPH = roadInfo.m_speedKMPH; - turn.m_startPoint = roadInfo.m_points[0]; - turn.m_endPoint = roadInfo.m_points[numPoints - 1]; - for (size_t i = 0; i < numPoints; ++i) { m2::PointD const & p = roadInfo.m_points[i]; - // @todo Is this a correct way to compare? - if (!m2::AlmostEqual(m_cross, p)) + if (!PointsAlmostEqual(m_cross, p)) continue; if (i > 0) { - turn.m_pos = RoadPos(featureId, true /* forward */, i - 1, p); - m_turns.push_back(turn); + // p + // o------------>o + + Edge edge(featureId, false /* forward */, i - 1, p, roadInfo.m_points[i - 1]); + m_outgoingEdges.push_back(edge); } if (i < numPoints - 1) { - turn.m_pos = RoadPos(featureId, false /* forward */, i, p); - m_turns.push_back(turn); + // p + // o------------>o + + Edge edge(featureId, true /* forward */, i, p, roadInfo.m_points[i + 1]); + m_outgoingEdges.push_back(edge); } } } -void IRoadGraph::ReconstructPath(RoadPosVectorT const & positions, Route & route) +// IRoadGraph ------------------------------------------------------------------ + +void IRoadGraph::ReconstructPath(TJunctionVector const & positions, Route & route) { CHECK(!positions.empty(), ("Can't reconstruct path from an empty list of positions.")); vector path; path.reserve(positions.size()); + double const speedMPS = GetMaxSpeedKMPH() * KMPH2MPS; + double trackTimeSec = 0.0; - m2::PointD prevPoint = positions[0].GetSegEndpoint(); + m2::PointD prevPoint = positions[0].GetPoint(); path.push_back(prevPoint); for (size_t i = 1; i < positions.size(); ++i) { - m2::PointD curPoint = positions[i].GetSegEndpoint(); + m2::PointD const curPoint = positions[i].GetPoint(); // By some reason there're two adjacent positions on a road with // the same end-points. This could happen, for example, when @@ -116,10 +207,6 @@ void IRoadGraph::ReconstructPath(RoadPosVectorT const & positions, Route & route path.push_back(curPoint); double const lengthM = CalcDistanceMeters(prevPoint, curPoint); - uint32_t const prevFeatureId = positions[i - 1].GetFeatureId(); - double const speedMPS = RoadPos::IsFakeFeatureId(prevFeatureId) - ? MAX_SPEED_MPS - : GetSpeedKMPH(prevFeatureId) * KMPH2MPS; trackTimeSec += lengthM / speedMPS; prevPoint = curPoint; } @@ -132,8 +219,8 @@ void IRoadGraph::ReconstructPath(RoadPosVectorT const & positions, Route & route ASSERT_GREATER_OR_EQUAL(path.size(), 2, ()); - // TODO: investigate whether it worth to reconstruct detailed turns - // and times. + /// @todo: investigate whether it's worth reconstructing detailed turns and times. + Route::TimesT times; times.emplace_back(path.size() - 1, trackTimeSec); @@ -145,106 +232,128 @@ void IRoadGraph::ReconstructPath(RoadPosVectorT const & positions, Route & route route.SetSectionTimes(times); } -void IRoadGraph::GetNearestTurns(RoadPos const & pos, TurnsVectorT & turns) +void IRoadGraph::GetOutgoingEdges(Junction const & junction, TEdgeVector & edges) { - uint32_t const featureId = pos.GetFeatureId(); - - // For fake start and final positions add vicinity turns as nearest - // turns. - if (featureId == RoadPos::kFakeStartFeatureId) + auto const itr = m_outgoingEdges.find(junction); + if (itr != m_outgoingEdges.end()) { - turns.insert(turns.end(), m_startVicinityTurns.begin(), m_startVicinityTurns.end()); - return; + edges.reserve(edges.size() + itr->second.size()); + edges.insert(edges.end(), itr->second.begin(), itr->second.end()); } - if (featureId == RoadPos::kFakeFinalFeatureId) + else { - turns.insert(turns.end(), m_finalVicinityTurns.begin(), m_finalVicinityTurns.end()); - return; + GetRegularOutgoingEdges(junction, edges); } +} - RoadInfo const roadInfo = GetRoadInfo(featureId); +void IRoadGraph::GetIngoingEdges(Junction const & junction, TEdgeVector & edges) +{ + size_t const wasSize = edges.size(); - if (roadInfo.m_speedKMPH <= 0.0) - return; + GetOutgoingEdges(junction, edges); - ASSERT_GREATER_OR_EQUAL(roadInfo.m_points.size(), 2, - ("Incorrect road - only", roadInfo.m_points.size(), "point(s).")); + size_t const size = edges.size(); + for (size_t i = wasSize; i < size; ++i) + edges[i] = edges[i].GetReverseEdge(); +} - m2::PointD const & cross = roadInfo.m_points[pos.GetSegStartPointId()]; - - CrossTurnsLoader loader(cross, turns); +void IRoadGraph::GetRegularOutgoingEdges(Junction const & junction, TEdgeVector & edges) +{ + m2::PointD const cross = junction.GetPoint(); + CrossEdgesLoader loader(cross, edges); ForEachFeatureClosestToCross(cross, loader); - - AddFakeTurns(pos, roadInfo, m_startVicinityRoadPoss, turns); - AddFakeTurns(pos, roadInfo, m_finalVicinityRoadPoss, turns); - - // It is also possible to move from a start's or final's vicinity - // positions to start or final points. - for (PossibleTurn const & turn : m_fakeTurns[pos]) - turns.push_back(turn); } -void IRoadGraph::AddFakeTurns(RoadPos const & pos, RoadInfo const & roadInfo, - RoadPosVectorT const & vicinity, TurnsVectorT & turns) +void IRoadGraph::ResetFakes() { - for (RoadPos const & vpos : vicinity) + m_outgoingEdges.clear(); +} + +void IRoadGraph::AddFakeEdges(Junction const & junction, vector> const & vicinity) +{ + for (auto const & v : vicinity) { - if (!vpos.SameRoadSegmentAndDirection(pos)) - continue; + Edge const & closestEdge = v.first; + Junction const p = v.second; - // It is also possible to move from a road position to start's or - // final's vicinity positions if they're on the same road segment. - PossibleTurn turn; - turn.m_secondsCovered = TimeBetweenSec(pos.GetSegEndpoint(), vpos.GetSegEndpoint()); - turn.m_speedKMPH = roadInfo.m_speedKMPH; - turn.m_startPoint = roadInfo.m_points.front(); - turn.m_endPoint = roadInfo.m_points.back(); - turn.m_pos = vpos; - turns.push_back(turn); - } -} + if (p == closestEdge.GetStartJunction() || p == closestEdge.GetEndJunction()) + { + // The point is mapped on the start junction of the edge or on the end junction of the edge: + // o M o M + // ^ ^ + // | | + // | | + // (P) A o--------------->o B or A o--------------->o B (P) (the feature is A->B) + // Here AB is a feature, M is a junction, which is projected to A (where P is projection), + // P is the closest junction of the feature to the junction M. -void IRoadGraph::ResetFakeTurns() -{ - m_startVicinityTurns.clear(); - m_startVicinityRoadPoss.clear(); - m_finalVicinityTurns.clear(); - m_finalVicinityRoadPoss.clear(); - m_fakeTurns.clear(); -} + // Add outgoing edges for M. + TEdgeVector & edgesM = m_outgoingEdges[junction]; + edgesM.push_back(Edge::MakeFake(junction, p)); -void IRoadGraph::AddFakeTurns(RoadPos const & rp, vector const & vicinity) -{ - vector * turns = nullptr; - vector * roadPoss = nullptr; - uint32_t const featureId = rp.GetFeatureId(); - switch (featureId) - { - case RoadPos::kFakeStartFeatureId: - turns = &m_startVicinityTurns; - roadPoss = &m_startVicinityRoadPoss; - break; - case RoadPos::kFakeFinalFeatureId: - turns = &m_finalVicinityTurns; - roadPoss = &m_finalVicinityRoadPoss; - break; - default: - CHECK(false, ("Can't add fake turns from a valid road (featureId ", featureId, ").")); + // Add outgoing edges for P. + TEdgeVector & edgesP = m_outgoingEdges[p]; + GetRegularOutgoingEdges(p, edgesP); + edgesP.push_back(Edge::MakeFake(p, junction)); + } + else + { + // The point is mapped in the middle of the feature: + // o M + // ^ + // | + // | + // A o<-------x------->o B (the feature is A->B) + // P + // Here AB is a feature, M is a junction and P is a projection of M on AB, + // Edge AB has been splitted to two fake edges AP and PB (similarly BA edge has been splitted to two + // fake edges BP and PA). In the result graph edges AB and BA are redundant, therefore edges AB and BA are + // replaced by fake edges AP + PB and BP + PA. + + Edge const pa(closestEdge.GetFeatureId(), false, closestEdge.GetSegId(), p, closestEdge.GetStartJunction()); + Edge const pb(closestEdge.GetFeatureId(), true, closestEdge.GetSegId(), p, closestEdge.GetEndJunction()); + Edge const pm = Edge::MakeFake(p, junction); + + // Add outgoing edges to point P. + TEdgeVector & edgesP = m_outgoingEdges[p]; + edgesP.push_back(pa); + edgesP.push_back(pb); + edgesP.push_back(pm); + + // Add outgoing edges for point M. + m_outgoingEdges[junction].push_back(pm.GetReverseEdge()); + + // Add outgoing edges for point A. AB edge will be replaced by AP edge. + TEdgeVector & edgesA = m_outgoingEdges[pa.GetEndJunction()]; + GetRegularOutgoingEdges(pa.GetEndJunction(), edgesA); + Edge const ap = pa.GetReverseEdge(); + edgesA.erase(remove_if(edgesA.begin(), edgesA.end(), [&](Edge const & e) { return e.SameRoadSegmentAndDirection(ap); }), edgesA.end()); + edgesA.push_back(ap); + + // Add outgoing edges for point B. BA edge will be replaced by BP edge. + TEdgeVector & edgesB = m_outgoingEdges[pb.GetEndJunction()]; + GetRegularOutgoingEdges(pb.GetEndJunction(), edgesB); + Edge const bp = pb.GetReverseEdge(); + edgesB.erase(remove_if(edgesB.begin(), edgesB.end(), [&](Edge const & e) { return e.SameRoadSegmentAndDirection(bp); }), edgesB.end()); + edgesB.push_back(bp); + } } - roadPoss->insert(roadPoss->end(), vicinity.begin(), vicinity.end()); + /// @todo There's case here when Start and Finish points are projected to the same segment of the feature. + /// Then, feature can be split into 3 pieces. + // o M o N + // ^ ^ + // | fe | + // | | + // A o<-------x--------------x------------->o B + // Here AB is feature, M and N are junction, which is projected to A or to B. - for (RoadPos const & vrp : vicinity) + // m_outgoingEdges may contain duplicates. Remove them. + for (auto & m : m_outgoingEdges) { - PossibleTurn turn; - turn.m_pos = vrp; - // @todo Do we need other fields? Do we even need m_secondsCovered? - turn.m_secondsCovered = TimeBetweenSec(rp.GetSegEndpoint(), vrp.GetSegEndpoint()); - turns->push_back(turn); - - // Add a fake turn from a vicincy road position to a fake point. - turn.m_pos = rp; - m_fakeTurns[vrp].push_back(turn); + TEdgeVector & edges = m.second; + sort(edges.begin(), edges.end()); + edges.erase(unique(edges.begin(), edges.end()), edges.end()); } } @@ -252,20 +361,58 @@ void IRoadGraph::AddFakeTurns(RoadPos const & rp, vector const & vicini RoadGraph::RoadGraph(IRoadGraph & roadGraph) : m_roadGraph(roadGraph) {} -void RoadGraph::GetAdjacencyListImpl(RoadPos const & v, vector & adj) const +double RoadGraph::GetSpeedMPS(Edge const & e) const { - IRoadGraph::TurnsVectorT turns; - m_roadGraph.GetNearestTurns(v, turns); - for (PossibleTurn const & turn : turns) + double const speedKMPH = (e.IsFake() ? m_roadGraph.GetMaxSpeedKMPH() : m_roadGraph.GetSpeedKMPH(e.GetFeatureId())); + ASSERT(speedKMPH <= m_roadGraph.GetMaxSpeedKMPH(), ()); + + return speedKMPH * KMPH2MPS; +} + +void RoadGraph::GetOutgoingEdgesListImpl(Junction const & v, vector & adj) const +{ + IRoadGraph::TEdgeVector edges; + m_roadGraph.GetOutgoingEdges(v, edges); + + adj.clear(); + adj.reserve(edges.size()); + + for (auto const & e : edges) { - RoadPos const & w = turn.m_pos; - adj.emplace_back(w, TimeBetweenSec(v.GetSegEndpoint(), w.GetSegEndpoint())); + double const speedMPS = GetSpeedMPS(e); + if (speedMPS <= 0.0) + continue; + + ASSERT_EQUAL(v, e.GetStartJunction(), ()); + + adj.emplace_back(e.GetEndJunction(), TimeBetweenSec(e.GetStartJunction(), e.GetEndJunction(), speedMPS)); } } -double RoadGraph::HeuristicCostEstimateImpl(RoadPos const & v, RoadPos const & w) const +void RoadGraph::GetIngoingEdgesListImpl(Junction const & v, vector & adj) const { - return TimeBetweenSec(v.GetSegEndpoint(), w.GetSegEndpoint()); + IRoadGraph::TEdgeVector edges; + m_roadGraph.GetIngoingEdges(v, edges); + + adj.clear(); + adj.reserve(edges.size()); + + for (auto const & e : edges) + { + double const speedMPS = GetSpeedMPS(e); + if (speedMPS <= 0.0) + continue; + + ASSERT_EQUAL(v, e.GetEndJunction(), ()); + + adj.emplace_back(e.GetStartJunction(), TimeBetweenSec(e.GetStartJunction(), e.GetEndJunction(), speedMPS)); + } +} + +double RoadGraph::HeuristicCostEstimateImpl(Junction const & v, Junction const & w) const +{ + double const speedMPS = m_roadGraph.GetMaxSpeedKMPH() * KMPH2MPS; + return TimeBetweenSec(v, w, speedMPS); } } // namespace routing diff --git a/routing/road_graph.hpp b/routing/road_graph.hpp index 8df4653ed3..ceca60440a 100644 --- a/routing/road_graph.hpp +++ b/routing/road_graph.hpp @@ -6,211 +6,187 @@ #include "base/string_utils.hpp" -#include "std/vector.hpp" +#include "std/initializer_list.hpp" +#include "std/limits.hpp" #include "std/map.hpp" +#include "std/vector.hpp" namespace routing { + class Route; -/// Defines position on a feature which represents a road with direction. -class RoadPos +/// The Junction class represents a node description on a road network graph +class Junction { public: - // Our routers (such as a-star) use RoadPos as vertices and we receive PointD from the user. - // Every time a route is calculated, we create a fake RoadPos with a fake featureId to - // place the real starting point on it (and also do the same for the final point). - // The constants here are used as those fake features' ids. - /// @todo The constant value is taken to comply with an assert in the constructor - /// that is terrible, wrong and to be rewritten in a separate CL. - static constexpr uint32_t kFakeStartFeatureId = (1U << 30) - 1; - static constexpr uint32_t kFakeFinalFeatureId = (1U << 30) - 2; + Junction(); + Junction(m2::PointD const & point); + Junction(Junction const &) = default; + Junction & operator=(Junction const &) = default; - RoadPos() : m_featureId(0), m_segId(0), m_segEndpoint(0, 0) {} - RoadPos(uint32_t featureId, bool forward, size_t segId, - m2::PointD const & p = m2::PointD::Zero()); + inline bool operator==(Junction const & r) const { return m_point == r.m_point; } + inline bool operator<(Junction const & r) const { return m_point < r.m_point; } - static bool IsFakeFeatureId(uint32_t featureId); - - uint32_t GetFeatureId() const { return m_featureId >> 1; } - bool IsForward() const { return (m_featureId & 1) != 0; } - uint32_t GetSegId() const { return m_segId; } - uint32_t GetSegStartPointId() const { return m_segId + (IsForward() ? 0 : 1); } - uint32_t GetSegEndPointId() const { return m_segId + (IsForward() ? 1 : 0); } - m2::PointD const & GetSegEndpoint() const { return m_segEndpoint; } - - inline bool SameRoadSegmentAndDirection(RoadPos const & r) const - { - return m_featureId == r.m_featureId && m_segId == r.m_segId; - } - - inline bool operator==(RoadPos const & r) const - { - return m_featureId == r.m_featureId && m_segId == r.m_segId && m_segEndpoint == r.m_segEndpoint; - } - - inline bool operator!=(RoadPos const & r) const { return !(*this == r); } - - inline bool operator<(RoadPos const & r) const - { - if (m_featureId != r.m_featureId) - return m_featureId < r.m_featureId; - if (m_segId != r.m_segId) - return m_segId < r.m_segId; - if (m_segEndpoint != r.m_segEndpoint) - return m_segEndpoint < r.m_segEndpoint; - return false; - } + m2::PointD const & GetPoint() const { return m_point; } private: - friend string DebugPrint(RoadPos const & r); + friend string DebugPrint(Junction const & r); + + // Point of the junction + m2::PointD m_point; +}; + +/// The Edge class represents an edge description on a road network graph +class Edge +{ +private: + static constexpr uint32_t kFakeFeatureId = numeric_limits::max(); + +public: + static Edge MakeFake(Junction const & startJunction, Junction const & endJunction); + + Edge(uint32_t featureId, bool forward, size_t segId, Junction const & startJunction, Junction const & endJunction); + Edge(Edge const &) = default; + Edge & operator=(Edge const &) = default; + + inline uint32_t GetFeatureId() const { return m_featureId; } + inline bool IsForward() const { return m_forward; } + inline uint32_t GetSegId() const { return m_segId; } + inline Junction const & GetStartJunction() const { return m_startJunction; } + inline Junction const & GetEndJunction() const { return m_endJunction; } + inline bool IsFake() const { return m_featureId == kFakeFeatureId; } + + Edge GetReverseEdge() const; + + bool SameRoadSegmentAndDirection(Edge const & r) const; + + bool operator==(Edge const & r) const; + bool operator<(Edge const & r) const; + +private: + friend string DebugPrint(Edge const & r); // Feature on which position is defined. uint32_t m_featureId; + // Is the feature along the road. + bool m_forward; + // Ordinal number of the segment on the road. uint32_t m_segId; - // End-point of the segment on the road. - m2::PointD m_segEndpoint; + // Start junction of the segment on the road. + Junction m_startJunction; + + // End junction of the segment on the road. + Junction m_endJunction; }; -/// The turn from the old to the new road. -struct PossibleTurn -{ - /// New road information. - RoadPos m_pos; - - /// Start point on the old road. - m2::PointD m_startPoint; - - /// End point on the old road. - m2::PointD m_endPoint; - - /// Speed on the old road. - double m_speedKMPH; - - /// Distance and time to get to this turn on old road. - double m_metersCovered; - double m_secondsCovered; - - PossibleTurn() : m_metersCovered(0.0), m_secondsCovered(0.0) {} -}; - -inline string DebugPrint(PossibleTurn const & r) { return DebugPrint(r.m_pos); } - class IRoadGraph { public: - typedef vector TurnsVectorT; - typedef vector RoadPosVectorT; - typedef vector PointsVectorT; + typedef vector TJunctionVector; + typedef vector TEdgeVector; - // This struct contains the part of a feature's metadata that is - // relevant for routing. + /// This struct contains the part of a feature's metadata that is + /// relevant for routing. struct RoadInfo { - RoadInfo() : m_speedKMPH(0.0), m_bidirectional(false) {} - - RoadInfo(RoadInfo const & ri) - : m_points(ri.m_points), m_speedKMPH(ri.m_speedKMPH), m_bidirectional(ri.m_bidirectional) - { - } - - RoadInfo(RoadInfo && ri) - : m_points(move(ri.m_points)), - m_speedKMPH(ri.m_speedKMPH), - m_bidirectional(ri.m_bidirectional) - { - } + RoadInfo(); + RoadInfo(RoadInfo && ri); + RoadInfo(bool bidirectional, double speedKMPH, initializer_list const & points); + RoadInfo(RoadInfo const &) = default; + RoadInfo & operator=(RoadInfo const &) = default; buffer_vector m_points; double m_speedKMPH; bool m_bidirectional; }; - class CrossTurnsLoader + /// This class is responsible for loading edges in a cross. + /// It loades only outgoing edges. + class CrossEdgesLoader { public: - CrossTurnsLoader(m2::PointD const & cross, TurnsVectorT & turns); + CrossEdgesLoader(m2::PointD const & cross, TEdgeVector & outgoingEdges); void operator()(uint32_t featureId, RoadInfo const & roadInfo); private: - m2::PointD m_cross; - TurnsVectorT & m_turns; + m2::PointD const m_cross; + TEdgeVector & m_outgoingEdges; }; virtual ~IRoadGraph() = default; /// Construct route by road positions (doesn't include first and last section). - void ReconstructPath(RoadPosVectorT const & positions, Route & route); + void ReconstructPath(TJunctionVector const & junctions, Route & route); - /// Finds all nearest feature sections (turns), that route to the - /// "pos" section. - void GetNearestTurns(RoadPos const & pos, TurnsVectorT & turns); + /// Finds all nearest outgoing edges, that route to the junction. + void GetOutgoingEdges(Junction const & junction, TEdgeVector & edges); + + /// Finds all nearest ingoing edges, that route to the junction. + void GetIngoingEdges(Junction const & junction, TEdgeVector & edges); /// Removes all fake turns and vertices from the graph. - void ResetFakeTurns(); + void ResetFakes(); /// Adds fake turns from fake position rp to real vicinity /// positions. - void AddFakeTurns(RoadPos const & rp, RoadPosVectorT const & vicinity); + void AddFakeEdges(Junction const & junction, vector> const & vicinities); - // Returns RoadInfo for a road corresponding to featureId. + /// Returns RoadInfo for a road corresponding to featureId. virtual RoadInfo GetRoadInfo(uint32_t featureId) = 0; - // Returns speed in KM/H for a road corresponding to featureId. + /// Returns speed in KM/H for a road corresponding to featureId. virtual double GetSpeedKMPH(uint32_t featureId) = 0; -protected: - // Calls turnsLoader on each feature which is close to cross. + /// Returns max speed in KM/H + virtual double GetMaxSpeedKMPH() = 0; + + /// Calls edgesLoader on each feature which is close to cross. virtual void ForEachFeatureClosestToCross(m2::PointD const & cross, - CrossTurnsLoader & turnsLoader) = 0; + CrossEdgesLoader & edgesLoader) = 0; - void AddFakeTurns(RoadPos const & pos, RoadInfo const & roadInfo, RoadPosVectorT const & vicinity, - TurnsVectorT & turns); +private: + /// Finds all outgoing regular (non-fake) edges for junction. + void GetRegularOutgoingEdges(Junction const & junction, TEdgeVector & edges); - // The way we find edges leading from start/final positions and from all other positions - // differ: for start/final we find positions in some vicinity of the starting - // point; for other positions we extract turns from the road graph. This non-uniformity - // comes from the fact that a start/final position does not necessarily fall on a feature - // (i.e. on a road). - vector m_startVicinityTurns; - vector m_startVicinityRoadPoss; - - vector m_finalVicinityTurns; - vector m_finalVicinityRoadPoss; - - map m_fakeTurns; + // Map of outgoing edges for junction + map m_outgoingEdges; }; -// A class which represents an edge used by RoadGraph. -struct RoadEdge +/// A class which represents an weighted edge used by RoadGraph. +struct WeightedEdge { - RoadEdge(RoadPos const & target, double weight) : target(target), weight(weight) {} + WeightedEdge(Junction const & target, double weight) : target(target), weight(weight) {} - inline RoadPos const & GetTarget() const { return target; } + inline Junction const & GetTarget() const { return target; } inline double GetWeight() const { return weight; } - RoadPos const target; + Junction const target; double const weight; }; -// A wrapper around IGraph, which makes it possible to use IRoadGraph -// with routing algorithms. -class RoadGraph : public Graph +/// A wrapper around IGraph, which makes it possible to use IRoadGraph +/// with routing algorithms. +class RoadGraph : public Graph { public: RoadGraph(IRoadGraph & roadGraph); private: - friend class Graph; + friend class Graph; - // Graph implementation: - void GetAdjacencyListImpl(RoadPos const & v, vector & adj) const; - double HeuristicCostEstimateImpl(RoadPos const & v, RoadPos const & w) const; + // Returns speed in M/S for specified edge + double GetSpeedMPS(Edge const & edge) const; + + // Graph implementation: + void GetOutgoingEdgesListImpl(Junction const & v, vector & adj) const; + void GetIngoingEdgesListImpl(Junction const & v, vector & adj) const; + double HeuristicCostEstimateImpl(Junction const & v, Junction const & w) const; IRoadGraph & m_roadGraph; }; diff --git a/routing/road_graph_router.cpp b/routing/road_graph_router.cpp index 8a2cb0dc8b..1971a1e19b 100644 --- a/routing/road_graph_router.cpp +++ b/routing/road_graph_router.cpp @@ -1,5 +1,5 @@ #include "routing/features_road_graph.hpp" -#include "routing/nearest_road_pos_finder.hpp" +#include "routing/nearest_edge_finder.hpp" #include "routing/road_graph_router.hpp" #include "routing/route.hpp" #include "routing/vehicle_model.hpp" @@ -16,19 +16,20 @@ namespace routing { + namespace { -// TODO (@gorshenin, @pimenov, @ldragunov): MAX_ROAD_CANDIDATES == 2 -// means that only one closest feature (in both directions) will be -// examined when searching for features in the vicinity of start and -// final points. It is an oversimplification that is not as easily +// TODO (@gorshenin, @pimenov, @ldragunov): MAX_ROAD_CANDIDATES == 1 +// means that only two closest feature will be examined when searching +// for features in the vicinity of start and final points. +// It is an oversimplification that is not as easily // solved as tuning up this constant because if you set it too high // you risk to find a feature that you cannot in fact reach because of // an obstacle. Using only the closest feature minimizes (but not // eliminates) this risk. -size_t const MAX_ROAD_CANDIDATES = 2; +size_t const MAX_ROAD_CANDIDATES = 1; double const FEATURE_BY_POINT_RADIUS_M = 100.0; -} // namespace +} // namespace RoadGraphRouter::~RoadGraphRouter() {} @@ -38,19 +39,21 @@ RoadGraphRouter::RoadGraphRouter(Index const * pIndex, unique_ptr { } -void RoadGraphRouter::GetRoadPos(m2::PointD const & pt, vector & pos) +void RoadGraphRouter::GetClosestEdges(m2::PointD const & pt, vector> & edges) { - NearestRoadPosFinder finder(pt, m2::PointD::Zero() /* undirected */, *m_roadGraph.get()); - auto f = [&finder, this](FeatureType & ft) + NearestEdgeFinder finder(pt, *m_roadGraph.get()); + + auto const f = [&finder, this](FeatureType & ft) { if (ft.GetFeatureType() == feature::GEOM_LINE && m_vehicleModel->GetSpeed(ft) > 0.0) finder.AddInformationSource(ft.GetID().m_offset); }; + m_pIndex->ForEachInRect( f, MercatorBounds::RectByCenterXYAndSizeInMeters(pt, FEATURE_BY_POINT_RADIUS_M), FeaturesRoadGraph::GetStreetReadScale()); - finder.MakeResult(pos, MAX_ROAD_CANDIDATES); + finder.MakeResult(edges, MAX_ROAD_CANDIDATES); } bool RoadGraphRouter::IsMyMWM(MwmSet::MwmId const & mwmID) const @@ -66,29 +69,29 @@ IRouter::ResultCode RoadGraphRouter::CalculateRoute(m2::PointD const & startPoin // Despite adding fake notes and calculating their vicinities on the fly // we still need to check that startPoint and finalPoint are in the same MWM // and probably reset the graph. So the checks stay here. - vector finalVicinity; + + LOG(LDEBUG, ("Calculate route from", startPoint, "to", finalPoint)); + string const mwmName = m_countryFileFn(finalPoint); - MwmSet::MwmLock const mwmLock = const_cast(*m_pIndex).GetMwmLockByFileName(mwmName); - if (!mwmLock.IsLocked()) - return EndPointNotFound; - MwmSet::MwmId mwmID = mwmLock.GetId(); - - if (!IsMyMWM(mwmID)) - m_roadGraph.reset(new FeaturesRoadGraph(m_pIndex, mwmID)); - - if (!m_roadGraph) - return EndPointNotFound; - - GetRoadPos(finalPoint, finalVicinity); - - if (finalVicinity.empty()) - return EndPointNotFound; - if (m_countryFileFn(startPoint) != mwmName) return PointsInDifferentMWM; - vector startVicinity; - GetRoadPos(startPoint, startVicinity); + MwmSet::MwmLock const mwmLock = const_cast(*m_pIndex).GetMwmLockByFileName(mwmName); + if (!mwmLock.IsLocked()) + return RouteFileNotExist; + + MwmSet::MwmId const mwmID = mwmLock.GetId(); + if (!IsMyMWM(mwmID)) + m_roadGraph.reset(new FeaturesRoadGraph(m_vehicleModel.get(), m_pIndex, mwmID)); + + vector> finalVicinity; + GetClosestEdges(finalPoint, finalVicinity); + + if (finalVicinity.empty()) + return EndPointNotFound; + + vector> startVicinity; + GetClosestEdges(startPoint, startVicinity); if (startVicinity.empty()) return StartPointNotFound; @@ -96,16 +99,18 @@ IRouter::ResultCode RoadGraphRouter::CalculateRoute(m2::PointD const & startPoin my::Timer timer; timer.Reset(); - RoadPos startPos(RoadPos::kFakeStartFeatureId, true /* forward */, 0 /* segId */, startPoint); - RoadPos finalPos(RoadPos::kFakeFinalFeatureId, true /* forward */, 0 /* segId */, finalPoint); + Junction const startPos(startPoint); + Junction const finalPos(finalPoint); - m_roadGraph->ResetFakeTurns(); - m_roadGraph->AddFakeTurns(startPos, startVicinity); - m_roadGraph->AddFakeTurns(finalPos, finalVicinity); + m_roadGraph->ResetFakes(); + m_roadGraph->AddFakeEdges(startPos, startVicinity); + m_roadGraph->AddFakeEdges(finalPos, finalVicinity); - vector routePos; + vector routePos; IRouter::ResultCode const resultCode = CalculateRoute(startPos, finalPos, routePos); + m_roadGraph->ResetFakes(); + LOG(LINFO, ("Route calculation time:", timer.ElapsedSeconds(), "result code:", resultCode)); if (IRouter::NoError == resultCode) diff --git a/routing/road_graph_router.hpp b/routing/road_graph_router.hpp index bd6b76241b..6fbd6a7423 100644 --- a/routing/road_graph_router.hpp +++ b/routing/road_graph_router.hpp @@ -17,6 +17,7 @@ class Index; namespace routing { + class RoadGraphRouter : public IRouter { public: @@ -28,20 +29,24 @@ public: ResultCode CalculateRoute(m2::PointD const & startPoint, m2::PointD const & startDirection, m2::PointD const & finalPoint, Route & route) override; - virtual ResultCode CalculateRoute(RoadPos const & startPos, RoadPos const & finalPos, - vector & route) = 0; + virtual void SetRoadGraph(unique_ptr && roadGraph) { m_roadGraph = move(roadGraph); } inline IRoadGraph * GetGraph() { return m_roadGraph.get(); } protected: + virtual ResultCode CalculateRoute(Junction const & startPos, Junction const & finalPos, + vector & route) = 0; + +private: /// @todo This method fits better in features_road_graph. - void GetRoadPos(m2::PointD const & pt, vector & pos); + void GetClosestEdges(m2::PointD const & pt, vector> & edges); bool IsMyMWM(MwmSet::MwmId const & mwmID) const; unique_ptr m_roadGraph; unique_ptr const m_vehicleModel; - Index const * m_pIndex; // non-owning ptr - TMwmFileByPointFn m_countryFileFn; + Index const * const m_pIndex; // non-owning ptr + TMwmFileByPointFn const m_countryFileFn; }; + } // namespace routing diff --git a/routing/router.hpp b/routing/router.hpp index 57e1524d48..e2fcdb6746 100644 --- a/routing/router.hpp +++ b/routing/router.hpp @@ -61,4 +61,4 @@ public: typedef function RoutingVisualizerFn; -} +} // namespace routing diff --git a/routing/routing.pro b/routing/routing.pro index 12a65af8f0..4dd1248cfb 100644 --- a/routing/routing.pro +++ b/routing/routing.pro @@ -17,7 +17,7 @@ SOURCES += \ async_router.cpp \ cross_routing_context.cpp \ features_road_graph.cpp \ - nearest_road_pos_finder.cpp \ + nearest_edge_finder.cpp \ online_cross_fetcher.cpp \ osrm2feature_map.cpp \ osrm_online_router.cpp \ @@ -37,7 +37,7 @@ HEADERS += \ base/graph.hpp \ cross_routing_context.hpp \ features_road_graph.hpp \ - nearest_road_pos_finder.hpp \ + nearest_edge_finder.hpp \ online_cross_fetcher.hpp \ osrm2feature_map.hpp \ osrm_data_facade.hpp \ diff --git a/routing/routing_tests/astar_algorithm_test.cpp b/routing/routing_tests/astar_algorithm_test.cpp index a910ea3bde..b62b75e79c 100644 --- a/routing/routing_tests/astar_algorithm_test.cpp +++ b/routing/routing_tests/astar_algorithm_test.cpp @@ -6,8 +6,11 @@ #include "std/utility.hpp" #include "std/vector.hpp" -namespace routing +namespace routing_test { + +using namespace routing; + struct Edge { Edge(unsigned v, double w) : v(v), w(w) {} @@ -33,10 +36,20 @@ private: void GetAdjacencyListImpl(unsigned v, vector & adj) const { - auto it = m_adjs.find(v); - if (it == m_adjs.end()) - return; - adj.assign(it->second.begin(), it->second.end()); + adj.clear(); + auto const it = m_adjs.find(v); + if (it != m_adjs.end()) + adj = it->second; + } + + void GetIngoingEdgesListImpl(unsigned v, vector & adj) const + { + GetAdjacencyListImpl(v, adj); + } + + void GetOutgoingEdgesListImpl(unsigned v, vector & adj) const + { + GetAdjacencyListImpl(v, adj); } double HeuristicCostEstimateImpl(unsigned v, unsigned w) const { return 0; } @@ -70,8 +83,9 @@ UNIT_TEST(AStarAlgorithm_Sample) graph.AddEdge(2, 4, 10); graph.AddEdge(3, 4, 3); - vector expectedRoute = {0, 1, 2, 3, 4}; + vector const expectedRoute = {0, 1, 2, 3, 4}; TestAStar(graph, expectedRoute); } -} // namespace routing + +} // namespace routing_test diff --git a/routing/routing_tests/astar_router_test.cpp b/routing/routing_tests/astar_router_test.cpp index 89cca6d9db..c05bda4923 100644 --- a/routing/routing_tests/astar_router_test.cpp +++ b/routing/routing_tests/astar_router_test.cpp @@ -1,7 +1,6 @@ #include "testing/testing.hpp" #include "routing/routing_tests/road_graph_builder.hpp" -#include "routing/routing_tests/features_road_graph_test.hpp" #include "routing/astar_router.hpp" #include "routing/features_road_graph.hpp" @@ -15,9 +14,14 @@ using namespace routing; using namespace routing_test; -void TestAStarRouterMock(RoadPos const & startPos, RoadPos const & finalPos, +namespace +{ + +void TestAStarRouterMock(Junction const & startPos, Junction const & finalPos, m2::PolylineD const & expected) { + classificator::Load(); + AStarRouter router([](m2::PointD const & /*point*/) { return "Dummy_map.mwm"; @@ -27,7 +31,8 @@ void TestAStarRouterMock(RoadPos const & startPos, RoadPos const & finalPos, InitRoadGraphMockSourceWithTest2(*graph); router.SetRoadGraph(move(graph)); } - vector result; + + vector result; TEST_EQUAL(IRouter::NoError, router.CalculateRoute(startPos, finalPos, result), ()); Route route(router.GetName()); @@ -35,24 +40,246 @@ void TestAStarRouterMock(RoadPos const & startPos, RoadPos const & finalPos, TEST_EQUAL(expected, route.GetPoly(), ()); } +void AddRoad(RoadGraphMockSource & graph, double speedKMPH, initializer_list const & points) +{ + graph.AddRoad(IRoadGraph::RoadInfo(true /* bidir */, speedKMPH, points)); +} + +void AddRoad(RoadGraphMockSource & graph, initializer_list const & points) +{ + double const speedKMPH = graph.GetMaxSpeedKMPH(); + graph.AddRoad(IRoadGraph::RoadInfo(true /* bidir */, speedKMPH, points)); +} + +} // namespace + UNIT_TEST(AStarRouter_Graph2_Simple1) { - classificator::Load(); + Junction const startPos = m2::PointD(0, 0); + Junction const finalPos = m2::PointD(80, 55); - RoadPos startPos(1, true /* forward */, 0, m2::PointD(0, 0)); - RoadPos finalPos(7, true /* forward */, 0, m2::PointD(80, 55)); + m2::PolylineD const expected = {m2::PointD(0, 0), m2::PointD(5, 10), m2::PointD(5, 40), + m2::PointD(18, 55), m2::PointD(39, 55), m2::PointD(80, 55)}; - m2::PolylineD expected = {m2::PointD(0, 0), m2::PointD(5, 10), m2::PointD(5, 40), - m2::PointD(18, 55), m2::PointD(39, 55), m2::PointD(80, 55)}; TestAStarRouterMock(startPos, finalPos, expected); } UNIT_TEST(AStarRouter_Graph2_Simple2) { - RoadPos startPos(7, false /* forward */, 0, m2::PointD(80, 55)); - RoadPos finalPos(0, true /* forward */, 4, m2::PointD(80, 0)); - m2::PolylineD expected = {m2::PointD(80, 55), m2::PointD(39, 55), m2::PointD(37, 30), - m2::PointD(70, 30), m2::PointD(70, 10), m2::PointD(70, 0), - m2::PointD(80, 0)}; + Junction const startPos = m2::PointD(80, 55); + Junction const finalPos = m2::PointD(80, 0); + + m2::PolylineD const expected = {m2::PointD(80, 55), m2::PointD(39, 55), m2::PointD(37, 30), + m2::PointD(70, 30), m2::PointD(70, 10), m2::PointD(70, 0), + m2::PointD(80, 0)}; + TestAStarRouterMock(startPos, finalPos, expected); } + +UNIT_TEST(AStarRouter_SimpleGraph_RouteIsFound) +{ + classificator::Load(); + + AStarRouter router([](m2::PointD const & /*point*/) + { + return "Dummy_map.mwm"; + }); + { + unique_ptr graph(new RoadGraphMockSource()); + + AddRoad(*graph, {m2::PointD(0, 0), m2::PointD(40, 0)}); // feature 0 + AddRoad(*graph, {m2::PointD(40, 0), m2::PointD(40, 30)}); // feature 1 + AddRoad(*graph, {m2::PointD(40, 30), m2::PointD(40, 100)}); // feature 2 + AddRoad(*graph, {m2::PointD(40, 100), m2::PointD(0, 60)}); // feature 3 + AddRoad(*graph, {m2::PointD(0, 60), m2::PointD(0, 30)}); // feature 4 + AddRoad(*graph, {m2::PointD(0, 30), m2::PointD(0, 0)}); // feature 5 + + router.SetRoadGraph(move(graph)); + } + + Junction const start = m2::PointD(0, 0); + Junction const finish = m2::PointD(40, 100); + + m2::PolylineD const expected = {m2::PointD(0, 0), m2::PointD(0, 30), m2::PointD(0, 60), m2::PointD(40, 100)}; + + vector result; + TEST_EQUAL(IRouter::NoError, router.CalculateRoute(start, finish, result), ()); + + Route route(router.GetName()); + router.GetGraph()->ReconstructPath(result, route); + TEST_EQUAL(expected, route.GetPoly(), ()); +} + +UNIT_TEST(AStarRouter_SimpleGraph_RoutesInConnectedComponents) +{ + classificator::Load(); + + unique_ptr graph(new RoadGraphMockSource()); + + double const speedKMPH = graph->GetMaxSpeedKMPH(); + + // Roads in the first connected component. + vector const roadInfo_1 = + { + IRoadGraph::RoadInfo(true /* bidir */, speedKMPH, {m2::PointD(10, 10), m2::PointD(90, 10)}), // feature 0 + IRoadGraph::RoadInfo(true /* bidir */, speedKMPH, {m2::PointD(90, 10), m2::PointD(90, 90)}), // feature 1 + IRoadGraph::RoadInfo(true /* bidir */, speedKMPH, {m2::PointD(90, 90), m2::PointD(10, 90)}), // feature 2 + IRoadGraph::RoadInfo(true /* bidir */, speedKMPH, {m2::PointD(10, 90), m2::PointD(10, 10)}), // feature 3 + }; + vector const featureId_1 = { 0, 1, 2, 3 }; // featureIDs in the first connected component + + // Roads in the second connected component. + vector const roadInfo_2 = + { + IRoadGraph::RoadInfo(true /* bidir */, speedKMPH, {m2::PointD(30, 30), m2::PointD(70, 30)}), // feature 4 + IRoadGraph::RoadInfo(true /* bidir */, speedKMPH, {m2::PointD(70, 30), m2::PointD(70, 70)}), // feature 5 + IRoadGraph::RoadInfo(true /* bidir */, speedKMPH, {m2::PointD(70, 70), m2::PointD(30, 70)}), // feature 6 + IRoadGraph::RoadInfo(true /* bidir */, speedKMPH, {m2::PointD(30, 70), m2::PointD(30, 30)}), // feature 7 + }; + vector const featureId_2 = { 4, 5, 6, 7 }; // featureIDs in the second connected component + + for (auto const & ri : roadInfo_1) + graph->AddRoad(IRoadGraph::RoadInfo(ri)); + + for (auto const & ri : roadInfo_2) + graph->AddRoad(IRoadGraph::RoadInfo(ri)); + + AStarRouter router([](m2::PointD const & /*point*/) + { + return "Dummy_map.mwm"; + }); + router.SetRoadGraph(move(graph)); + + // In this test we check that there is no any route between pairs from different connected components, + // but there are routes between points in one connected component. + + // Check if there is no any route between points in different connected components. + for (size_t i = 0; i < roadInfo_1.size(); ++i) + { + Junction const start(roadInfo_1[i].m_points[0]); + for (size_t j = 0; j < roadInfo_2.size(); ++j) + { + Junction const finish(roadInfo_2[j].m_points[0]); + vector route; + TEST_EQUAL(IRouter::RouteNotFound, router.CalculateRoute(start, finish, route), ()); + TEST_EQUAL(IRouter::RouteNotFound, router.CalculateRoute(finish, start, route), ()); + } + } + + // Check if there is route between points in the first connected component. + for (size_t i = 0; i < roadInfo_1.size(); ++i) + { + Junction const start(roadInfo_1[i].m_points[0]); + for (size_t j = i + 1; j < roadInfo_1.size(); ++j) + { + Junction const finish(roadInfo_1[j].m_points[0]); + vector route; + TEST_EQUAL(IRouter::NoError, router.CalculateRoute(start, finish, route), ()); + TEST_EQUAL(IRouter::NoError, router.CalculateRoute(finish, start, route), ()); + } + } + + // Check if there is route between points in the second connected component. + for (size_t i = 0; i < roadInfo_2.size(); ++i) + { + Junction const start(roadInfo_2[i].m_points[0]); + for (size_t j = i + 1; j < roadInfo_2.size(); ++j) + { + Junction const finish(roadInfo_2[j].m_points[0]); + vector route; + TEST_EQUAL(IRouter::NoError, router.CalculateRoute(start, finish, route), ()); + TEST_EQUAL(IRouter::NoError, router.CalculateRoute(finish, start, route), ()); + } + } +} + +UNIT_TEST(AStarRouter_SimpleGraph_PickTheFasterRoad1) +{ + classificator::Load(); + + AStarRouter router([](m2::PointD const & /*point*/) + { + return "Dummy_map.mwm"; + }); + { + unique_ptr graph(new RoadGraphMockSource()); + + AddRoad(*graph, 5.0, {m2::PointD(2,1), m2::PointD(2,2), m2::PointD(2,3)}); + AddRoad(*graph, 5.0, {m2::PointD(10,1), m2::PointD(10,2), m2::PointD(10,3)}); + + AddRoad(*graph, 5.0, {m2::PointD(2,3), m2::PointD(4,3), m2::PointD(6,3), m2::PointD(8,3), m2::PointD(10,3)}); + AddRoad(*graph, 3.0, {m2::PointD(2,2), m2::PointD(6,2), m2::PointD(10,2)}); + AddRoad(*graph, 4.0, {m2::PointD(2,1), m2::PointD(10,1)}); + + router.SetRoadGraph(move(graph)); + } + + // path1 = 1/5 + 8/5 + 1/5 = 2 + // path2 = 8/3 = 2.666(6) + // path3 = 1/5 + 8/4 + 1/5 = 2.4 + + vector route; + TEST_EQUAL(IRouter::NoError, router.CalculateRoute(m2::PointD(2,2), m2::PointD(10,2), route), ()); + TEST_EQUAL(route, vector({m2::PointD(2,2), m2::PointD(2,3), m2::PointD(4,3), m2::PointD(6,3), + m2::PointD(8,3), m2::PointD(10,3), m2::PointD(10,2)}), ()); +} + +UNIT_TEST(AStarRouter_SimpleGraph_PickTheFasterRoad2) +{ + classificator::Load(); + + AStarRouter router([](m2::PointD const & /*point*/) + { + return "Dummy_map.mwm"; + }); + { + unique_ptr graph(new RoadGraphMockSource()); + + AddRoad(*graph, 5.0, {m2::PointD(2,1), m2::PointD(2,2), m2::PointD(2,3)}); + AddRoad(*graph, 5.0, {m2::PointD(10,1), m2::PointD(10,2), m2::PointD(10,3)}); + + AddRoad(*graph, 5.0, {m2::PointD(2,3), m2::PointD(4,3), m2::PointD(6,3), m2::PointD(8,3), m2::PointD(10,3)}); + AddRoad(*graph, 4.1, {m2::PointD(2,2), m2::PointD(6,2), m2::PointD(10,2)}); + AddRoad(*graph, 4.4, {m2::PointD(2,1), m2::PointD(10,1)}); + + router.SetRoadGraph(move(graph)); + } + + // path1 = 1/5 + 8/5 + 1/5 = 2 + // path2 = 8/4.1 = 1.95 + // path3 = 1/5 + 8/4.4 + 1/5 = 2.2 + + vector route; + TEST_EQUAL(IRouter::NoError, router.CalculateRoute(m2::PointD(2,2), m2::PointD(10,2), route), ()); + TEST_EQUAL(route, vector({m2::PointD(2,2), m2::PointD(6,2), m2::PointD(10,2)}), ()); +} + +UNIT_TEST(AStarRouter_SimpleGraph_PickTheFasterRoad3) +{ + classificator::Load(); + + AStarRouter router([](m2::PointD const & /*point*/) + { + return "Dummy_map.mwm"; + }); + { + unique_ptr graph(new RoadGraphMockSource()); + + AddRoad(*graph, 5.0, {m2::PointD(2,1), m2::PointD(2,2), m2::PointD(2,3)}); + AddRoad(*graph, 5.0, {m2::PointD(10,1), m2::PointD(10,2), m2::PointD(10,3)}); + + AddRoad(*graph, 4.8, {m2::PointD(2,3), m2::PointD(4,3), m2::PointD(6,3), m2::PointD(8,3), m2::PointD(10,3)}); + AddRoad(*graph, 3.9, {m2::PointD(2,2), m2::PointD(6,2), m2::PointD(10,2)}); + AddRoad(*graph, 4.9, {m2::PointD(2,1), m2::PointD(10,1)}); + + router.SetRoadGraph(move(graph)); + } + + // path1 = 1/5 + 8/4.8 + 1/5 = 2.04 + // path2 = 8/3.9 = 2.05 + // path3 = 1/5 + 8/4.9 + 1/5 = 2.03 + + vector route; + TEST_EQUAL(IRouter::NoError, router.CalculateRoute(m2::PointD(2,2), m2::PointD(10,2), route), ()); + TEST_EQUAL(route, vector({m2::PointD(2,2), m2::PointD(2,1), m2::PointD(10,1), m2::PointD(10,2)}), ()); +} diff --git a/routing/routing_tests/features_road_graph_test.hpp b/routing/routing_tests/features_road_graph_test.hpp deleted file mode 100644 index ef64150d26..0000000000 --- a/routing/routing_tests/features_road_graph_test.hpp +++ /dev/null @@ -1,46 +0,0 @@ -#pragma once - -#include "routing/features_road_graph.hpp" - -#include "indexer/index.hpp" - -#include "std/scoped_ptr.hpp" - -namespace routing_test -{ -class Name2IdMapping -{ - map m_name2Id; - map m_id2Name; - -public: - void operator()(FeatureType const & ft); - - uint32_t GetId(string const & name); - - string const & GetName(uint32_t id); -}; - -class FeatureRoadGraphTester -{ - unique_ptr m_graph; - Name2IdMapping m_mapping; - Index m_index; - - void FeatureID2Name(routing::RoadPos & pos); - -public: - FeatureRoadGraphTester(string const & name); - - unique_ptr StealGraph() { return move(m_graph); } - - void FeatureID2Name(routing::IRoadGraph::TurnsVectorT & vec); - void FeatureID2Name(vector & vec); - void Name2FeatureID(vector & vec); - - void GetPossibleTurns(routing::RoadPos const & pos, routing::IRoadGraph::TurnsVectorT & vec); - - template - void ReconstructPath(routing::RoadPos(&arr)[N], vector & vec); -}; -} diff --git a/routing/routing_tests/nearest_edge_finder_tests.cpp b/routing/routing_tests/nearest_edge_finder_tests.cpp new file mode 100644 index 0000000000..94c1f9b3bb --- /dev/null +++ b/routing/routing_tests/nearest_edge_finder_tests.cpp @@ -0,0 +1,53 @@ +#include "testing/testing.hpp" + +#include "routing/routing_tests/road_graph_builder.hpp" + +#include "routing/nearest_edge_finder.hpp" + +using namespace routing; +using namespace routing_test; + +void TestNearestOnMock1(m2::PointD const & point, size_t const candidatesCount, + vector> const & expected) +{ + unique_ptr graph(new RoadGraphMockSource()); + InitRoadGraphMockSourceWithTest1(*graph); + + NearestEdgeFinder finder(point, *graph.get()); + for (size_t i = 0; i < graph->GetRoadCount(); ++i) + finder.AddInformationSource(i); + + vector> result; + TEST(finder.HasCandidates(), ()); + finder.MakeResult(result, candidatesCount); + + TEST_EQUAL(result, expected, ()); +} + +UNIT_TEST(StarterPosAtBorder_Mock1Graph) +{ + vector> const expected = + { + make_pair(Edge(0, true /* forward */, 0, m2::PointD(0, 0), m2::PointD(5, 0)), m2::PointD(0, 0)) + }; + TestNearestOnMock1(m2::PointD(0, 0), 1, expected); +} + +UNIT_TEST(MiddleEdgeTest_Mock1Graph) +{ + vector> const expected = + { + make_pair(Edge(0, true /* forward */, 0, m2::PointD(0, 0), m2::PointD(5, 0)), m2::PointD(3, 0)) + }; + TestNearestOnMock1(m2::PointD(3, 3), 1, expected); +} + +UNIT_TEST(MiddleSegmentTest_Mock1Graph) +{ + vector> const expected = + { + make_pair(Edge(0, true /* forward */, 2, m2::PointD(10, 0), m2::PointD(15, 0)), m2::PointD(12.5, 0)), + make_pair(Edge(1, true /* forward */, 2, m2::PointD(10, 0), m2::PointD(10, 5)), m2::PointD(10, 2.5)) + }; + TestNearestOnMock1(m2::PointD(12.5, 2.5), 2, expected); +} diff --git a/routing/routing_tests/nearest_road_pos_finder_tests.cpp b/routing/routing_tests/nearest_road_pos_finder_tests.cpp deleted file mode 100644 index 697ff159ca..0000000000 --- a/routing/routing_tests/nearest_road_pos_finder_tests.cpp +++ /dev/null @@ -1,51 +0,0 @@ -#include "testing/testing.hpp" - -#include "routing/routing_tests/road_graph_builder.hpp" -#include "routing/routing_tests/features_road_graph_test.hpp" - -#include "routing/nearest_road_pos_finder.hpp" - -using namespace routing; -using namespace routing_test; - -void TestNearestOnMock1(m2::PointD const & point, size_t const candidatesCount, - vector const & expected) -{ - unique_ptr graph(new RoadGraphMockSource()); - InitRoadGraphMockSourceWithTest1(*graph); - NearestRoadPosFinder finder(point, m2::PointD::Zero(), *graph.get()); - for (size_t i = 0; i < 4; ++i) - finder.AddInformationSource(i); - vector result; - TEST(finder.HasCandidates(), ()); - finder.MakeResult(result, candidatesCount); - - for (RoadPos const & pos : expected) - TEST(find(result.begin(), result.end(), pos) != result.end(), ()); -} - -UNIT_TEST(StarterPosAtBorder_Mock1Graph) -{ - RoadPos pos1(0, true /* forward */, 0, m2::PointD(0, 0)); - RoadPos pos2(0, false /* backward */, 0, m2::PointD(0, 0)); - vector expected = {pos1, pos2}; - TestNearestOnMock1(m2::PointD(0, 0), 2, expected); -} - -UNIT_TEST(MiddleEdgeTest_Mock1Graph) -{ - RoadPos pos1(0, false /* backward */, 0, m2::PointD(3, 0)); - RoadPos pos2(0, true /* forward */, 0, m2::PointD(3, 0)); - vector expected = {pos1, pos2}; - TestNearestOnMock1(m2::PointD(3, 3), 2, expected); -} - -UNIT_TEST(MiddleSegmentTest_Mock1Graph) -{ - RoadPos pos1(0, false /* backward */, 2, m2::PointD(13, 0)); - RoadPos pos2(0, true /* forward */, 2, m2::PointD(13, 0)); - RoadPos pos3(3, false /* backward */, 1, m2::PointD(15, 5)); - RoadPos pos4(3, true /* forward */, 1, m2::PointD(15, 5)); - vector expected = {pos1, pos2, pos3, pos4}; - TestNearestOnMock1(m2::PointD(13, 3), 4, expected); -} diff --git a/routing/routing_tests/road_graph_builder.cpp b/routing/routing_tests/road_graph_builder.cpp index d59857bee6..3570f28f1f 100644 --- a/routing/routing_tests/road_graph_builder.cpp +++ b/routing/routing_tests/road_graph_builder.cpp @@ -1,37 +1,24 @@ #include "road_graph_builder.hpp" -#include "../../base/logging.hpp" +#include "base/logging.hpp" -#include "../../std/algorithm.hpp" +#include "std/algorithm.hpp" using namespace routing; +namespace +{ + +double const MAX_SPEED_KMPH = 5.0; + +} // namespace + namespace routing_test { + void RoadGraphMockSource::AddRoad(RoadInfo && ri) { - /// @todo Do CHECK for RoadInfo params. - uint32_t const roadId = m_roads.size(); - CHECK_GREATER_OR_EQUAL(ri.m_points.size(), 2, ("Empty road")); - size_t const numSegments = ri.m_points.size() - 1; - - for (size_t segId = 0; segId < numSegments; ++segId) - { - PossibleTurn t; - t.m_startPoint = ri.m_points.front(); - t.m_endPoint = ri.m_points.back(); - t.m_speedKMPH = ri.m_speedKMPH; - - t.m_pos = RoadPos(roadId, true /* forward */, segId, ri.m_points[segId + 1] /* segEndPoint */); - m_turns[t.m_pos.GetSegEndpoint()].push_back(t); - if (ri.m_bidirectional) - { - t.m_pos = RoadPos(roadId, false /* forward */, segId, ri.m_points[segId] /* segEndPoint */); - m_turns[t.m_pos.GetSegEndpoint()].push_back(t); - } - } - m_roads.push_back(move(ri)); } @@ -47,18 +34,25 @@ double RoadGraphMockSource::GetSpeedKMPH(uint32_t featureId) return m_roads[featureId].m_speedKMPH; } +double RoadGraphMockSource::GetMaxSpeedKMPH() +{ + return MAX_SPEED_KMPH; +} + void RoadGraphMockSource::ForEachFeatureClosestToCross(m2::PointD const & /* cross */, - CrossTurnsLoader & turnsLoader) + CrossEdgesLoader & edgesLoader) { for (size_t roadId = 0; roadId < m_roads.size(); ++roadId) - turnsLoader(roadId, m_roads[roadId]); + edgesLoader(roadId, m_roads[roadId]); } void InitRoadGraphMockSourceWithTest1(RoadGraphMockSource & src) { + double const speedKMPH = MAX_SPEED_KMPH; + IRoadGraph::RoadInfo ri0; ri0.m_bidirectional = true; - ri0.m_speedKMPH = 40; + ri0.m_speedKMPH = speedKMPH; ri0.m_points.push_back(m2::PointD(0, 0)); ri0.m_points.push_back(m2::PointD(5, 0)); ri0.m_points.push_back(m2::PointD(10, 0)); @@ -67,7 +61,7 @@ void InitRoadGraphMockSourceWithTest1(RoadGraphMockSource & src) IRoadGraph::RoadInfo ri1; ri1.m_bidirectional = true; - ri1.m_speedKMPH = 40; + ri1.m_speedKMPH = speedKMPH; ri1.m_points.push_back(m2::PointD(10, -10)); ri1.m_points.push_back(m2::PointD(10, -5)); ri1.m_points.push_back(m2::PointD(10, 0)); @@ -76,13 +70,13 @@ void InitRoadGraphMockSourceWithTest1(RoadGraphMockSource & src) IRoadGraph::RoadInfo ri2; ri2.m_bidirectional = true; - ri2.m_speedKMPH = 40; + ri2.m_speedKMPH = speedKMPH; ri2.m_points.push_back(m2::PointD(15, -5)); ri2.m_points.push_back(m2::PointD(15, 0)); IRoadGraph::RoadInfo ri3; ri3.m_bidirectional = true; - ri3.m_speedKMPH = 40; + ri3.m_speedKMPH = speedKMPH; ri3.m_points.push_back(m2::PointD(20, 0)); ri3.m_points.push_back(m2::PointD(25, 5)); ri3.m_points.push_back(m2::PointD(15, 5)); @@ -96,9 +90,11 @@ void InitRoadGraphMockSourceWithTest1(RoadGraphMockSource & src) void InitRoadGraphMockSourceWithTest2(RoadGraphMockSource & graph) { + double const speedKMPH = MAX_SPEED_KMPH; + IRoadGraph::RoadInfo ri0; ri0.m_bidirectional = true; - ri0.m_speedKMPH = 40; + ri0.m_speedKMPH = speedKMPH; ri0.m_points.push_back(m2::PointD(0, 0)); ri0.m_points.push_back(m2::PointD(10, 0)); ri0.m_points.push_back(m2::PointD(25, 0)); @@ -108,21 +104,21 @@ void InitRoadGraphMockSourceWithTest2(RoadGraphMockSource & graph) IRoadGraph::RoadInfo ri1; ri1.m_bidirectional = true; - ri1.m_speedKMPH = 40; + ri1.m_speedKMPH = speedKMPH; ri1.m_points.push_back(m2::PointD(0, 0)); ri1.m_points.push_back(m2::PointD(5, 10)); ri1.m_points.push_back(m2::PointD(5, 40)); IRoadGraph::RoadInfo ri2; ri2.m_bidirectional = true; - ri2.m_speedKMPH = 40; + ri2.m_speedKMPH = speedKMPH; ri2.m_points.push_back(m2::PointD(12, 25)); ri2.m_points.push_back(m2::PointD(10, 10)); ri2.m_points.push_back(m2::PointD(10, 0)); IRoadGraph::RoadInfo ri3; ri3.m_bidirectional = true; - ri3.m_speedKMPH = 40; + ri3.m_speedKMPH = speedKMPH; ri3.m_points.push_back(m2::PointD(5, 10)); ri3.m_points.push_back(m2::PointD(10, 10)); ri3.m_points.push_back(m2::PointD(70, 10)); @@ -130,13 +126,13 @@ void InitRoadGraphMockSourceWithTest2(RoadGraphMockSource & graph) IRoadGraph::RoadInfo ri4; ri4.m_bidirectional = true; - ri4.m_speedKMPH = 40; + ri4.m_speedKMPH = speedKMPH; ri4.m_points.push_back(m2::PointD(25, 0)); ri4.m_points.push_back(m2::PointD(27, 25)); IRoadGraph::RoadInfo ri5; ri5.m_bidirectional = true; - ri5.m_speedKMPH = 40; + ri5.m_speedKMPH = speedKMPH; ri5.m_points.push_back(m2::PointD(35, 0)); ri5.m_points.push_back(m2::PointD(37, 30)); ri5.m_points.push_back(m2::PointD(70, 30)); @@ -144,20 +140,20 @@ void InitRoadGraphMockSourceWithTest2(RoadGraphMockSource & graph) IRoadGraph::RoadInfo ri6; ri6.m_bidirectional = true; - ri6.m_speedKMPH = 40; + ri6.m_speedKMPH = speedKMPH; ri6.m_points.push_back(m2::PointD(70, 0)); ri6.m_points.push_back(m2::PointD(70, 10)); ri6.m_points.push_back(m2::PointD(70, 30)); IRoadGraph::RoadInfo ri7; ri7.m_bidirectional = true; - ri7.m_speedKMPH = 40; + ri7.m_speedKMPH = speedKMPH; ri7.m_points.push_back(m2::PointD(39, 55)); ri7.m_points.push_back(m2::PointD(80, 55)); IRoadGraph::RoadInfo ri8; ri8.m_bidirectional = true; - ri8.m_speedKMPH = 40; + ri8.m_speedKMPH = speedKMPH; ri8.m_points.push_back(m2::PointD(5, 40)); ri8.m_points.push_back(m2::PointD(18, 55)); ri8.m_points.push_back(m2::PointD(39, 55)); diff --git a/routing/routing_tests/road_graph_builder.hpp b/routing/routing_tests/road_graph_builder.hpp index c721206b8b..4082b90c56 100644 --- a/routing/routing_tests/road_graph_builder.hpp +++ b/routing/routing_tests/road_graph_builder.hpp @@ -4,37 +4,23 @@ namespace routing_test { + class RoadGraphMockSource : public routing::IRoadGraph { - vector m_roads; - - struct LessPoint - { - bool operator()(m2::PointD const & p1, m2::PointD const & p2) const - { - double const eps = 1.0E-6; - - if (p1.x + eps < p2.x) - return true; - else if (p1.x > p2.x + eps) - return false; - - return (p1.y + eps < p2.y); - } - }; - - typedef vector TurnsVectorT; - typedef map TurnsMapT; - TurnsMapT m_turns; - public: void AddRoad(RoadInfo && ri); + inline size_t GetRoadCount() const { return m_roads.size(); } + // routing::IRoadGraph overrides: RoadInfo GetRoadInfo(uint32_t featureId) override; double GetSpeedKMPH(uint32_t featureId) override; + double GetMaxSpeedKMPH() override; void ForEachFeatureClosestToCross(m2::PointD const & cross, - CrossTurnsLoader & turnsLoader) override; + CrossEdgesLoader & edgeLoader) override; + +private: + vector m_roads; }; void InitRoadGraphMockSourceWithTest1(RoadGraphMockSource & graphMock); diff --git a/routing/routing_tests/road_graph_nearest_edges_test.cpp b/routing/routing_tests/road_graph_nearest_edges_test.cpp new file mode 100644 index 0000000000..b4e8d72ed4 --- /dev/null +++ b/routing/routing_tests/road_graph_nearest_edges_test.cpp @@ -0,0 +1,90 @@ +#include "testing/testing.hpp" + +#include "routing/road_graph.hpp" +#include "routing/routing_tests/road_graph_builder.hpp" +#include "std/algorithm.hpp" + +namespace routing_test +{ + +using namespace routing; + +UNIT_TEST(RoadGraph_NearestEdges) +{ + // ^ 1st road + // o + // | + // | + // o + // | + // | 0th road + // o--o--x--o--o > + // | + // | + // o + // | + // | + // o + // + // Two roads are intersecting in (0, 0). + RoadGraphMockSource graph; + { + IRoadGraph::RoadInfo ri0; + ri0.m_points.emplace_back(-2, 0); + ri0.m_points.emplace_back(-1, 0); + ri0.m_points.emplace_back(0, 0); + ri0.m_points.emplace_back(1, 0); + ri0.m_points.emplace_back(2, 0); + ri0.m_speedKMPH = 5; + ri0.m_bidirectional = true; + + IRoadGraph::RoadInfo ri1; + ri1.m_points.emplace_back(0, -2); + ri1.m_points.emplace_back(0, -1); + ri1.m_points.emplace_back(0, 0); + ri1.m_points.emplace_back(0, 1); + ri1.m_points.emplace_back(0, 2); + ri1.m_speedKMPH = 5; + ri1.m_bidirectional = true; + + graph.AddRoad(move(ri0)); + graph.AddRoad(move(ri1)); + } + + // We are standing at x junction. + Junction const crossPos(m2::PointD(0, 0)); + + // Expected outgoing edges. + IRoadGraph::TEdgeVector expectedOutgoing = + { + Edge(0 /* first road */, false /* forward */, 1 /* segId */, m2::PointD(0, 0), m2::PointD(-1, 0)), + Edge(0 /* first road */, true /* forward */, 2 /* segId */, m2::PointD(0, 0), m2::PointD(1, 0)), + Edge(1 /* second road */, false /* forward */, 1 /* segId */, m2::PointD(0, 0), m2::PointD(0, -1)), + Edge(1 /* second road */, true /* forward */, 2 /* segId */, m2::PointD(0, 0), m2::PointD(0, 1)), + }; + sort(expectedOutgoing.begin(), expectedOutgoing.end()); + + // Expected ingoing edges. + IRoadGraph::TEdgeVector expectedIngoing = + { + Edge(0 /* first road */, true /* forward */, 1 /* segId */, m2::PointD(-1, 0), m2::PointD(0, 0)), + Edge(0 /* first road */, false /* forward */, 2 /* segId */, m2::PointD(1, 0), m2::PointD(0, 0)), + Edge(1 /* second road */, true /* forward */, 1 /* segId */, m2::PointD(0, -1), m2::PointD(0, 0)), + Edge(1 /* second road */, false /* forward */, 2 /* segId */, m2::PointD(0, 1), m2::PointD(0, 0)), + }; + sort(expectedIngoing.begin(), expectedIngoing.end()); + + // Check outgoing edges. + IRoadGraph::TEdgeVector actualOutgoing; + graph.GetOutgoingEdges(crossPos, actualOutgoing); + sort(actualOutgoing.begin(), actualOutgoing.end()); + TEST_EQUAL(expectedOutgoing, actualOutgoing, ()); + + // Check ingoing edges. + IRoadGraph::TEdgeVector actualIngoing; + graph.GetIngoingEdges(crossPos, actualIngoing); + sort(actualIngoing.begin(), actualIngoing.end()); + TEST_EQUAL(expectedIngoing, actualIngoing, ()); +} + +} // namespace routing_test diff --git a/routing/routing_tests/road_graph_nearest_turns_test.cpp b/routing/routing_tests/road_graph_nearest_turns_test.cpp deleted file mode 100644 index cddde57ef2..0000000000 --- a/routing/routing_tests/road_graph_nearest_turns_test.cpp +++ /dev/null @@ -1,102 +0,0 @@ -#include "testing/testing.hpp" - -#include "routing/road_graph.hpp" -#include "routing/routing_tests/road_graph_builder.hpp" -#include "std/algorithm.hpp" - -namespace routing -{ -UNIT_TEST(RoadGraph_NearestTurns) -{ - // 1st road - // o - // | - // | - // o - // | - // | 0th road - // o--o--x--o--o - // | - // | - // o - // | - // | - // o - // - // Two roads intersecting at (0, 0). - routing_test::RoadGraphMockSource graph; - { - IRoadGraph::RoadInfo ri0; - ri0.m_points.emplace_back(-2, 0); - ri0.m_points.emplace_back(-1, 0); - ri0.m_points.emplace_back(0, 0); - ri0.m_points.emplace_back(1, 0); - ri0.m_points.emplace_back(2, 0); - ri0.m_speedKMPH = 5; - ri0.m_bidirectional = true; - - IRoadGraph::RoadInfo ri1; - ri1.m_points.emplace_back(0, -2); - ri1.m_points.emplace_back(0, -1); - ri1.m_points.emplace_back(0, 0); - ri1.m_points.emplace_back(0, 1); - ri1.m_points.emplace_back(0, 2); - ri1.m_speedKMPH = 5; - ri1.m_bidirectional = true; - - graph.AddRoad(move(ri0)); - graph.AddRoad(move(ri1)); - } - - // We are standing at ... x--o ... segment and are looking to the - // right. - RoadPos const crossPos(0 /* featureId */, true /* forward */, 2 /* segId */, m2::PointD(0, 0)); - IRoadGraph::RoadPosVectorT expected = { - // It is possible to get to the RoadPos we are standing at from - // RoadPos'es on the first road marked with >> and <<. - // - // ... - // | - // ...--o>>x< const & types, bool expectedValue) TEST_EQUAL(vehicleModel.IsOneWay(h), expectedValue, ()); } -} +} // namespace UNIT_TEST(VehicleModel_MaxSpeed) { diff --git a/routing/vehicle_model.cpp b/routing/vehicle_model.cpp index 6455cfdd7d..3d1fbe56d8 100644 --- a/routing/vehicle_model.cpp +++ b/routing/vehicle_model.cpp @@ -170,9 +170,9 @@ double PedestrianModel::GetSpeed(FeatureType const & f) const feature::TypesHolder types(f); if (IsFoot(types) && IsRoad(types)) - return m_maxSpeedKMpH; + return VehicleModel::GetSpeed(f); else return 0.0; } -} +} // namespace routing diff --git a/routing/vehicle_model.hpp b/routing/vehicle_model.hpp index c3cafcc2e1..1f084dbc44 100644 --- a/routing/vehicle_model.hpp +++ b/routing/vehicle_model.hpp @@ -96,4 +96,4 @@ public: //@} }; -} +} // namespace routing