@SuppressWarnings("unchecked")
private P run(AStarStorage storage, AStarGoal<N> goal, N start, int maxIterations) {
Preconditions.checkNotNull(goal);
Preconditions.checkNotNull(start);
Preconditions.checkNotNull(storage);
N node;
int iterations = 0;
while (true) {
node = (N) storage.removeBestNode();
if (node == null) {
return null;
}
if (goal.isFinished(node)) {
return (P) node.buildPlan();
}
storage.close(node);
for (AStarNode neighbour : node.getNeighbours()) {
f(goal, node, (N) neighbour);
if (!storage.shouldExamine(neighbour)) continue;
storage.open(neighbour);
neighbour.parent = node;
}
if (maxIterations >= 0 && iterations++ >= maxIterations) {
return null;
}
}
}
public void printCurrentList() {
PriorityQueue<Node> pq = new PriorityQueue<Node>(nodes);
AStarNode p;
System.out.print("\n[");
while ((p = (AStarNode) pq.poll()) != null) {
System.out.print("\n");
p.printNode();
}
System.out.println("]");
}
public AStarNode createStartNode(Object state) {
Position p = (Position) state;
AStarNode n = new AStarNode(p);
n.setDistTravelled(0);
n.setApproxTotalDist(p.distance((Position) getEndingState()));
List<Position> path = new ArrayList<Position>();
path.add(p);
n.setPath(path);
// System.out.print("createStartNode"); n.printNode();
return n;
}
public List<Node> expandFunc(Node n) {
AStarNode node = (AStarNode) n;
// loop computes the positions you can get to from node
List<Node> expandedNodes = new ArrayList<Node>();
List<Position> path = node.getPath();
Position pos = node.getPosition();
int x = pos.getX();
int y = pos.getY();
// this next pair of loops will create all the possible moves
// from pos.
for (int i = -1; i <= 1; i += 2) { // Checks nodes to left and right
// create the potential next position
int nextX = x + i;
int nextY = y;
// make sure next point is on the grid
if ((nextX + 1 > grid.length || nextY + 1 > grid[0].length) || (nextX < 0 || nextY < 0))
continue;
Position next = new Position(nextX, nextY);
// System.out.println("considering"+next);
if (inPath(next, path) || !next.open(grid)) continue;
// printCurrentList();
// System.out.println("available"+next);
AStarNode nodeTemp = new AStarNode(next);
// update distance travelled
nodeTemp.setDistTravelled(node.getDistTravelled() + pos.distance(next));
// update approximate total distance to destination
// note that we are computing the straight-line
// heuristic on the fly right here from next to endingState
nodeTemp.setApproxTotalDist(next.distance((Position) endingState));
// update internal path
nodeTemp.updatePath(path);
expandedNodes.add(nodeTemp); // could have just added
// them directly to nodelist
}
for (int j = -1; j <= 1; j += 2) { // Checks nodes above and below
// create the potential next position
int nextX = x;
int nextY = y + j;
// make sure next point is on the grid
if ((nextX + 1 > grid.length || nextY + 1 > grid[0].length) || (nextX < 0 || nextY < 0))
continue;
Position next = new Position(nextX, nextY);
// System.out.println("considering"+next);
if (inPath(next, path) || !next.open(grid)) continue;
// printCurrentList();
// System.out.println("available"+next);
AStarNode nodeTemp = new AStarNode(next);
// update distance travelled
nodeTemp.setDistTravelled(node.getDistTravelled() + pos.distance(next));
// update approximate total distance to destination
// note that we are computing the straight-line
// heuristic on the fly right here from next to endingState
nodeTemp.setApproxTotalDist(next.distance((Position) endingState));
// update internal path
nodeTemp.updatePath(path);
expandedNodes.add(nodeTemp); // could have just added
// them directly to nodelist
}
return expandedNodes;
} // end expandFunc
public void addNewNode(Queue<Node> old, AStarNode m) {
for (Node n : old) {
if (m.getPosition() == ((AStarNode) n).getPosition()) return;
}
old.offer(m);
}