/**
* Get link info of the link from start node to end node
*
* @param startNode
* @param endNode
* @param network
* @return the link info or {@code null} if error occurred
*/
public static LinkInfo getLink(int startNode, int endNode, LogicNetwork network) {
if (network.getNodes().size() <= startNode || network.getNodes().size() <= endNode) return null;
for (LinkInfo linkInfo : network.getLinks().get(startNode)) {
if (linkInfo.getEndNode() == endNode) return linkInfo;
}
return null;
}
public static PathInfo findShortestPathRoadNetwork(
LogicNetwork network,
NodeInfo startNode,
NodeInfo endNode,
AStarShortestPathHeuristics heutistics) {
/*
* Cost from start along best known path.
*/
Map<NodeInfo, Double> gScore = new HashMap<>();
/*
* Estimated total cost from start to goal through a node.
*/
Map<NodeInfo, Double> fScore = new HashMap<>();
initialize(gScore, fScore, network, startNode, endNode, heutistics);
// System.out.println(gScore.toString());
// System.out.println(fScore.toString());
/*
* The set of nodes already evaluated.
*/
Set<NodeInfo> closedSet = new HashSet<>();
/*
* The set of tentative nodes to be evaluated, initially containing the start node
*/
Set<NodeInfo> openSet = new HashSet<>();
openSet.add(startNode);
// System.out.println(openSet.toString());
/*
* Priority queue contains nodes in openSet sorted by length from the start node
*/
PriorityQueue<NodeInfo> openPriorityQueue =
new PriorityQueue<>(new AStarNodeScoreComparator(fScore));
openPriorityQueue.add(startNode);
// System.out.println(openPriorityQueue.toString());
/*
* The map of navigated nodes.
*/
Map<Integer, Integer> traceMap = new HashMap<>();
/*
* The A* algorithm
*/
while (openPriorityQueue.isEmpty() == false) {
NodeInfo current = openPriorityQueue.remove();
// System.out.println("Current: " + current.toString());
if (current.getNodeId() == endNode.getNodeId())
return reconstructPath(endNode, traceMap, network);
openSet.remove(current);
closedSet.add(current);
// update neighbors
for (LinkInfo neighborLink : network.getLinks().get(current.getNodeId())) {
// Ignore the neighbor which is already evaluated.
NodeInfo neighborNode = network.getNodes().get(neighborLink.getEndNode());
// System.out.println("neighborNode: " + neighborNode.toString());
if (closedSet.contains(neighborNode.getNodeId())) continue;
// length of this path.
double tentativeGScore = gScore.get(current) + neighborLink.getCost();
if (openSet.contains(neighborNode) == false) {
// Discover a new node
openSet.add(neighborNode);
} else if (gScore.get(neighborNode.getNodeId()) <= tentativeGScore) {
// This is not a better path.
continue;
}
// System.out.println(" Put neighborNode: " + neighborNode.toString());
// This path is the best until now. Record it!
traceMap.put(neighborNode.getNodeId(), current.getNodeId());
gScore.put(neighborNode, tentativeGScore);
// length of this path + estimated length to the end node
double tentativeFScore =
tentativeGScore + heutistics.heuristics(neighborNode.getNodeId(), endNode.getNodeId());
fScore.put(neighborNode, tentativeFScore);
openPriorityQueue.add(neighborNode);
}
}
return null;
}
