private static void solve(State initState) {
Set<State> visited = new HashSet<State>();
Map<State, State> pred = new HashMap<State, State>();
Map<State, Integer> dist = new HashMap<State, Integer>();
Deque<State> bfs = new ArrayDeque<State>();
bfs.offer(initState);
dist.put(initState, 0);
while (bfs.size() > 0) {
State s = bfs.poll();
int n = dist.get(s);
visited.add(s);
if (s.isFinal()) {
outputState(s, pred);
return;
}
for (State child : s.getChildren()) {
if (visited.contains(child)) continue;
if (!pred.containsKey(child)) pred.put(child, s);
if (!dist.containsKey(child)) {
dist.put(child, n + 1);
bfs.offer(child);
}
}
}
System.out.printf("%d %d %d\n", initState.a, initState.b, initState.c);
System.out.println("============");
}
private static void _connectAll(StateQueue queue, State s, String str) {
if (queue.toConnect.size() > 0) {
for (State st : queue.toConnect) {
st.isFinal = false;
st.connect(s, str, false);
}
queue.toConnect.clear();
}
}
/**
* A* algorithm
*
* @param vehicle the considered vehicle
* @param available the available tasks of the world
* @param carried the tasks picked up and currently carried by the vehicle
* @return a plan (list of actions)
*/
public static Plan computeAStar(Vehicle vehicle, TaskSet available, TaskSet carried) {
Plan plan = null;
ArrayList<Task> availableTasks = new ArrayList<Task>(available);
ArrayList<Task> carriedTasks = new ArrayList<Task>(carried);
PriorityQueue<State> Q =
new PriorityQueue<State>(1, new StateComparator()); // States we will have to visit
ArrayList<State> C = new ArrayList<State>(); // States we have already visited, prevent cycles
City initialCity = vehicle.getCurrentCity();
State initialState =
new State(
initialCity, availableTasks, carriedTasks, new ArrayList<Action>(), vehicle, 0, 0);
Q.add(initialState);
boolean foundFinalState = false;
State finalState = null;
while (!foundFinalState) {
if (Q.isEmpty()) {
foundFinalState = true;
} else {
State visitingState = Q.poll();
if (visitingState.isFinal()) {
finalState = visitingState;
foundFinalState = true;
} else if (!C.contains(visitingState)
|| (C.contains(visitingState)
&& C.get(C.indexOf(visitingState)).heuristicValue > visitingState.heuristicValue)) {
C.add(visitingState);
Q.addAll(visitingState.next()); // Hopefully at the end of the list
}
}
}
if (finalState != null) {
plan = new Plan(vehicle.getCurrentCity(), finalState.actionList);
}
return plan;
}
@Test
public void shouldCheckIfStateIsFinalAndReturnFalse() throws Exception {
assertFalse(state.isFinal());
}
