private Queue<SearchNode> GetSolution(Board initial) {
Queue<SearchNode> queue = new Queue<SearchNode>();
MinPQ<SearchNode> pq = new MinPQ<SearchNode>();
SearchNode node = new SearchNode();
node.brd = initial;
node.moves = 0;
// node.priority = initial.manhattan();
node.prev = null;
pq.insert(node);
SearchNode curNode = null;
while (!pq.isEmpty()) {
curNode = pq.delMin();
if (curNode.brd.isGoal()) break;
Iterable<Board> qe = curNode.brd.neighbors();
Iterator it = qe.iterator();
while (it.hasNext()) {
node.brd = (Board) it.next();
node.moves = curNode.moves + 1;
node.prev = curNode;
// node.priority = node.brd.manhattan();
queue.enqueue(node);
}
}
if (queue.isEmpty()) return null;
else {
do {
queue.enqueue(curNode);
curNode = curNode.prev;
} while (curNode != null);
return queue;
}
}
public void solve(T start, T target) {
reset();
Set<T> visited = new HashSet<T>();
SearchNode best = new SearchNode(null, start, target);
Queue<SearchNode> openList = new PriorityQueue<>();
openList.add(best);
solutionFound = false;
while (openList.size() > 0 && !solutionFound) {
best = openList.poll();
if (debug) {
System.out.println("best: " + best.getObject());
}
if (!visited.contains(best.getObject())) {
visited.add(best.getObject());
if (best.getObject().achieves(target)) {
solutionFound = true;
} else {
addSuccessors(best, openList, target);
}
}
}
if (solutionFound) {
reconstructMoves(best);
}
}
public boolean leftChild(PrimitiveConstraint choice, boolean status) {
boolean returnCode = true;
if (exitChildListeners != null) {
boolean code = false;
for (int i = 0; i < exitChildListeners.length; i++)
code |= exitChildListeners[i].leftChild(choice, status);
returnCode = code;
}
currentSearchNode = searchStack.pop();
// SearchNode previousSearchNode = currentSearchNode;
if (!status && returnCode) {
currentSearchNode = new SearchNode();
// currentSearchNode.v = var;
// if (previousSearchNode.dom instanceof JaCoP.core.IntDomain)
// currentSearchNode.dom = ((IntDomain)previousSearchNode.dom).subtract( value );
// else if (previousSearchNode.dom instanceof JaCoP.set.core.SetDomain)
// currentSearchNode.dom = ((SetDomain)previousSearchNode.dom).subtract( value, value );
// currentSearchNode.val = value;
currentSearchNode.id = searchNodeId++;
currentSearchNode.equal = false;
currentSearchNode.c = choice;
currentSearchNode.previous = searchStack.peek().id;
searchStack.push(currentSearchNode);
}
return returnCode;
}
/** Returns the best move to make from here during a playout. */
short bestSearchMove(SearchNode node, McRunnable runnable) {
final Board runnableBoard = runnable.getBoard();
final MersenneTwisterFast random = runnable.getRandom();
short result = node.getWinningMove();
if (result != NO_POINT && runnableBoard.isLegal(result)) {
// The isLegal() check is necessary to avoid superko violations
return result;
}
float bestSearchValue = searchValue(node, PASS);
result = PASS;
final ShortSet vacantPoints = runnableBoard.getVacantPoints();
int start;
start = random.nextInt(vacantPoints.size());
int i = start;
final int skip = PRIMES[random.nextInt(PRIMES.length)];
do {
final short move = vacantPoints.get(i);
final float searchValue = searchValue(node, move);
if (searchValue > bestSearchValue) {
if (runnable.isFeasible(move) && runnableBoard.isLegal(move)) {
bestSearchValue = searchValue;
result = move;
} else {
node.exclude(move);
}
}
// Advancing by a random prime skips through the array
// in a manner analogous to double hashing.
i = (i + skip) % vacantPoints.size();
} while (i != start);
return result;
}
public static SearchResult generic(SearchProblem prob, Comparator<SearchNode> cmp) {
PriorityQueue<SearchNode> frontier = new PriorityQueue<SearchNode>(10000, cmp);
frontier.offer(prob.startNode());
Set<State> explored = new HashSet<State>();
while (!frontier.isEmpty()) {
SearchNode candidate = frontier.poll();
if (candidate.isGoal()) {
return new SearchResult(candidate, explored.size(), frontier.size());
}
List<Action> nextActions = prob.actions(candidate.state);
for (Action action : nextActions) {
SearchNode child = prob.childNode(candidate, action);
if (!explored.contains(child.state)) {
frontier.offer(child);
explored.add(candidate.state);
}
}
}
return null;
}
private void addSuccessors(SearchNode best, Queue<SearchNode> openList, T target) {
for (T p : best.getObject().getSuccessors()) {
numNodes++;
SearchNode newNode = new SearchNode(best, p, target);
depth = Math.max(newNode.getDepth(), depth);
openList.add(newNode);
}
}
public void testSmall() {
State node5 = new State(5, 0, 0, true);
State node6 = new State(6, 0, 0, true);
State node2 = new State(2, 1, 6, false);
node2.next[0] = node5;
node2.cost[0] = 6;
State node3 = new State(3, 2, 2, false);
node3.next[0] = node5;
node3.cost[0] = 4;
node3.next[1] = node6;
node3.cost[1] = 2;
State node4 = new State(4, 1, 6, false);
node4.next[0] = node6;
node4.cost[0] = 6;
State node0 = new State(0, 2, 4, false);
node0.next[0] = node2;
node0.cost[0] = 2;
node0.next[1] = node3;
node0.cost[1] = 2;
State node1 = new State(1, 2, 3, false);
node1.next[0] = node3;
node1.cost[0] = 1;
node1.next[1] = node4;
node1.cost[1] = 1;
State[][] paths = new State[6][];
double[] costs = new double[6];
paths[0] = new State[] {node6, node3, node1};
costs[0] = 3;
paths[1] = new State[] {node6, node3, node0};
costs[1] = 4;
paths[2] = new State[] {node5, node3, node1};
costs[2] = 5;
paths[3] = new State[] {node5, node3, node0};
costs[3] = 6;
paths[4] = new State[] {node6, node4, node1};
costs[4] = 7;
paths[5] = new State[] {node5, node2, node0};
costs[5] = 8;
AStar s = new AStar(new State[] {node0, node1}, 7);
int i = 0;
while (s.hasNext()) {
assertTrue("number of answers > " + i, i < 6);
SearchNode n = s.nextAnswer();
assertEquals("costs[" + i + "] != " + n.getPriority(), costs[i], n.getPriority(), 1e-5);
int j = 0;
while (n != null) {
assertTrue("path length > " + j, j < 3);
assertTrue("path[" + i + "][" + j + "] != " + n, paths[i][j] == n.getState());
j++;
n = (SearchNode) n.getParent();
}
assertTrue("path length != " + j, j == 3);
i++;
}
assertTrue("number of answers != " + i, i == 6);
}
private int[] solve() {
SearchNode s;
// Symmetry constraint: color[start] = 0
s = branch(new SearchNode(), 0, 0, null);
if (s != null) return s.solution();
else {
int[] cs = new int[V];
for (int v = 0; v < V; v++) cs[v] = -1;
return cs;
}
}
public int getChoiceValue() {
selectedValue = select.getChoiceValue();
currentSearchNode.val = selectedValue;
currentSearchNode.id = searchNodeId++;
currentSearchNode.previous = searchStack.peek().id;
searchStack.push(currentSearchNode);
return selectedValue;
}
public T getChoiceVariable(int index) {
selectedVar = select.getChoiceVariable(index);
if (selectedVar != null) {
currentSearchNode = new SearchNode();
currentSearchNode.v = selectedVar;
currentSearchNode.dom = selectedVar.dom().cloneLight();
}
return selectedVar;
}
private void reconstructMoves(SearchNode searcher) {
while (searcher != null) {
solution.add(searcher.getObject());
searcher = searcher.getParent();
}
for (int i = 0; i < solution.size() / 2; ++i) {
T temp = solution.get(i);
int other = solution.size() - i - 1;
solution.set(i, solution.get(other));
solution.set(other, temp);
}
}
/** Some nodes may have their biases updated. */
@Override
public void descend(McRunnable runnable) {
SearchNode node = getRoot();
assert node != null : "Fancy hash code: " + board.getFancyHash();
while (runnable.getBoard().getPasses() < 2) {
selectAndPlayMove(node, runnable);
final SearchNode child = table.findIfPresent(runnable.getBoard().getFancyHash());
if (child == null) {
return; // No child
}
if (child.getTotalRuns() > biasDelay && !child.biasUpdated()) {
child.updateBias(runnable);
}
node = child;
}
}
SearchNode(Board b, SearchNode prevNode) {
board = b;
prev = prevNode;
existingMoves = prevNode.getMoves() + 1;
manhattan = b.manhattan();
priority = existingMoves + manhattan;
}
@Override
public void fakeDescend(McRunnable runnable, short... moves) {
runnable.copyDataFrom(board);
final SearchNode node = getRoot();
assert node != null : "Fancy hash code: " + board.getFancyHash();
for (final short move : moves) {
System.out.println("Passing " + move + " to runnable");
runnable.acceptMove(move);
final SearchNode child = table.findIfPresent(runnable.getBoard().getFancyHash());
if (child == null) {
return; // No child
}
if (child.getTotalRuns() > biasDelay && !child.biasUpdated()) {
child.updateBias(runnable);
}
}
}
/**
* It creates a CPviz trace generator around proper select choice point object.
*
* @param select it specifies how the select choice points are being generated.
* @param vars it specifies variables which are being traced.
* @param treeFilename it specifies the file name for search tree trace (default tree.xml).
* @param visFilename it specifies the file name for variable trace (default vis.xml).
*/
private TraceGenerator(
SelectChoicePoint<T> select, Var[] vars, String treeFilename, String visFilename) {
this.select = select;
this.treeFilename = treeFilename;
this.visFilename = visFilename;
SearchNode rootNode = new SearchNode();
rootNode.id = 0;
searchStack.push(rootNode);
// for (Var v : vars) {
for (int i = 0; i < vars.length; i++) {
tracedVar.add(vars[i]);
varIndex.put(vars[i], i);
}
prepareTreeHeader();
prepareVizHeader();
}
public PrimitiveConstraint getChoiceConstraint(int index) {
PrimitiveConstraint c = select.getChoiceConstraint(index);
if (c == null) {
generateSuccessNode(currentSearchNode.id);
generateVisualizationNode(currentSearchNode.id, true);
} else {
currentSearchNode = new SearchNode();
currentSearchNode.c = c;
currentSearchNode.id = searchNodeId++;
currentSearchNode.previous = searchStack.peek().id;
searchStack.push(currentSearchNode);
}
return c;
}
// sequence of boards in a shortest solution; null if no solution
public Iterable<Board> solution() {
if (solution == null) {
return null;
}
List<Board> resultList = new LinkedList<Board>();
// Spec stated that Collections.reverse() is not allowed for this assignment.
// Using Stack to reverse the list.
Stack<SearchNode> stack = new Stack<SearchNode>();
SearchNode node = solution;
while (node != null) {
stack.push(node);
node = node.getPrev();
}
while (!stack.isEmpty()) {
resultList.add(stack.pop().getBoard());
}
return resultList;
}
// find a solution to the initial board (using the A* algorithm)
public Solver(Board initial) {
Board twin = initial.twin();
SearchNode minNode = null;
boolean isTwinRound = false;
MinPQ<SearchNode> currentPQ = null;
minPQ.insert(new SearchNode(initial));
minPQForTwin.insert(new SearchNode(twin));
while (true) {
// Searching solution in the initial board and and its twin board
// simultaneously(alternating in loops).
// It has been proven by Math theory that exactly one of the two
// will lead to the goal board. If a solution is found in the twin
// board, it immediately proves that the initial board is not solvable,
// and the search will terminate there.
// Otherwise, the initial board will reach a solution.
if (isTwinRound) {
currentPQ = minPQForTwin;
} else {
currentPQ = minPQ;
}
minNode = currentPQ.delMin();
if (minNode.getBoard().isGoal()) {
break;
} else {
for (Board neighbor : minNode.getBoard().neighbors()) {
// Insert the neighbors into the MinPQ if:
// 1. Current node contains the initial board(has no prev node)
// 2. The new neighbor is not the same as current node's previous search node.
// This is a critical optimization used to reduce unnecessary
// exploration of already visited search nodes.
if (minNode.getPrev() == null || !neighbor.equals(minNode.getPrev().getBoard())) {
currentPQ.insert(new SearchNode(neighbor, minNode));
}
}
// Flip the state of the isTwinRound flag
isTwinRound = isTwinRound ? false : true;
}
}
if (isTwinRound) {
isSolvable = false;
solution = null;
} else {
isSolvable = true;
solution = minNode;
moves = solution.getMoves();
}
}
@Override
public short bestPlayMove() {
double mostWins = 1;
short result = PASS;
final ShortSet vacantPoints = board.getVacantPoints();
final SearchNode root = getRoot();
do {
mostWins = root.getWins(PASS);
// If the move chosen on the previous pass through this loop was
// illegal (e.g., because it was never actually tried in a playout),
// throw it out
if (result != PASS) {
log("Rejected " + board.getCoordinateSystem().toString(result) + " as illegal");
root.exclude(result);
result = PASS;
}
for (int i = 0; i < vacantPoints.size(); i++) {
final short move = vacantPoints.get(i);
if (root.getWins(move) > mostWins) {
mostWins = root.getWins(move);
result = move;
}
}
} while (result != PASS && !board.isLegal(result));
// Consider resigning
if (root.getWinRate(result) < RESIGN_PARAMETER) {
return RESIGN;
}
log(
"Selected "
+ board.getCoordinateSystem().toString(result)
+ " with "
+ root.getWins(result)
+ " wins in "
+ root.getRuns(result)
+ " runs");
return result;
}
public boolean leftChild(T var, int value, boolean status) {
boolean returnCode = true;
if (exitChildListeners != null) {
boolean code = false;
for (int i = 0; i < exitChildListeners.length; i++)
code |= exitChildListeners[i].leftChild(var, value, status);
returnCode = code;
}
currentSearchNode = searchStack.pop();
SearchNode previousSearchNode = currentSearchNode;
if (!status && returnCode) {
currentSearchNode = new SearchNode();
currentSearchNode.v = var;
if (previousSearchNode.dom
instanceof uk.ac.stir.cs.homer.homerPolicyServer.overlap.JaCoP.core.IntDomain)
currentSearchNode.dom = ((IntDomain) previousSearchNode.dom).subtract(value);
else if (previousSearchNode.dom
instanceof uk.ac.stir.cs.homer.homerPolicyServer.overlap.JaCoP.set.core.SetDomain)
currentSearchNode.dom = ((SetDomain) previousSearchNode.dom).subtract(value, value);
currentSearchNode.val = value;
currentSearchNode.id = searchNodeId++;
currentSearchNode.equal = false;
currentSearchNode.previous = searchStack.peek().id;
searchStack.push(currentSearchNode);
}
return returnCode;
}
@Override
public int compareTo(SearchNode o) {
return (int) Math.signum(priority() - o.priority());
}
public SearchNode bugSearch(MapLocation start, MapLocation target) throws GameActionException {
MapLocation s = start, t = target;
int closest = s.distanceSquaredTo(t);
int closestL = closest, closestR = closest;
SearchNode current = new SearchNode(start, 1, null, true),
currentL = new SearchNode(start, 1, null, true),
currentR = new SearchNode(start, 1, null, true);
boolean isTracingL = false, isTracingR = false;
Direction curDir = current.loc.directionTo(t);
Direction curDirL = curDir, curDirR = curDir;
if (debug) System.out.println("Source: " + s + "; Target: " + t);
while (!(current.loc.x == t.x && current.loc.y == t.y)
&& !(currentL.loc.x == t.x && currentL.loc.y == t.y)
&& !(currentR.loc.x == t.x && currentR.loc.y == t.y)) {
if (debug)
System.out.println(
"Current: " + current.loc + ";Right " + currentR.loc + ";Left " + currentL.loc);
if (!isTracingL || !isTracingR) {
// we're done tracing
if (isTracingL) {
// the right bug finished first
isTracingL = false;
current = currentR;
closest = closestR;
} else if (isTracingR) {
// the left bug finished first
isTracingR = false;
current = currentL;
closest = closestL;
}
curDir = directionTo(current.loc, t);
if (curDir != null) {
current = current.update(curDir);
} else {
current.isPivot = true;
closest = current.loc.distanceSquaredTo(t);
closestL = closest;
closestR = closest;
isTracingL = true;
isTracingR = true;
curDir = current.loc.directionTo(t);
curDirL = curDir;
curDirR = curDir;
while (!isGood(current.loc.add(curDirL))) {
curDirL = curDirL.rotateRight();
}
while (!isGood(current.loc.add(curDirR))) {
curDirR = curDirR.rotateLeft();
}
currentL = current.update(curDirL);
currentR = current.update(curDirR);
if (currentL.loc.distanceSquaredTo(t) < closestL)
closestL = currentL.loc.distanceSquaredTo(t);
if (currentR.loc.distanceSquaredTo(t) < closestR)
closestR = currentR.loc.distanceSquaredTo(t);
}
} else { // we're tracing
if (currentL.loc.distanceSquaredTo(t) < currentR.loc.distanceSquaredTo(t)) {
if (debug) System.out.println("LEFT TRACE");
// the left trace is closer
curDirL = directionTo(currentL.loc, t);
if (curDirL != null && currentL.loc.add(curDirL).distanceSquaredTo(t) < closestL) {
if (debug) System.out.print("FINISH LEFT TRACE. GOING " + curDirL);
isTracingL = false;
currentL.isPivot = true;
if (debug) System.out.println("LEFT PIVOT");
currentL = currentL.update(curDirL);
if (currentL.loc.distanceSquaredTo(t) < closestL)
closestL = currentL.loc.distanceSquaredTo(t);
} else {
curDirL = currentL.loc.directionTo(currentL.prevLoc.loc).rotateRight().rotateRight();
int i = 2;
while (!isGood(currentL.loc.add(curDirL))) {
curDirL = curDirL.rotateRight();
i++;
}
if (i < 4
|| curDirL != Direction.EAST
&& curDirL != Direction.WEST
&& curDirL != Direction.NORTH
&& curDirL != Direction.SOUTH) {
currentL.isPivot = true;
if (debug) System.out.println("LEFT PIVOT");
}
if (curDirL != directionTo(currentL.prevLoc.loc, currentL.loc)) {
currentL.isPivot = true;
if (debug) System.out.println("LEFT PIVOT");
}
currentL = currentL.update(curDirL);
if (currentL.loc.distanceSquaredTo(t) < closestL)
closestL = currentL.loc.distanceSquaredTo(t);
}
} else {
// the right trace is closer
if (debug) System.out.println("RIGHT TRACE");
curDirR = directionTo(currentR.loc, t);
if (curDirR != null && currentR.loc.add(curDirR).distanceSquaredTo(t) < closestR) {
if (debug) System.out.println("FINISH RIGHT TRACE. GOING " + curDirR);
isTracingR = false;
currentR.isPivot = true;
if (debug) System.out.println("RIGHT PIVOT");
currentR = currentR.update(curDirR);
if (currentR.loc.distanceSquaredTo(t) < closestR)
closestR = currentR.loc.distanceSquaredTo(t);
} else {
curDirR = currentR.loc.directionTo(currentR.prevLoc.loc).rotateLeft().rotateLeft();
int i = 2;
while (!isGood(currentR.loc.add(curDirR))) {
curDirR = curDirR.rotateLeft();
i++;
}
if (i < 4
|| curDirR != Direction.EAST
&& curDirR != Direction.WEST
&& curDirR != Direction.NORTH
&& curDirR != Direction.SOUTH) {
currentR.isPivot = true;
if (debug) System.out.println("RIGHT PIVOT");
}
if (curDirR != directionTo(currentR.prevLoc.loc, currentR.loc)) {
currentR.isPivot = true;
if (debug) System.out.println("RIGHT PIVOT");
}
currentR = currentR.update(curDirR);
if (currentR.loc.distanceSquaredTo(t) < closestR)
closestR = currentR.loc.distanceSquaredTo(t);
}
}
}
}
current.isPivot = true;
currentL.isPivot = true;
currentR.isPivot = true;
if (current.loc.equals(t)) {
return current;
}
if (currentL.loc.equals(t)) {
return currentL;
}
if (currentR.loc.equals(t)) {
return currentR;
}
throw new GameActionException(null, "Unable to find a path from " + s + " to " + t);
}
private void outputSN(SearchNode sn) {
System.out.println("------------");
System.out.println(sn.board.toString());
System.out.println("Moves = " + sn.moves + " Prio = " + sn.priority());
}
/** Compare the SearchNode with the anchor one by priority value. */
public int compareTo(SearchNode that) {
if (this.priority() < that.priority()) return -1;
else if (this.priority() > that.priority()) return 1;
else return 0;
}
/**
* Performs a bi-directional best-first graph search on the tf graph to try to find a path from
* sourceFrame to targetFrame, at the given time. One priority queue is used to keep a sorted list
* of all search nodes (from both directions, ordered descending by their potential of
* contributing to a good solution). At the moment, the cost of the path from A to B is defined as
* the largest absolute difference between the time stamps of the transforms from A to B and the
* given time point. This corresponds to searching for a transform path that needs the least
* amount of inter- and extrapolation.
*
* <p>Note: often in search, if we talk about expanding a search node, we say that the node
* expands and its _children_ are added to the queue. Yet, the tf graph is stored by linking child
* frames to their _parent_ frames, not the other way around. So, if a search node is expanded,
* the _parent_ frames are added to the queue. This may be a bit confusing.
*/
protected boolean lookupLists(
Frame targetFrame,
Frame sourceFrame,
long time,
LinkedList<TransformStorage> inverseTransforms,
LinkedList<TransformStorage> forwardTransforms) {
// wrap the source and target frames in search nodes
SearchNode<Frame> sourceNode = new SearchNode<Frame>(sourceFrame);
SearchNode<Frame> targetNode = new SearchNode<Frame>(targetFrame);
// set beginning of forward path (from source)
sourceNode.backwardStep = sourceNode;
// set beginning of backward path (form target)
targetNode.forwardStep = targetNode;
// create a hash map that map frames to search nodes. This is necessary to keep track of
// which frames have already been visited (and from which direction).
HashMap<Frame, SearchNode<Frame>> frameToNode = new HashMap<Frame, SearchNode<Frame>>();
// add source and target search nodes to the map
frameToNode.put(sourceFrame, sourceNode);
frameToNode.put(targetFrame, targetNode);
// create a priority queue, which will hold the search nodes ordered by cost (descending)
PriorityQueue<SearchNode<Frame>> Q = new PriorityQueue<SearchNode<Frame>>();
// at the source and target search nodes to the queue
Q.add(sourceNode);
Q.add(targetNode);
// perform the search
while (!Q.isEmpty()) {
// poll most potential search node from queue
SearchNode<Frame> frameNode = Q.poll();
Frame frame = frameNode.content;
// if the node is both visited from the source and from the target node, a path has been found
if (frameNode.backwardStep != null && frameNode.forwardStep != null) {
// found the best path from source to target through FRAME.
// create inverse list (from source to FRAME)
SearchNode<Frame> node = frameNode;
while (node.content != sourceNode.content) {
inverseTransforms.addLast(node.backwardStep.content.getData(time, node.content));
node = node.backwardStep;
}
// create forward list (from FRAME to target)
node = frameNode;
while (node.content != targetNode.content) {
forwardTransforms.addLast(node.forwardStep.content.getData(time, node.content));
node = node.forwardStep;
}
return true;
}
// expand search node
for (Frame parentFrame : frame.getParentFrames()) {
SearchNode<Frame> parentFrameNode = frameToNode.get(parentFrame);
boolean addToQueue = false;
if (parentFrameNode == null) {
// node was not yet visited
parentFrameNode = new SearchNode<Frame>(parentFrame);
frameToNode.put(parentFrame, parentFrameNode);
addToQueue = true;
} else {
// node is already visited
if ((parentFrameNode.backwardStep == null && frameNode.forwardStep == null)
|| (parentFrameNode.forwardStep == null && frameNode.backwardStep == null)) {
// node was visited, but from other direction.
// create new search node that represents this frame, visited from both sides
// this allows the other search node of this frame to still be expanded first
parentFrameNode = new SearchNode<Frame>(parentFrameNode);
addToQueue = true;
}
}
// add search node belonging to parent frame to the queue
if (addToQueue) {
// determine cost (based on max absolute difference in time stamp)
TimeCache cache = frame.getTimeCache(parentFrame);
parentFrameNode.cost =
Math.max(
(double) cache.timeToNearestTransform(time),
Math.max(parentFrameNode.cost, frameNode.cost));
// if visiting forward (from source), set backward step to remember path
if (frameNode.backwardStep != null) parentFrameNode.backwardStep = frameNode;
// if visiting backward (from target), set forward step to remember path
if (frameNode.forwardStep != null) parentFrameNode.forwardStep = frameNode;
// add node to queue
Q.add(parentFrameNode);
}
}
}
// target and source frames are not connected.
return false;
}
/** Branch and bound. */
private SearchNode branch(SearchNode s, int v, int color, SearchNode best) {
SearchNode newBest, nextTry;
assert s != null && v >= 0 && color >= 0 && v < V && color < V;
assert best != null || v == 0 && color == 0; // best only null if first call.
if (!s.setColor(v, color)) return null;
if (best != null && s.bound() > best.maxColor()) return best;
if (best == null || s.maxColor() < best.maxColor()) newBest = s;
else newBest = best;
if (!newBest.solved()) {
for (int w = v; w < V; w++) {
for (int nextColor : s.nextColors(w, newBest.maxColor())) {
nextTry = branch(new SearchNode(s), w, nextColor, newBest);
if (nextTry != null && nextTry.maxColor() < newBest.maxColor()) newBest = nextTry;
}
}
}
return newBest;
}
@Override
public int compareTo(SearchNode that) {
return this.priority() - that.priority();
}
private static double getHCost(int choice, SearchNode crntNode) {
switch (choice) {
case 1:
// h1: straight line distance heuristic function.
return Math.sqrt(
Math.pow((goalNode.getxPos() - crntNode.getxPos()), 2)
+ Math.pow((goalNode.getyPos() - crntNode.getyPos()), 2));
case 2:
// h2: the sum of the displacement along the x and y axes heuristic function.
return Math.abs(goalNode.getxPos() - crntNode.getxPos())
+ Math.abs(goalNode.getyPos() - crntNode.getyPos());
case 3:
// h3: 0.5h1 + 0.5h2
return 0.5
* (Math.sqrt(
Math.pow((goalNode.getxPos() - crntNode.getxPos()), 2)
+ Math.pow((goalNode.getyPos() - crntNode.getyPos()), 2)))
+ 0.5
* ((goalNode.getxPos() - crntNode.getxPos())
+ (goalNode.getyPos() - crntNode.getyPos()));
default:
return Math.sqrt(
Math.pow((goalNode.getxPos() - crntNode.getxPos()), 2)
+ Math.pow((goalNode.getyPos() - crntNode.getyPos()), 2));
}
}
public static void main(String[] args) throws Exception {
// Creates the grid and prints it.
System.out.println("Enter the nn value");
nn = keyboard.nextInt();
System.out.println("Enter the mm value");
mm = keyboard.nextInt();
System.out.println("Enter the max cost range value");
maxCostRange = keyboard.nextInt();
System.out.println("Enter the p value");
p = keyboard.nextDouble();
// Ask the user which heuristic function they want to use.
do {
System.out.println("\nMake a choice");
System.out.println("-------------");
System.out.println("1-Heuristic Function (h1) = Straight-line distance");
System.out.println("2-Heuristic Function (h2) = Sum of displacement in x and y axes");
System.out.println("3-Heuristic Function (h3) = 0.5h1+0.5h2");
heuristicChoice = keyboard.nextInt();
} while (heuristicChoice != 1 && heuristicChoice != 2 && heuristicChoice != 3);
// Print the random maze generated.
System.out.print("\nRandom Maze");
System.out.println("\n-----------");
g = new GridProblem(nn, mm, maxCostRange, p);
g.print();
// Set the start and goal states.
startState = g.startState;
goalState = g.goalState;
startNode = new SearchNode(startState, null);
goalNode = new SearchNode(goalState, null);
startNode.setFscore(0 + getHCost(heuristicChoice, startNode));
open.offer(startNode);
stateToNode.put(startState, startNode);
// The while loop executes until we haven't reach the goal state and
// there are still search nodes in the priority queue.
while (!open.isEmpty()) {
// Get the first element from the priority queue.
crntNode = open.peek();
// We've reached the destination. We print the path leading to the goal node
// and return.
if (g.isGoal(crntNode.getGridState())) {
System.out.println("\nOutput");
System.out.println("------");
constructPath(crntNode);
return;
}
// Get the node that is visited next. The neighbors (those that can be reached with legal
// operations) of this node will be added to the queue as well.
crntNode = open.poll();
crntState = crntNode.getGridState();
closed.add(crntNode);
a = g.getLegalOps(crntNode.getGridState());
// Goes through all of the neighbors of the current node. The neighbors get added to the queue
// if they may potentially be on the optimal path to the goal node.
for (int i = 0; i < a.size(); i++) {
// Gets an operation to perform on the current state and gets the next state
// after this operation is performed.
op = a.get(i);
nxtState = g.nextState((State) crntState, op);
// If the next state returned is equal to the current state, then it must be a wall
// so we ignore this operation.
if (nxtState == crntState) {
continue;
}
// Check to see whether a search node has already been created for the current state.
nxtNode = stateToNode.get(nxtState);
if (nxtNode == null) {
nxtNode = new SearchNode(nxtState, crntNode);
stateToNode.put(nxtState, nxtNode);
}
// Compute a temporary gScore for the neighbor.
tmpGScore = crntNode.getgScore() + g.cost(crntState, op);
// Check to see whether the gScore for the neighbor is greater than
// the gScore calculated in the above operation. If it is, any search nodes
// present in the priority queue or list will have to be removed as it may
// potentially be on the optimal path.
if (tmpGScore < nxtNode.getgScore()) {
if (open.contains(nxtNode)) {
open.remove(nxtNode); // The given node can be reached from a more optimal path.
}
if (closed.contains(nxtNode)) {
closed.remove(
nxtNode); // A node already considered can be reached from a more optimal path.
}
}
// Adjust/set the cost of the neighbor and add it to the priority queue.
if (!open.contains(nxtNode) && !closed.contains(nxtNode)) {
nxtNode.setParent(crntNode); // Set the parent for the given node.
nxtNode.setOperator(op);
nxtNode.setgScore(tmpGScore); // Set the new cost to get to this node from the start node.
nxtNode.setFscore(
nxtNode.getgScore()
+ getHCost(
heuristicChoice,
nxtNode)); // Set the estimated cost to get from this node to the goal node.
open.offer(nxtNode); // Add the node to the priority queue.
}
}
}
// There are no more search nodes in the priority queue and we haven't reached the goal
// node yet. Thus, there is no valid path from the start node to the goal node.
System.out.println("No path exists!");
}
private static void constructPath(SearchNode searchNode) {
if (searchNode.getParent() == null) { // We're at the start node
System.out.println("(i,j)=(" + searchNode.getxPos() + "," + searchNode.getyPos() + ")");
System.out.println("F cost: " + searchNode.getFscore());
System.out.println("G cost: " + searchNode.getgScore());
return;
}
constructPath(searchNode.getParent());
searchNode.getOperator().print();
System.out.println();
System.out.println("(i,j)=(" + searchNode.getxPos() + "," + searchNode.getyPos() + ")");
System.out.println("F cost: " + searchNode.getFscore());
System.out.println("G cost: " + searchNode.getgScore());
if (g.isGoal(searchNode.getGridState())) {
System.out.println("\nFinal cost is: " + searchNode.getgScore());
}
}
