/**
* Initialize gScore and fScore
*
* @param gScore
* @param fScore
* @param network
* @param startNode
* @param endNode
* @param heutistics
*/
public static void initialize(
Map<NodeInfo, Double> gScore,
Map<NodeInfo, Double> fScore,
LogicNetwork network,
NodeInfo startNode,
NodeInfo endNode,
AStarShortestPathHeuristics heutistics) {
for (NodeInfo node : network.getNodes()) {
gScore.put(node, Double.POSITIVE_INFINITY);
fScore.put(node, Double.POSITIVE_INFINITY);
}
gScore.put(startNode, 0.0);
fScore.put(startNode, heutistics.heuristics(startNode.getNodeId(), endNode.getNodeId()));
}
/**
* Reconstruct the path from start node to end node
*
* @param result
* @param endNode
* @param traceMap
* @return the path or {@code null} if error occurred
*/
public static PathInfo reconstructPath(
NodeInfo endNode, Map<Integer, Integer> traceMap, LogicNetwork network) {
PathInfo result = new PathInfo();
int current = endNode.getNodeId();
ArrayList<LinkInfo> tmpLinks = new ArrayList<>(); // the links in the reverse order
while (traceMap.containsKey(current)) {
int previous = traceMap.get(current);
LinkInfo linkInfo = getLink(previous, current, network);
tmpLinks.add(linkInfo);
current = previous;
}
// put the links in the correct order
for (int i = tmpLinks.size() - 1; 0 <= i; i--) {
result.getLinks().add(tmpLinks.get(i));
}
// calculate values properties of the path
double pathCost = 0.0;
double attractiveness = 0.0;
for (LinkInfo linkInfo : result.getLinks()) {
pathCost += linkInfo.getCost();
attractiveness += linkInfo.getAttractiveness();
result.getNodes().add(network.getNodes().get(linkInfo.getStartNode()));
}
result.setPathCost(pathCost);
result.setAttractiveness(attractiveness);
result.setNumLinks(result.getLinks().size());
result.setSimple(true);
result.getNodes().add(endNode);
return result;
}
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;
}