AStar: avoid rounding errors in tests

graphhopper · Aug 24, 2015 · a00e73e · a00e73e
1 parent 1968e7f
commit a00e73e
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 13 deletions.
diff --git a/core/src/main/java/com/graphhopper/routing/AStar.java b/core/src/main/java/com/graphhopper/routing/AStar.java
@@ -92,7 +92,7 @@ public Path calcPath( int from, int to )
 
     private Path runAlgo()
     {
-        double currWeightToGoal, distEstimation;
+        double currWeightToGoal, estimationFullWeight;
         EdgeExplorer explorer = outEdgeExplorer;
         while (true)
         {
@@ -118,20 +118,22 @@ private Path runAlgo()
                     continue;
 
                 AStarEdge ase = fromMap.get(traversalId);
-                if ((ase == null) || ase.weightOfVisitedPath > alreadyVisitedWeight)
+                if (ase == null || ase.weightOfVisitedPath > alreadyVisitedWeight)
                 {
                     currWeightToGoal = weightApprox.approximate(neighborNode);
-                    distEstimation = alreadyVisitedWeight + currWeightToGoal;
+                    estimationFullWeight = alreadyVisitedWeight + currWeightToGoal;
                     if (ase == null)
                     {
-                        ase = new AStarEdge(iter.getEdge(), neighborNode, distEstimation, alreadyVisitedWeight);
+                        ase = new AStarEdge(iter.getEdge(), neighborNode, estimationFullWeight, alreadyVisitedWeight);
                         fromMap.put(traversalId, ase);
                     } else
                     {
-                        assert (ase.weight > distEstimation) : "Inconsistent distance estimate";
+                        assert (ase.weight > 0.9999999 * estimationFullWeight) : "Inconsistent distance estimate "
+                                + ase.weight + " vs " + estimationFullWeight + " (" + ase.weight / estimationFullWeight + "), and:"
+                                + ase.weightOfVisitedPath + " vs " + alreadyVisitedWeight + " (" + ase.weightOfVisitedPath / alreadyVisitedWeight + ")";
                         prioQueueOpenSet.remove(ase);
                         ase.edge = iter.getEdge();
-                        ase.weight = distEstimation;
+                        ase.weight = estimationFullWeight;
                         ase.weightOfVisitedPath = alreadyVisitedWeight;
                     }
 

diff --git a/core/src/main/java/com/graphhopper/routing/AStarBidirection.java b/core/src/main/java/com/graphhopper/routing/AStarBidirection.java
@@ -237,7 +237,7 @@ boolean fillEdgesTo()
     }
 
     private void fillEdges( AStarEdge currEdge, PriorityQueue<AStarEdge> prioQueueOpenSet,
-                            TIntObjectMap<AStarEdge> shortestWeightMap, EdgeExplorer explorer, boolean reverse )
+                            TIntObjectMap<AStarEdge> bestWeightMap, EdgeExplorer explorer, boolean reverse )
     {
 
         int currNode = currEdge.adjNode;
@@ -256,21 +256,23 @@ private void fillEdges( AStarEdge currEdge, PriorityQueue<AStarEdge> prioQueueOp
             if (Double.isInfinite(alreadyVisitedWeight))
                 continue;
 
-            AStarEdge ase = shortestWeightMap.get(traversalId);
+            AStarEdge ase = bestWeightMap.get(traversalId);
             if (ase == null || ase.weightOfVisitedPath > alreadyVisitedWeight)
             {
                 double currWeightToGoal = weightApprox.approximate(neighborNode, reverse);
-                double estimationFullDist = alreadyVisitedWeight + currWeightToGoal;
+                double estimationFullWeight = alreadyVisitedWeight + currWeightToGoal;
                 if (ase == null)
                 {
-                    ase = new AStarEdge(iter.getEdge(), neighborNode, estimationFullDist, alreadyVisitedWeight);
-                    shortestWeightMap.put(traversalId, ase);
+                    ase = new AStarEdge(iter.getEdge(), neighborNode, estimationFullWeight, alreadyVisitedWeight);
+                    bestWeightMap.put(traversalId, ase);
                 } else
                 {
-                    assert (ase.weight > estimationFullDist) : "Inconsistent distance estimate";
+                    assert (ase.weight > 0.999999 * estimationFullWeight) : "Inconsistent distance estimate "
+                                + ase.weight + " vs " + estimationFullWeight + " (" + ase.weight / estimationFullWeight + "), and:"
+                                + ase.weightOfVisitedPath + " vs " + alreadyVisitedWeight + " (" + ase.weightOfVisitedPath / alreadyVisitedWeight + ")";
                     prioQueueOpenSet.remove(ase);
                     ase.edge = iter.getEdge();
-                    ase.weight = estimationFullDist;
+                    ase.weight = estimationFullWeight;
                     ase.weightOfVisitedPath = alreadyVisitedWeight;
                 }