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()); } }