|
|
|
@ -17,7 +17,12 @@ public class AStar { |
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/** |
|
|
|
|
* @return the nodes |
|
|
|
|
*/ |
|
|
|
|
public List<Node> getNodes() { |
|
|
|
|
// Variable initializations
|
|
|
|
|
List<Node> nodes = new ArrayList<Node>(); |
|
|
|
|
|
|
|
|
|
Set<Node> open = new HashSet<Node>(); |
|
|
|
@ -26,33 +31,40 @@ public class AStar { |
|
|
|
|
Node start = new Node(world, world.getStart().getX(), world.getStart().getY()); |
|
|
|
|
Node stop = new Node(world, world.getStop().getX(), world.getStop().getY()); |
|
|
|
|
|
|
|
|
|
// Calculate the initial attributes
|
|
|
|
|
start.setG(0); |
|
|
|
|
start.setH(manhattanDistance(start, stop)); |
|
|
|
|
start.setF(start.getH()); |
|
|
|
|
|
|
|
|
|
// Add the first node to the open set
|
|
|
|
|
open.add(start); |
|
|
|
|
|
|
|
|
|
while (true) { |
|
|
|
|
Node current = null; |
|
|
|
|
|
|
|
|
|
// Stop when there are no other nodes in the open set
|
|
|
|
|
if (open.size() == 0) { |
|
|
|
|
throw new RuntimeException("no route"); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// Find the node with the lowest F
|
|
|
|
|
for (Node node : open) { |
|
|
|
|
if (current == null || node.getF() < current.getF()) { |
|
|
|
|
current = node; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// Stop when the destination node is found
|
|
|
|
|
if (current.equals(stop)) { |
|
|
|
|
stop.setParent(current.getParent()); |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// Swtich the current node from open to closed list
|
|
|
|
|
open.remove(current); |
|
|
|
|
closed.add(current); |
|
|
|
|
|
|
|
|
|
// Add four adjacent neihbours
|
|
|
|
|
List<Node> neighbors = new ArrayList<Node>(); |
|
|
|
|
neighbors.add(new Node(world, current.getX() + world.getSquareEdgeLenght(), current.getY())); |
|
|
|
|
neighbors.add(new Node(world, current.getX() - world.getSquareEdgeLenght(), current.getY())); |
|
|
|
@ -61,18 +73,24 @@ public class AStar { |
|
|
|
|
current.setNeighbors(neighbors); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// Foreach through the neighbours
|
|
|
|
|
for (Node neighbor : current.getNeighbors()) { |
|
|
|
|
|
|
|
|
|
// Ignore occupied and out-of-the-map modes
|
|
|
|
|
if (neighbor == null || isOccupied(neighbor) || isOutsideMap(neighbor)) { |
|
|
|
|
continue; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// Recalculate the G
|
|
|
|
|
int nextG = current.getG() + neighbor.getCost(); |
|
|
|
|
|
|
|
|
|
// Comapre and use the one with lower G (better path)
|
|
|
|
|
if (nextG < neighbor.getG()) { |
|
|
|
|
open.remove(neighbor); |
|
|
|
|
closed.remove(neighbor); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// Add the new neighbour to the open set if not present
|
|
|
|
|
if (!open.contains(neighbor) && !closed.contains(neighbor)) { |
|
|
|
|
neighbor.setG(nextG); |
|
|
|
|
neighbor.setH(manhattanDistance(neighbor, stop)); |
|
|
|
@ -83,6 +101,7 @@ public class AStar { |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// Create the List of the path
|
|
|
|
|
Node current = stop; |
|
|
|
|
while (current.getParent() != null) { |
|
|
|
|
nodes.add(current); |
|
|
|
|