diff --git a/routing/index_router.cpp b/routing/index_router.cpp index 7c983bab81..17c5abc1b6 100644 --- a/routing/index_router.cpp +++ b/routing/index_router.cpp @@ -152,6 +152,48 @@ void PushPassedSubroutes(Checkpoints const & checkpoints, vector= kMinCosAngForAlmostParallelVectors; +} + +double IndexRouter::BestEdgeComparator::GetSquaredDist(Edge const & edge) const +{ + m2::DistanceToLineSquare squaredDistance; + squaredDistance.SetBounds(edge.GetStartJunction().GetPoint(), edge.GetEndJunction().GetPoint()); + return squaredDistance(m_point); +} + +// IndexRouter ------------------------------------------------------------------------------------ IndexRouter::IndexRouter(string const & name, TCountryFileFn const & countryFileFn, CourntryRectFn const & countryRectFn, shared_ptr numMwmIds, unique_ptr> numMwmTree, @@ -499,7 +541,7 @@ WorldGraph IndexRouter::MakeWorldGraph() return graph; } -bool IndexRouter::FindBestSegment(m2::PointD const & point, m2::PointD const & startDirection, +bool IndexRouter::FindBestSegment(m2::PointD const & point, m2::PointD const & direction, bool isOutgoing, WorldGraph & worldGraph, Segment & bestSegment) const { @@ -514,74 +556,29 @@ bool IndexRouter::FindBestSegment(m2::PointD const & point, m2::PointD const & s vector> candidates; m_roadGraph.FindClosestEdges(point, kMaxRoadCandidates, candidates); - // Minimum distance to neighbouring segments. - double minDist = numeric_limits::max(); - size_t minIndex = candidates.size(); + auto const getSegmentByEdge = [&numMwmId](Edge const & edge){ + return Segment(numMwmId, edge.GetFeatureId().m_index, edge.GetSegId(), edge.IsForward()); + }; - // Minimum distance to neighbouring segments which are parallel to |startDirection|. - double minDistForParallel = numeric_limits::max(); - size_t minIndexForParallel = candidates.size(); + // Getting rid of knowingly bad candidates. + candidates.erase(remove_if(candidates.begin(), candidates.end(), [&](pair const & p){ + Edge const & edge = p.first; + return edge.GetFeatureId().m_mwmId != mwmId || IsDeadEnd(getSegmentByEdge(edge), isOutgoing, worldGraph); + }), candidates.end()); - for (size_t i = 0; i < candidates.size(); ++i) + if (candidates.empty()) + return false; + + BestEdgeComparator bestEdgeComparator(point, direction); + Edge & bestEdge = candidates[0].first; + for (size_t i = 1; i < candidates.size(); ++i) { Edge const & edge = candidates[i].first; - Segment const segment(numMwmId, edge.GetFeatureId().m_index, edge.GetSegId(), edge.IsForward()); - - if (edge.GetFeatureId().m_mwmId != mwmId || IsDeadEnd(segment, isOutgoing, worldGraph)) - continue; - - // Distance without taking into account angle. - m2::DistanceToLineSquare squaredDistance; - squaredDistance.SetBounds(edge.GetStartJunction().GetPoint(), edge.GetEndJunction().GetPoint()); - double const dist = squaredDistance(point); - auto const UpdateMinValuesIfNeeded = [&](double & minDistToUpdate, size_t & minIndexToUpdate) { - if (minDistToUpdate > dist) - { - minDistToUpdate = dist; - minIndexToUpdate = i; - } - }; - - UpdateMinValuesIfNeeded(minDist, minIndex); - - // Distance for almost parallel vectors. - if (startDirection.IsAlmostZero()) - continue; - - m2::PointD const candidateDirection = - edge.GetEndJunction().GetPoint() - edge.GetStartJunction().GetPoint(); - if (candidateDirection.IsAlmostZero()) - continue; - - double const cosAng = - m2::DotProduct(startDirection.Normalize(), candidateDirection.Normalize()); - // Let us consider that vectors are almost parallel if cos of angle between them is more - // than |kMinCosAngForAlmostParallelVectors|. - // Note. If cosinus of an angle is more 0.97. The angle should be more than 14 degrees. - double constexpr kMinCosAngForAlmostParallelVectors = 0.97; - if (cosAng < kMinCosAngForAlmostParallelVectors) - continue; - - // Vectors of |edge| and gps heading are almost parallel. - UpdateMinValuesIfNeeded(minDistForParallel, minIndexForParallel); + if (bestEdgeComparator.Compare(edge, bestEdge)) + bestEdge = edge; } - if (minIndex == candidates.size()) - { - CHECK_EQUAL(minIndexForParallel, candidates.size(), ()); - return false; - } - - // Note. The idea behind choosing |bestEdge| is - // * trying to find closest to |point| edge (segment) which is almost parallel to - // |startDirection|; - // * if there's no such edge (segment) trying to find the segment which is closest the |point|. - // * if anyway nothing is found - returns false. - Edge const & bestEdge = - (minIndexForParallel == candidates.size() ? candidates[minIndex].first - : candidates[minIndexForParallel].first); - bestSegment = Segment(m_numMwmIds->GetId(file), bestEdge.GetFeatureId().m_index, - bestEdge.GetSegId(), true /* forward */); + bestSegment = getSegmentByEdge(bestEdge); return true; } diff --git a/routing/index_router.hpp b/routing/index_router.hpp index c392eea790..69563ac52a 100644 --- a/routing/index_router.hpp +++ b/routing/index_router.hpp @@ -30,6 +30,27 @@ class IndexGraphStarter; class IndexRouter : public IRouter { public: + class BestEdgeComparator final + { + public: + BestEdgeComparator(m2::PointD const & point, m2::PointD const & direction); + + /// \returns true if |edge1| is closer to |m_point|, |m_direction| than |edge2|. + bool Compare(Edge const & edge1, Edge const & edge2) const; + + private: + /// \returns true if |edge| is almost parallel with vector |m_direction|. + /// \note According to current implementation vectors |edge| and |m_direction| + /// are almost parallel if angle between them less than 14 degrees. + bool IsAlmostParallel(Edge const & edge) const; + + /// \returns the square of shortest distance from |m_point| to |edge| in mercator. + double GetSquaredDist(Edge const & edge) const; + + m2::PointD const m_point; + m2::PointD const m_direction; + }; + IndexRouter(string const & name, TCountryFileFn const & countryFileFn, CourntryRectFn const & countryRectFn, shared_ptr numMwmIds, unique_ptr> numMwmTree, shared_ptr trafficStash, @@ -65,12 +86,13 @@ private: WorldGraph MakeWorldGraph(); - /// \brief Finds best edges which may be considered as the start of the finish of the route. - /// If it's possible |bestSegment| will be filled with a segment which is almost parallel - /// to |startDirection|. + /// \brief Finds the best segment (edge) which may be considered as the start of the finish of the route. + /// According to current implementation if a segment is laying near |point| and is almost parallel + /// to |direction| vector, the segment will be better than others. If there's no an an almost parallel + /// segment in neighbourhoods the closest segment will be chosen. /// \param isOutgoing == true is |point| is considered as the start of the route. /// isOutgoing == false is |point| is considered as the finish of the route. - bool FindBestSegment(m2::PointD const & point, m2::PointD const & startDirection, bool isOutgoing, + bool FindBestSegment(m2::PointD const & point, m2::PointD const & direction, bool isOutgoing, WorldGraph & worldGraph, Segment & bestSegment) const; // Input route may contains 'leaps': shortcut edges from mwm border enter to exit. // ProcessLeaps replaces each leap with calculated route through mwm. diff --git a/routing/routing_session.cpp b/routing/routing_session.cpp index 25c990c4cc..dd8de8106b 100644 --- a/routing/routing_session.cpp +++ b/routing/routing_session.cpp @@ -234,7 +234,7 @@ RoutingSession::State RoutingSession::OnLocationPositionChanged(GpsInfo const & } } - if (m_lastGoodPosition != m2::PointD::Zero()) + if (m_lastGoodPosition != m2::PointD::Zero() && m_userCurrentPosition != m2::PointD::Zero()) m_currentDirection = m_userCurrentPosition - m_lastGoodPosition; m_lastGoodPosition = m_userCurrentPosition;