diff --git a/routing/features_road_graph.cpp b/routing/features_road_graph.cpp index a7b5992f46..f06969680d 100644 --- a/routing/features_road_graph.cpp +++ b/routing/features_road_graph.cpp @@ -17,9 +17,6 @@ namespace { uint32_t const FEATURE_CACHE_SIZE = 10; double const READ_CROSS_EPSILON = 1.0E-4; - -bool IsStart(RoadPos const & rp) { return rp.GetFeatureId() == RoadPos::kFakeStartFeatureId; } -bool IsFinal(RoadPos const & rp) { return rp.GetFeatureId() == RoadPos::kFakeFinalFeatureId; } } // namespace /// @todo Factor out vehicle model as a parameter for the features graph. @@ -37,20 +34,13 @@ uint32_t FeaturesRoadGraph::GetStreetReadScale() { return scales::GetUpperScale( class CrossFeaturesLoader { - FeaturesRoadGraph & m_graph; - m2::PointD m_point; - IRoadGraph::TurnsVectorT & m_turns; - size_t m_count; - public: - CrossFeaturesLoader(FeaturesRoadGraph & graph, m2::PointD const & pt, - IRoadGraph::TurnsVectorT & turns) - : m_graph(graph), m_point(pt), m_turns(turns), m_count(0) + CrossFeaturesLoader(FeaturesRoadGraph & graph, m2::PointD const & point, + IRoadGraph::CrossTurnsLoader & turnsLoader) + : m_graph(graph), m_point(point), m_turnsLoader(turnsLoader) { } - size_t GetCount() const { return m_count; } - void operator()(FeatureType & ft) { FeatureID const fID = ft.GetID(); @@ -72,36 +62,13 @@ public: IRoadGraph::RoadInfo const & ri = m_graph.GetCachedRoadInfo(fID.m_offset, ft, false); ASSERT_EQUAL(speed, ri.m_speedKMPH, ()); - size_t const count = ri.m_points.size(); - - PossibleTurn t; - t.m_speedKMPH = ri.m_speedKMPH; - t.m_startPoint = ri.m_points[0]; - t.m_endPoint = ri.m_points[count - 1]; - - for (size_t i = 0; i < count; ++i) - { - m2::PointD const & p = ri.m_points[i]; - - /// @todo Is this a correct way to compare? - if (!m2::AlmostEqual(m_point, p)) - continue; - - if (i > 0) - { - ++m_count; - t.m_pos = RoadPos(fID.m_offset, true, i - 1, p); - m_turns.push_back(t); - } - - if (i < count - 1) - { - ++m_count; - t.m_pos = RoadPos(fID.m_offset, false, i, p); - m_turns.push_back(t); - } - } + m_turnsLoader(fID.m_offset, ri); } + +private: + FeaturesRoadGraph & m_graph; + m2::PointD m_point; + IRoadGraph::CrossTurnsLoader & m_turnsLoader; }; void FeaturesRoadGraph::LoadFeature(uint32_t featureId, FeatureType & ft) @@ -114,36 +81,26 @@ void FeaturesRoadGraph::LoadFeature(uint32_t featureId, FeatureType & ft) ASSERT_GREATER(ft.GetPointsCount(), 1, (featureId)); } -void FeaturesRoadGraph::GetNearestTurns(RoadPos const & pos, vector & turns) +IRoadGraph::RoadInfo FeaturesRoadGraph::GetRoadInfo(uint32_t featureId) { - if (IsStart(pos)) - { - turns.insert(turns.end(), m_startVicinityTurns.begin(), m_startVicinityTurns.end()); - return; - } - if (IsFinal(pos)) - { - turns.insert(turns.end(), m_finalVicinityTurns.begin(), m_finalVicinityTurns.end()); - return; - } - - uint32_t const featureId = pos.GetFeatureId(); FeatureType ft; - RoadInfo const ri = GetCachedRoadInfo(featureId, ft, true); + return GetCachedRoadInfo(featureId, ft, true); +} - if (ri.m_speedKMPH <= 0.0) - return; +double FeaturesRoadGraph::GetSpeedKMPH(uint32_t featureId) +{ + FeatureType ft; + LoadFeature(featureId, ft); + return GetSpeedKMPHFromFt(ft); +} - ASSERT_GREATER_OR_EQUAL(ri.m_points.size(), 2, - ("Incorrect road - only", ri.m_points.size(), "point(s).")); - - m2::PointD const point = ri.m_points[pos.GetSegStartPointId()]; - - // Find possible turns to startPoint from other features. - CrossFeaturesLoader crossLoader(*this, point, turns); - m_pIndex->ForEachInRect(crossLoader, - m2::RectD(point.x - READ_CROSS_EPSILON, point.y - READ_CROSS_EPSILON, - point.x + READ_CROSS_EPSILON, point.y + READ_CROSS_EPSILON), +void FeaturesRoadGraph::ForEachClosestToCrossFeature(m2::PointD const & cross, + CrossTurnsLoader & turnsLoader) +{ + CrossFeaturesLoader featuresLoader(*this, cross, turnsLoader); + 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), scales::GetUpperScale()); } @@ -157,13 +114,6 @@ double FeaturesRoadGraph::GetSpeedKMPHFromFt(FeatureType const & ft) const return m_vehicleModel->GetSpeed(ft); } -double FeaturesRoadGraph::GetSpeedKMPH(uint32_t featureId) -{ - FeatureType ft; - LoadFeature(featureId, ft); - return GetSpeedKMPHFromFt(ft); -} - IRoadGraph::RoadInfo const & FeaturesRoadGraph::GetCachedRoadInfo(uint32_t const ftId, FeatureType & ft, bool fullLoad) { diff --git a/routing/features_road_graph.hpp b/routing/features_road_graph.hpp index ae37d37914..abb5e6be4f 100644 --- a/routing/features_road_graph.hpp +++ b/routing/features_road_graph.hpp @@ -26,9 +26,6 @@ class FeaturesRoadGraph : public IRoadGraph public: FeaturesRoadGraph(Index const * pIndex, size_t mwmID); - // IRoadGraph overrides: - void GetNearestTurns(RoadPos const & pos, vector & turns) override; - static uint32_t GetStreetReadScale(); inline size_t GetMwmID() const { return m_mwmID; } @@ -40,13 +37,19 @@ public: return (double)m_cacheMiss / (double)m_cacheAccess; } +protected: + // IRoadGraph overrides: + RoadInfo GetRoadInfo(uint32_t featureId) override; + double GetSpeedKMPH(uint32_t featureId) override; + void ForEachClosestToCrossFeature(m2::PointD const & cross, + CrossTurnsLoader & turnsLoader) override; + private: friend class CrossFeaturesLoader; bool IsOneWay(FeatureType const & ft) const; double GetSpeedKMPHFromFt(FeatureType const & ft) const; - double GetSpeedKMPH(uint32_t featureId) override; void LoadFeature(uint32_t featureId, FeatureType & ft); diff --git a/routing/road_graph.cpp b/routing/road_graph.cpp index 135ccd6b62..edc4f93728 100644 --- a/routing/road_graph.cpp +++ b/routing/road_graph.cpp @@ -47,6 +47,45 @@ string DebugPrint(RoadPos const & r) // IRoadGraph ------------------------------------------------------------------ +IRoadGraph::CrossTurnsLoader::CrossTurnsLoader(m2::PointD const & cross, TurnsVectorT & turns) + : m_cross(cross), m_turns(turns) +{ +} + +void IRoadGraph::CrossTurnsLoader::operator()(uint32_t featureId, RoadInfo const & roadInfo) +{ + size_t const numPoints = roadInfo.m_points.size(); + + if (!numPoints) + return; + + 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)) + continue; + + if (i > 0) + { + turn.m_pos = RoadPos(featureId, true /* forward */, i - 1, p); + m_turns.push_back(turn); + } + + if (i < numPoints - 1) + { + turn.m_pos = RoadPos(featureId, false /* forward */, i, p); + m_turns.push_back(turn); + } + } +} + void IRoadGraph::ReconstructPath(RoadPosVectorT const & positions, Route & route) { CHECK(!positions.empty(), ("Can't reconstruct path from an empty list of positions.")); @@ -96,6 +135,61 @@ void IRoadGraph::ReconstructPath(RoadPosVectorT const & positions, Route & route route.SetSectionTimes(times); } +void IRoadGraph::GetNearestTurns(RoadPos const & pos, TurnsVectorT & turns) +{ + uint32_t const featureId = pos.GetFeatureId(); + + if (featureId == RoadPos::kFakeStartFeatureId) + { + turns.insert(turns.end(), m_startVicinityTurns.begin(), m_startVicinityTurns.end()); + return; + } + if (featureId == RoadPos::kFakeFinalFeatureId) + { + turns.insert(turns.end(), m_finalVicinityTurns.begin(), m_finalVicinityTurns.end()); + return; + } + + RoadInfo const roadInfo = GetRoadInfo(featureId); + + if (roadInfo.m_speedKMPH <= 0.0) + return; + + ASSERT_GREATER_OR_EQUAL(roadInfo.m_points.size(), 2, + ("Incorrect road - only", roadInfo.m_points.size(), "point(s).")); + + m2::PointD const & cross = roadInfo.m_points[pos.GetSegStartPointId()]; + + CrossTurnsLoader loader(cross, turns); + ForEachClosestToCrossFeature(cross, loader); +} + +void IRoadGraph::SetFakeTurns(RoadPos const & rp, vector const & vicinity) +{ + vector * turns = nullptr; + uint32_t const featureId = rp.GetFeatureId(); + switch (featureId) + { + case RoadPos::kFakeStartFeatureId: + turns = &m_startVicinityTurns; + break; + case RoadPos::kFakeFinalFeatureId: + turns = &m_finalVicinityTurns; + break; + default: + CHECK(false, ("Can't add fake turns from a valid road:", featureId)); + } + turns->clear(); + for (RoadPos const & vrp : vicinity) + { + 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); + } +} + // RoadGraph ------------------------------------------------------------------- RoadGraph::RoadGraph(IRoadGraph & roadGraph) : m_roadGraph(roadGraph) {} diff --git a/routing/road_graph.hpp b/routing/road_graph.hpp index 1a9f5118f3..642ca0c95f 100644 --- a/routing/road_graph.hpp +++ b/routing/road_graph.hpp @@ -122,16 +122,41 @@ public: bool m_bidirectional; }; + class CrossTurnsLoader + { + public: + CrossTurnsLoader(m2::PointD const & cross, TurnsVectorT & turns); + + void operator()(uint32_t featureId, RoadInfo const & roadInfo); + + private: + m2::PointD m_cross; + TurnsVectorT & m_turns; + }; + virtual ~IRoadGraph() = default; + /// Construct route by road positions (doesn't include first and last section). + void ReconstructPath(RoadPosVectorT const & positions, Route & route); + /// Finds all nearest feature sections (turns), that route to the /// "pos" section. - virtual void GetNearestTurns(RoadPos const & pos, TurnsVectorT & turns) = 0; + void GetNearestTurns(RoadPos const & pos, TurnsVectorT & turns); + /// Adds fake turns from fake position rp to real vicinity + /// positions. + void SetFakeTurns(RoadPos const & rp, vector const & vicinity); + +protected: + // 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. virtual double GetSpeedKMPH(uint32_t featureId) = 0; - /// Construct route by road positions (doesn't include first and last section). - virtual void ReconstructPath(RoadPosVectorT const & positions, Route & route); + // Calls turnsLoader on each feature which is close to cross. + virtual void ForEachClosestToCrossFeature(m2::PointD const & cross, + CrossTurnsLoader & turnsLoader) = 0; // 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 diff --git a/routing/road_graph_router.cpp b/routing/road_graph_router.cpp index a84c97280e..87b672086f 100644 --- a/routing/road_graph_router.cpp +++ b/routing/road_graph_router.cpp @@ -20,26 +20,6 @@ namespace { size_t const MAX_ROAD_CANDIDATES = 2; double const FEATURE_BY_POINT_RADIUS_M = 100.0; - -/// @todo Code duplication with road_graph.cpp -double TimeBetweenSec(m2::PointD const & v, m2::PointD const & w) -{ - static double const kMaxSpeedMPS = 5000.0 / 3600; - return MercatorBounds::DistanceOnEarth(v, w) / kMaxSpeedMPS; -} - -void AddFakeTurns(RoadPos const & rp, vector const & vicinity, - vector & turns) -{ - for (RoadPos const & vrp : vicinity) - { - PossibleTurn t; - t.m_pos = vrp; - /// @todo Do we need other fields? Do we even need m_secondsCovered? - t.m_secondsCovered = TimeBetweenSec(rp.GetSegEndpoint(), vrp.GetSegEndpoint()); - turns.push_back(t); - } -} } // namespace RoadGraphRouter::~RoadGraphRouter() @@ -96,8 +76,8 @@ IRouter::ResultCode RoadGraphRouter::CalculateRoute(m2::PointD const & startPoin RoadPos startPos(RoadPos::kFakeStartFeatureId, true /* forward */, 0 /* segId */, startPoint); RoadPos finalPos(RoadPos::kFakeFinalFeatureId, true /* forward */, 0 /* segId */, finalPoint); - AddFakeTurns(startPos, startVicinity, m_roadGraph->m_startVicinityTurns); - AddFakeTurns(finalPos, finalVicinity, m_roadGraph->m_finalVicinityTurns); + m_roadGraph->SetFakeTurns(startPos, startVicinity); + m_roadGraph->SetFakeTurns(finalPos, finalVicinity); vector routePos; IRouter::ResultCode resultCode = CalculateRoute(startPos, finalPos, routePos); diff --git a/routing/routing_tests/road_graph_builder.cpp b/routing/routing_tests/road_graph_builder.cpp index a3978b7402..a16b4f0f4f 100644 --- a/routing/routing_tests/road_graph_builder.cpp +++ b/routing/routing_tests/road_graph_builder.cpp @@ -35,47 +35,10 @@ void RoadGraphMockSource::AddRoad(RoadInfo && ri) m_roads.push_back(move(ri)); } -void RoadGraphMockSource::GetNearestTurns(RoadPos const & pos, TurnsVectorT & turns) +IRoadGraph::RoadInfo RoadGraphMockSource::GetRoadInfo(uint32_t featureId) { - // TODO (@gorshenin): this partially duplicates code in - // CrossFeaturesLoader. Possible solution is to make - // CrossFeaturesLoader abstract enough to be used here and in - // FeaturesRoadGraph. - - CHECK_LESS(pos.GetFeatureId(), m_roads.size(), ("Invalid feature id.")); - RoadInfo const & curRoad = m_roads.at(pos.GetFeatureId()); - - CHECK_LESS(pos.GetSegStartPointId(), curRoad.m_points.size(), ("Invalid point id.")); - m2::PointD const curPoint = curRoad.m_points[pos.GetSegStartPointId()]; - - for (size_t featureId = 0; featureId < m_roads.size(); ++featureId) - { - RoadInfo const & road = m_roads[featureId]; - auto const & points = road.m_points; - if (road.m_speedKMPH <= 0.0) - continue; - PossibleTurn turn; - turn.m_startPoint = points.front(); - turn.m_endPoint = points.back(); - turn.m_speedKMPH = road.m_speedKMPH; - for (size_t i = 0; i < points.size(); ++i) - { - m2::PointD point = points[i]; - if (!m2::AlmostEqual(curPoint, point)) - continue; - if (i > 0) - { - turn.m_pos = RoadPos(featureId, true /* forward */, i - 1, point); - turns.push_back(turn); - } - - if (i + 1 < points.size()) - { - turn.m_pos = RoadPos(featureId, false /* forward */, i, point); - turns.push_back(turn); - } - } - } + CHECK_LESS(featureId, m_roads.size(), ("Invalid feature id.")); + return m_roads[featureId]; } double RoadGraphMockSource::GetSpeedKMPH(uint32_t featureId) @@ -84,6 +47,13 @@ double RoadGraphMockSource::GetSpeedKMPH(uint32_t featureId) return m_roads[featureId].m_speedKMPH; } +void RoadGraphMockSource::ForEachClosestToCrossFeature(m2::PointD const & /* cross */, + CrossTurnsLoader & turnsLoader) +{ + for (size_t roadId = 0; roadId < m_roads.size(); ++roadId) + turnsLoader(roadId, m_roads[roadId]); +} + void InitRoadGraphMockSourceWithTest1(RoadGraphMockSource & src) { IRoadGraph::RoadInfo ri0; diff --git a/routing/routing_tests/road_graph_builder.hpp b/routing/routing_tests/road_graph_builder.hpp index 40f3268495..bf2f22f3b4 100644 --- a/routing/routing_tests/road_graph_builder.hpp +++ b/routing/routing_tests/road_graph_builder.hpp @@ -31,8 +31,10 @@ public: void AddRoad(RoadInfo && ri); // routing::IRoadGraph overrides: - void GetNearestTurns(routing::RoadPos const & pos, TurnsVectorT & turns) override; + RoadInfo GetRoadInfo(uint32_t featureId) override; double GetSpeedKMPH(uint32_t featureId) override; + void ForEachClosestToCrossFeature(m2::PointD const & cross, + CrossTurnsLoader & turnsLoader) override; }; void InitRoadGraphMockSourceWithTest1(RoadGraphMockSource & graphMock);