From c4141880f8704400adb8bcfe00b93d8ac6cb0899 Mon Sep 17 00:00:00 2001
From: Lev Dragunov <l.dragunov@corp.mail.ru>
Date: Thu, 10 Mar 2016 15:38:44 +0300
Subject: [PATCH] AStar result distance fix.

---
 routing/base/astar_algorithm.hpp            | 14 ++++++++------
 routing/routing_tests/astar_router_test.cpp |  6 +++---
 2 files changed, 11 insertions(+), 9 deletions(-)

diff --git a/routing/base/astar_algorithm.hpp b/routing/base/astar_algorithm.hpp
index 95c0c87109..103468b17c 100644
--- a/routing/base/astar_algorithm.hpp
+++ b/routing/base/astar_algorithm.hpp
@@ -221,7 +221,7 @@ typename AStarAlgorithm<TGraph>::Result AStarAlgorithm<TGraph>::FindPath(
     if (stateV.vertex == finalVertex)
     {
       ReconstructPath(stateV.vertex, parent, result.path);
-      result.distance = stateV.distance;
+      result.distance = stateV.distance - graph.HeuristicCostEstimate(stateV.vertex, finalVertex) + graph.HeuristicCostEstimate(startVertex, finalVertex);
       ASSERT_EQUAL(graph.HeuristicCostEstimate(stateV.vertex, finalVertex), 0, ());
       return Result::OK;
     }
@@ -271,6 +271,7 @@ typename AStarAlgorithm<TGraph>::Result AStarAlgorithm<TGraph>::FindPathBidirect
 
   bool foundAnyPath = false;
   double bestPathReducedLength = 0.0;
+  double bestPathRealLength = 0.0;
 
   forward.bestDistance[startVertex] = 0.0;
   forward.queue.push(State(startVertex, 0.0 /* distance */));
@@ -321,11 +322,7 @@ typename AStarAlgorithm<TGraph>::Result AStarAlgorithm<TGraph>::FindPathBidirect
       {
         ReconstructPathBidirectional(cur->bestVertex, nxt->bestVertex, cur->parent, nxt->parent,
                                      result.path);
-        result.distance = bestPathReducedLength;
-        if (curTop > 0)
-          result.distance -= cur->ConsistentHeuristic(nxt->bestVertex);
-        if (nxtTop > 0)
-          result.distance -= nxt->ConsistentHeuristic(nxt->bestVertex);
+        result.distance = bestPathRealLength;
         CHECK(!result.path.empty(), ());
         if (!cur->forward)
           reverse(result.path.begin(), result.path.end());
@@ -372,6 +369,11 @@ typename AStarAlgorithm<TGraph>::Result AStarAlgorithm<TGraph>::FindPathBidirect
         if (!foundAnyPath || bestPathReducedLength > curPathReducedLength)
         {
           bestPathReducedLength = curPathReducedLength;
+
+          bestPathRealLength = stateV.distance + len + distW;
+          bestPathRealLength += cur->pS - pV;
+          bestPathRealLength += nxt->pS - nxt->ConsistentHeuristic(stateW.vertex);
+
           foundAnyPath = true;
           cur->bestVertex = stateV.vertex;
           nxt->bestVertex = stateW.vertex;
diff --git a/routing/routing_tests/astar_router_test.cpp b/routing/routing_tests/astar_router_test.cpp
index e38422f9da..cb15d76e58 100644
--- a/routing/routing_tests/astar_router_test.cpp
+++ b/routing/routing_tests/astar_router_test.cpp
@@ -213,7 +213,7 @@ UNIT_TEST(AStarRouter_SimpleGraph_PickTheFasterRoad1)
              ());
   TEST_EQUAL(result.path, vector<Junction>({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)}), ());
-  TEST(my::AlmostEqualAbs(result.distance, 159655., 1.), ("Distance error:", result.distance));
+  TEST(my::AlmostEqualAbs(result.distance, 800451., 1.), ("Distance error:", result.distance));
 }
 
 UNIT_TEST(AStarRouter_SimpleGraph_PickTheFasterRoad2)
@@ -240,7 +240,7 @@ UNIT_TEST(AStarRouter_SimpleGraph_PickTheFasterRoad2)
              algorithm.CalculateRoute(graph, m2::PointD(2, 2), m2::PointD(10, 2), delegate, result),
              ());
   TEST_EQUAL(result.path, vector<Junction>({m2::PointD(2,2), m2::PointD(6,2), m2::PointD(10,2)}), ());
-  TEST(my::AlmostEqualAbs(result.distance, 140663., 1.), ("Distance error:", result.distance));
+  TEST(my::AlmostEqualAbs(result.distance, 781458., 1.), ("Distance error:", result.distance));
 }
 
 UNIT_TEST(AStarRouter_SimpleGraph_PickTheFasterRoad3)
@@ -267,5 +267,5 @@ UNIT_TEST(AStarRouter_SimpleGraph_PickTheFasterRoad3)
              algorithm.CalculateRoute(graph, m2::PointD(2, 2), m2::PointD(10, 2), delegate, result),
              ());
   TEST_EQUAL(result.path, vector<Junction>({m2::PointD(2,2), m2::PointD(2,1), m2::PointD(10,1), m2::PointD(10,2)}), ());
-  TEST(my::AlmostEqualAbs(result.distance, 173616., 1.), ("Distance error:", result.distance));
+  TEST(my::AlmostEqualAbs(result.distance, 814412., 1.), ("Distance error:", result.distance));
 }