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