pastk/organicmaps
1
Fork 0
forked from organicmaps/organicmaps
Code Pull requests Activity

Fixed heuristic for the wave start point

Browse source
This commit is contained in:
Constantin Shalnev 2015-05-21 16:27:49 +03:00 • committed by Alex Zolotarev
parent b9d22c8fb1
commit c96d0aea16
1 changed files with 5 additions and 4 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

9
routing/base/astar_algorithm.hpp
View file

@ -88,7 +88,7 @@ private:
: forward(forward), startVertex(startVertex), finalVertex(finalVertex), graph(graph)
{
bestVertex = forward ? startVertex : finalVertex;
pS = ConsistentHeuristic(startVertex);
pS = ConsistentHeuristic(bestVertex);
}
double TopDistance() const
@ -102,8 +102,8 @@ private:
// p_r(v) + p_f(v) = const. Note: this condition is called consistence.
double ConsistentHeuristic(TVertexType const & v) const
{
double piF = graph.HeuristicCostEstimate(v, finalVertex);
double piR = graph.HeuristicCostEstimate(v, startVertex);
double const piF = graph.HeuristicCostEstimate(v, finalVertex);
double const piR = graph.HeuristicCostEstimate(v, startVertex);
double const piRT = graph.HeuristicCostEstimate(finalVertex, startVertex);
double const piFS = graph.HeuristicCostEstimate(startVertex, finalVertex);
if (forward)
@ -337,7 +337,7 @@ typename AStarAlgorithm<TGraph>::Result AStarAlgorithm<TGraph>::FindPathBidirect
double const pV = cur->ConsistentHeuristic(stateV.vertex);
double const pW = cur->ConsistentHeuristic(stateW.vertex);
double const reducedLen = len + pW - pV;
double const pRW = nxt->ConsistentHeuristic(stateW.vertex);
CHECK(reducedLen >= -kEpsilon, ("Invariant violated:", reducedLen, "<", -kEpsilon));
double const newReducedDist = stateV.distance + max(reducedLen, 0.0);
@ -348,6 +348,7 @@ typename AStarAlgorithm<TGraph>::Result AStarAlgorithm<TGraph>::FindPathBidirect
auto const itNxt = nxt->bestDistance.find(stateW.vertex);
if (itNxt != nxt->bestDistance.end())
{
double const pRW = nxt->ConsistentHeuristic(stateW.vertex);
double const distW = itNxt->second;
// Length that the path we've just found has in the original graph:
// find the length of the path's parts in the reduced forward and backward