// Estimated total cost from start to goal through y.
fScore.put(start, (gScore.get(start) + chessPiece.getHeuristicMoves(start, end)));
// The open set initially contains the start node
Node startNode = new Node(start, fScore.get(start));
openSet.add(startNode);
// While we still have nodes to explore
while (!openSet.isEmpty()){
// Get the node with the lowest f-score from the priority queue
Node current = openSet.poll();
Square currentSquare = current.getSquare();
// If we've found the goal, return the path we took to get there
if (currentSquare.equals(end))
return reconstructPath(currentSquare);
// Add current to closedset, and move the chess piece to that square
closedSet.add(currentSquare);
chessPiece.move(currentSquare);
// For each neighbour from the current chess piece position
for (Square neighbour : chessPiece.getValidMoves()){
if (closedSet.contains(neighbour))
continue;
// A tentative g score, used to determine whether we explore a node or not
int tentativeGScore = gScore.get(currentSquare) + 1;
// Here we clone the openSet and modify the clone, as we are
// unable to iterate through a PriorityQueue.
PriorityQueue<Node> clone = new PriorityQueue<Node>(openSet);
boolean isInSet = false;
while (!clone.isEmpty()){
Node n = clone.poll();
if (n.getSquare().equals(neighbour)){
isInSet = true;
break;
}
}
// If neighbor not in openset or tentative_g_score < g_score[neighbor]
if (!isInSet || tentativeGScore < gScore.get(neighbour)){
// Calculate a new f-score for the square, and update its "path" (cameFrom)
cameFrom.put(neighbour, currentSquare);
gScore.put(neighbour, tentativeGScore);
fScore.put(neighbour, tentativeGScore + chessPiece.getHeuristicMoves(neighbour, end));
// Add the neighbour to the open set
Node n = new Node(neighbour, fScore.get(neighbour));
openSet.add(n);
}
}
}
// If no path was found, return failure