com.graphhopper.routing.AStar Maven / Gradle / Ivy
Show all versions of graphhopper Show documentation
/*
* Licensed to GraphHopper and Peter Karich under one or more contributor
* license agreements. See the NOTICE file distributed with this work for
* additional information regarding copyright ownership.
*
* GraphHopper licenses this file to you under the Apache License,
* Version 2.0 (the "License"); you may not use this file except in
* compliance with the License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package com.graphhopper.routing;
import com.graphhopper.util.DistancePlaneProjection;
import gnu.trove.map.TIntObjectMap;
import gnu.trove.map.hash.TIntObjectHashMap;
import java.util.PriorityQueue;
import com.graphhopper.routing.util.FlagEncoder;
import com.graphhopper.routing.util.TraversalMode;
import com.graphhopper.routing.util.Weighting;
import com.graphhopper.routing.util.WeightApproximator;
import com.graphhopper.routing.util.BeelineWeightApproximator;
import com.graphhopper.storage.SPTEntry;
import com.graphhopper.storage.Graph;
import com.graphhopper.util.EdgeExplorer;
import com.graphhopper.util.EdgeIterator;
/**
* This class implements the A* algorithm according to
* http://en.wikipedia.org/wiki/A*_search_algorithm
*
* Different distance calculations can be used via setApproximation.
*
* @author Peter Karich
*/
public class AStar extends AbstractRoutingAlgorithm
{
private WeightApproximator weightApprox;
private int visitedCount;
private TIntObjectMap fromMap;
private PriorityQueue prioQueueOpenSet;
private AStarEntry currEdge;
private int to1 = -1;
public AStar( Graph g, FlagEncoder encoder, Weighting weighting, TraversalMode tMode )
{
super(g, encoder, weighting, tMode);
initCollections(1000);
BeelineWeightApproximator defaultApprox = new BeelineWeightApproximator(nodeAccess, weighting);
defaultApprox.setDistanceCalc(new DistancePlaneProjection());
setApproximation(defaultApprox);
}
/**
* @param approx defines how distance to goal Node is approximated
*/
public AStar setApproximation( WeightApproximator approx )
{
weightApprox = approx;
return this;
}
protected void initCollections( int size )
{
fromMap = new TIntObjectHashMap();
prioQueueOpenSet = new PriorityQueue(size);
}
@Override
public Path calcPath( int from, int to )
{
checkAlreadyRun();
to1 = to;
weightApprox.setGoalNode(to);
double weightToGoal = weightApprox.approximate(from);
currEdge = new AStarEntry(EdgeIterator.NO_EDGE, from, 0 + weightToGoal, 0);
if (!traversalMode.isEdgeBased())
{
fromMap.put(from, currEdge);
}
return runAlgo();
}
private Path runAlgo()
{
double currWeightToGoal, estimationFullWeight;
EdgeExplorer explorer = outEdgeExplorer;
while (true)
{
int currVertex = currEdge.adjNode;
visitedCount++;
if (isWeightLimitExceeded())
return createEmptyPath();
if (finished())
break;
EdgeIterator iter = explorer.setBaseNode(currVertex);
while (iter.next())
{
if (!accept(iter, currEdge.edge))
continue;
int neighborNode = iter.getAdjNode();
int traversalId = traversalMode.createTraversalId(iter, false);
double alreadyVisitedWeight = weighting.calcWeight(iter, false, currEdge.edge)
+ currEdge.weightOfVisitedPath;
if (Double.isInfinite(alreadyVisitedWeight))
continue;
AStarEntry ase = fromMap.get(traversalId);
if (ase == null || ase.weightOfVisitedPath > alreadyVisitedWeight)
{
currWeightToGoal = weightApprox.approximate(neighborNode);
estimationFullWeight = alreadyVisitedWeight + currWeightToGoal;
if (ase == null)
{
ase = new AStarEntry(iter.getEdge(), neighborNode, estimationFullWeight, alreadyVisitedWeight);
fromMap.put(traversalId, ase);
} else
{
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 = estimationFullWeight;
ase.weightOfVisitedPath = alreadyVisitedWeight;
}
ase.parent = currEdge;
prioQueueOpenSet.add(ase);
updateBestPath(iter, ase, traversalId);
}
}
if (prioQueueOpenSet.isEmpty())
return createEmptyPath();
currEdge = prioQueueOpenSet.poll();
if (currEdge == null)
throw new AssertionError("Empty edge cannot happen");
}
return extractPath();
}
@Override
protected Path extractPath()
{
return new Path(graph, flagEncoder).setWeight(currEdge.weight).setEdgeEntry(currEdge).extract();
}
@Override
protected SPTEntry createEdgeEntry( int node, double weight )
{
throw new IllegalStateException("use AStarEdge constructor directly");
}
@Override
protected boolean finished()
{
return currEdge.adjNode == to1;
}
@Override
public int getVisitedNodes()
{
return visitedCount;
}
@Override
protected boolean isWeightLimitExceeded()
{
return currEdge.weight > weightLimit;
}
public static class AStarEntry extends SPTEntry
{
double weightOfVisitedPath;
public AStarEntry( int edgeId, int adjNode, double weightForHeap, double weightOfVisitedPath )
{
super(edgeId, adjNode, weightForHeap);
this.weightOfVisitedPath = weightOfVisitedPath;
}
@Override
public final double getWeightOfVisitedPath()
{
return weightOfVisitedPath;
}
}
@Override
public String getName()
{
return AlgorithmOptions.ASTAR;
}
}