diff --git a/routing/features_road_graph.cpp b/routing/features_road_graph.cpp index 56e5ce0048..21d6bc1757 100644 --- a/routing/features_road_graph.cpp +++ b/routing/features_road_graph.cpp @@ -132,8 +132,87 @@ void FeaturesRoadGraph::LoadFeature(uint32_t id, FeatureType & ft) void FeaturesRoadGraph::GetNearestTurns(RoadPos const & pos, vector & turns) { - GetNearestTurnsImpl(pos, true /* isForward */, turns); - GetNearestTurnsImpl(pos, false /* isForward */, turns); + uint32_t const featureId = pos.GetFeatureId(); + FeatureType ft; + CachedFeature const fc = GetCachedFeature(featureId, ft, true); + + if (fc.m_speed <= 0.0) + return; + + size_t const numPoints = fc.m_points.size(); + ASSERT_GREATER_OR_EQUAL(numPoints, 2, ("Incorrect road - only", numPoints, "point(s).")); + + bool const isForward = pos.IsForward(); + uint32_t const segId = pos.GetSegId(); + + double const segmentDistance = + CalcDistanceMeters(pos.GetSegEndpoint(), fc.m_points[pos.GetSegStartPointId()]); + double const segmentTime = segmentDistance / fc.m_speed; + + m2::PointD const point = fc.m_points[pos.GetSegStartPointId()]; + + // Find possible turns to startPoint from other features. + CrossFeaturesLoader crossLoader(featureId, *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), + READ_ROAD_SCALE); + + indexCheck++; + + if (crossLoader.GetCount() > 0) + indexFound++; + + { + PossibleTurn revTurn; + revTurn.m_speed = fc.m_speed; + revTurn.m_metersCovered = 0; + revTurn.m_secondsCovered = 0; + revTurn.m_startPoint = fc.m_points.front(); + revTurn.m_endPoint = fc.m_points.back(); + revTurn.m_pos = + RoadPos(pos.GetFeatureId(), !pos.IsForward(), pos.GetSegId(), pos.GetSegEndpoint()); + turns.push_back(revTurn); + } + + // Following code adds "fake" turn from an adjacent segment to pos. + // That's why m_startPoint and m_endPoint are set to the bounds of + // the current road. + PossibleTurn thisTurn; + thisTurn.m_speed = fc.m_speed; + thisTurn.m_metersCovered = segmentDistance; + thisTurn.m_secondsCovered = segmentTime; + thisTurn.m_startPoint = fc.m_points.front(); + thisTurn.m_endPoint = fc.m_points.back(); + + // Push turn points for this feature. + if (isForward && segId > 0) + { + thisTurn.m_pos = RoadPos(featureId, isForward, segId - 1, point); + turns.push_back(thisTurn); + } + if (!isForward && segId + 2 < numPoints) + { + thisTurn.m_pos = RoadPos(featureId, isForward, segId + 1, point); + turns.push_back(thisTurn); + } + + // Checks that the road is not a loop. + if (!m2::AlmostEqual(fc.m_points.front(), fc.m_points.back())) + return; + + if (isForward && segId == 0) + { + // It seems that it's possible to get from the last segment to the first. + thisTurn.m_pos = RoadPos(featureId, isForward, numPoints - 2 /* segId */, point); + turns.push_back(thisTurn); + } + if (!isForward && segId + 2 == numPoints) + { + // It seems that it's possible to get from the first segment to the last. + thisTurn.m_pos = RoadPos(featureId, isForward, 0 /* segId */, point); + turns.push_back(thisTurn); + } } void FeaturesRoadGraph::ReconstructPath(RoadPosVectorT const & positions, Route & route) @@ -253,80 +332,4 @@ FeaturesRoadGraph::CachedFeature const & FeaturesRoadGraph::GetCachedFeature( return f; } - -void FeaturesRoadGraph::GetNearestTurnsImpl(RoadPos const & pos, bool isForward, vector & turns) -{ - uint32_t const featureId = pos.GetFeatureId(); - FeatureType ft; - CachedFeature const fc = GetCachedFeature(featureId, ft, true); - - if (fc.m_speed <= 0.0) - return; - - size_t const numPoints = fc.m_points.size(); - ASSERT_GREATER_OR_EQUAL(numPoints, 2, ("Incorrect road - only", numPoints, "points")); - - uint32_t const segId = pos.GetSegId(); - - PossibleTurn thisTurn; - thisTurn.m_speed = fc.m_speed; - thisTurn.m_startPoint = fc.m_points.front(); - thisTurn.m_endPoint = fc.m_points.back(); - - double const segmentDistance = CalcDistanceMeters(fc.m_points[pos.GetSegStartPointId()], - fc.m_points[pos.GetSegEndPointId()]); - double const segmentTime = segmentDistance / fc.m_speed; - - m2::PointD const point = isForward ? fc.m_points[segId + 1] : fc.m_points[segId]; - - // Find possible turns to startPoint from other features. - CrossFeaturesLoader crossLoader(featureId, *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), - READ_ROAD_SCALE); - - indexCheck++; - - if (crossLoader.GetCount() > 0) - indexFound++; - - // Push turn points for this feature. - if (segId > 0) - { - thisTurn.m_pos = RoadPos(featureId, true /* isForward */, segId - 1, fc.m_points[segId]); - turns.push_back(thisTurn); - } - if (segId + 2 < numPoints) - { - thisTurn.m_pos = RoadPos(featureId, false /* isForward */, segId + 1, fc.m_points[segId + 1]); - turns.push_back(thisTurn); - } - - // Update distance and time information. - for (PossibleTurn & turn : turns) - { - turn.m_metersCovered = segmentDistance; - turn.m_secondsCovered = segmentTime; - turn.m_speed = fc.m_speed; - } - - // Checks that the road is not a loop. - if (!m2::AlmostEqual(fc.m_points.front(), fc.m_points.back())) - return; - - if (segId == 0) - { - // It seems that it's possible to get from the last segment to the first. - thisTurn.m_pos = RoadPos(featureId, true /* isForward */, numPoints - 2 /* segId */, fc.m_points.back()); - turns.push_back(thisTurn); - } - if (segId + 2 == numPoints) - { - // It seems that it's possible to get from the first segment to the last. - thisTurn.m_pos = RoadPos(featureId, false /* isForward */, 0 /* segId */, fc.m_points.front()); - turns.push_back(thisTurn); - } -} - } // namespace routing diff --git a/routing/features_road_graph.hpp b/routing/features_road_graph.hpp index d9763b9f3d..073a64a6cf 100644 --- a/routing/features_road_graph.hpp +++ b/routing/features_road_graph.hpp @@ -28,6 +28,8 @@ class FeaturesRoadGraph : public IRoadGraph bool m_isOneway; }; + // TODO (@gorshenin): ft is not set when feature is not loaded from + // cache, investigate how to fix this. CachedFeature const & GetCachedFeature(uint32_t const ftId, FeatureType & ft, bool fullLoad); public: @@ -55,11 +57,6 @@ private: double GetSpeed(FeatureType const & ft) const; void LoadFeature(uint32_t id, FeatureType & ft); - // Returns nearest turns when pos is traversed in forward (when - // isForward is set) or backward (when isForward is not set) - // direction. - void GetNearestTurnsImpl(RoadPos const & pos, bool isForward, vector & turns); - Index const * m_pIndex; size_t m_mwmID; unique_ptr m_vehicleModel;