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