private Path aStar() {
init();
// While there are still node to explore
while (!openSet.isEmpty()) {
// Get the node with the lowest f_score value
Node currentNode = openSet.poll();
Square currentSquare = currentNode.getSquare();
// System.out.println(currentSquare.toString() + " openSet");
// If current square is the destination square, return the reconstructed path
if (currentSquare.equals(endPosition)) {
return reconstructPath(currentSquare);
}
closedSet.add(currentSquare);
piece.move(currentSquare);
// for (Square neighbourSquare : piece.getValidMoves()) {
// System.out.print(neighbourSquare + " ");
// }
// System.out.println();
// Explore the neighbours of the current square
for (Square neighbourSquare : piece.getValidMoves()) {
// System.out.println(neighbourSquare.toString() + " neighbour");
// If the current neighbour square has already been explored, pass
if (closedSet.contains(neighbourSquare)) {
continue;
}
// Get a tentative gScore, default the distance from 'piece' to 'neighbourSquare' to 1
// (step).
int tentativeGScore = gScore.get(currentSquare) + 1;
if ((!openSet.contains(neighbourSquare)) || tentativeGScore < gScore.get(neighbourSquare)) {
cameFrom.put(neighbourSquare, currentSquare);
gScore.put(neighbourSquare, tentativeGScore);
fScore.put(
neighbourSquare,
(tentativeGScore
+ ((AStarPiece) piece).getHeuristicMoves(neighbourSquare, endPosition)));
// Add this neighbour to the open set
Node newNode = new Node(neighbourSquare, fScore.get(neighbourSquare));
openSet.add(newNode);
}
}
}
// If no path is found, return null
return null;
}