|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 |
private Function<Node, Double> heuristic; private Function<Node, Double> cost; public ASTAR(Function<Node, Double> heuristic, Function<Node, Double> cost) { this.heuristic = heuristic; this.cost = cost; } //A class to store heuristic and costs and also the possibility to compare each other. private class CostPair implements Comparable<CostPair> { private double heuristic; private double cost; private CostPair(double heuristic, double cost) { this.heuristic = heuristic; this.cost = cost; } @Override public int compareTo(CostPair other) { return Double.compare(heuristic + cost, other.heuristic + other.cost); } } public Node search(Node start, Predicate<Node> endPredicate) { // Check if the start node is the endPredicate if (endPredicate.test(start)) return start; // The queue containing all the nodes we have to search for StablePriorityQueue<CostPair, Node> nodeQueue = new StablePriorityQueue<>(); // Add the start node as the head. The priority does not matter since we // remove it anyway. nodeQueue.add(new Pair<>(new CostPair(heuristic.apply(start), cost.apply(start)), start)); // Adjacent, definition from the handbook.pdf contain all neighbors. // That means, that we would search in a node which we already searched for at another position. // Because of the insertion and lookup time O(1) in both situations, we chose the HashSt. HashSet<Node> checkedNodes = new HashSet<>(); // Repeat till we have no element in the queue left. while (nodeQueue.size() > 0) { Pair<CostPair, Node> currentNode = nodeQueue.poll(); if (endPredicate.test(currentNode.s)) return currentNode.s; // Nothing found. Add all it's children currentNode.s.adjacent().stream().filter(node -> !checkedNodes.contains(node)) .forEach(node -> { CostPair pair = new CostPair(heuristic.apply(node), currentNode.f.cost + cost.apply(node)); nodeQueue.add(new Pair<>(pair, node)); checkedNodes.add(currentNode.s); //Add the node to the hashset, so that we do not search for that again. }); } // We cant reach that state, but just in case return null. return null; } |