From 712ce57199a2b7b8cb59e8a32308e8e8925845c4 Mon Sep 17 00:00:00 2001
From: Vladimir Byko-Ianko <v.bykoianko@corp.mail.ru>
Date: Tue, 18 Jun 2019 11:41:35 +0300
Subject: [PATCH] [routing] NearestEdgeFinder::AddInformationSource()
 refactoring.

---
 routing/nearest_edge_finder.cpp | 78 +++++++++++++++++++--------------
 routing/nearest_edge_finder.hpp |  4 +-
 2 files changed, 47 insertions(+), 35 deletions(-)

diff --git a/routing/nearest_edge_finder.cpp b/routing/nearest_edge_finder.cpp
index 7127a2d323..2696459988 100644
--- a/routing/nearest_edge_finder.cpp
+++ b/routing/nearest_edge_finder.cpp
@@ -19,55 +19,65 @@ void NearestEdgeFinder::AddInformationSource(FeatureID const & featureId,
                                              IRoadGraph::JunctionVec const & junctions,
                                              bool bidirectional)
 {
+  if (!featureId.IsValid())
+    return;
+
   Candidate res;
 
   size_t const count = junctions.size();
   ASSERT_GREATER(count, 1, ());
   for (size_t i = 1; i < count; ++i)
   {
-    // @TODO All the calculations below should be done after the closest point is found.
-    // Otherway we do unnecessary calculation significant number of times.
     m2::ParametrizedSegment<m2::PointD> segment(junctions[i - 1].GetPoint(),
                                                 junctions[i].GetPoint());
 
-    m2::PointD const pt = segment.ClosestPointTo(m_point);
-    double const squaredDist = m_point.SquaredLength(pt);
+    m2::PointD const closestPoint = segment.ClosestPointTo(m_point);
+    double const squaredDist = m_point.SquaredLength(closestPoint);
+
     if (squaredDist < res.m_squaredDist)
     {
-      Junction const & segStart = junctions[i - 1];
-      Junction const & segEnd = junctions[i];
-      feature::TAltitude const startAlt = segStart.GetAltitude();
-      feature::TAltitude const endAlt = segEnd.GetAltitude();
-
-      double const segLenM = MercatorBounds::DistanceOnEarth(segStart.GetPoint(), segEnd.GetPoint());
-      feature::TAltitude projPointAlt = feature::kDefaultAltitudeMeters;
-      if (segLenM == 0.0)
-      {
-        projPointAlt = startAlt;
-      }
-      else
-      {
-        double const distFromStartM = MercatorBounds::DistanceOnEarth(segStart.GetPoint(), pt);
-        ASSERT_LESS_OR_EQUAL(distFromStartM, segLenM, (featureId));
-        projPointAlt = startAlt + static_cast<feature::TAltitude>((endAlt - startAlt) * distFromStartM / segLenM);
-      }
-
-      res.m_squaredDist = squaredDist;
-      res.m_fid = featureId;
       res.m_segId = static_cast<uint32_t>(i - 1);
-      res.m_segStart = segStart;
-      res.m_segEnd = segEnd;
-      res.m_bidirectional = bidirectional;
-
-      ASSERT_NOT_EQUAL(res.m_segStart.GetAltitude() , feature::kInvalidAltitude, ());
-      ASSERT_NOT_EQUAL(res.m_segEnd.GetAltitude(), feature::kInvalidAltitude, ());
-
-      res.m_projPoint = Junction(pt, projPointAlt);
+      res.m_squaredDist = squaredDist;
     }
   }
 
-  if (res.m_fid.IsValid())
-    m_candidates.push_back(res);
+  if (res.m_segId == Candidate::kInvalidSegmentId)
+    return;
+
+  // Closest point to |this->m_point| found. It has index |res.m_segId + 1| in |junctions|.
+  size_t const idx = res.m_segId + 1;
+  Junction const & segStart = junctions[idx - 1];
+  Junction const & segEnd = junctions[idx];
+  feature::TAltitude const startAlt = segStart.GetAltitude();
+  feature::TAltitude const endAlt = segEnd.GetAltitude();
+  m2::ParametrizedSegment<m2::PointD> segment(junctions[idx - 1].GetPoint(),
+                                              junctions[idx].GetPoint());
+  m2::PointD const closestPoint = segment.ClosestPointTo(m_point);
+
+  double const segLenM = MercatorBounds::DistanceOnEarth(segStart.GetPoint(), segEnd.GetPoint());
+  feature::TAltitude projPointAlt = feature::kDefaultAltitudeMeters;
+  if (segLenM == 0.0)
+  {
+    projPointAlt = startAlt;
+  }
+  else
+  {
+    double const distFromStartM = MercatorBounds::DistanceOnEarth(segStart.GetPoint(), closestPoint);
+    ASSERT_LESS_OR_EQUAL(distFromStartM, segLenM, (featureId));
+    projPointAlt =
+        startAlt + static_cast<feature::TAltitude>((endAlt - startAlt) * distFromStartM / segLenM);
+  }
+
+  res.m_fid = featureId;
+  res.m_segStart = segStart;
+  res.m_segEnd = segEnd;
+  res.m_bidirectional = bidirectional;
+
+  ASSERT_NOT_EQUAL(res.m_segStart.GetAltitude(), feature::kInvalidAltitude, ());
+  ASSERT_NOT_EQUAL(res.m_segEnd.GetAltitude(), feature::kInvalidAltitude, ());
+  res.m_projPoint = Junction(closestPoint, projPointAlt);
+
+  m_candidates.push_back(res);
 }
 
 void NearestEdgeFinder::MakeResult(vector<pair<Edge, Junction>> & res, size_t const maxCountFeatures)
diff --git a/routing/nearest_edge_finder.hpp b/routing/nearest_edge_finder.hpp
index aa6026ad86..b9acb9911e 100644
--- a/routing/nearest_edge_finder.hpp
+++ b/routing/nearest_edge_finder.hpp
@@ -25,8 +25,10 @@ class NearestEdgeFinder
 {
   struct Candidate
   {
+    static uint32_t constexpr kInvalidSegmentId = std::numeric_limits<uint32_t>::max();
+
     double m_squaredDist = std::numeric_limits<double>::max();
-    uint32_t m_segId = 0;
+    uint32_t m_segId = kInvalidSegmentId;
     Junction m_segStart;
     Junction m_segEnd;
     Junction m_projPoint;