A* search algorithm

Question

NodeData stores all information of the node needed by the AStar algorithm. This information includes the value of g, h, and f. However, the value of all 3 variables are dependent on source and destination, thus obtains at runtime.

@param <T>

I'm looking for reviews on optimization, accuracy and best practices.

final class NodeData<T> { 

    private final T nodeId;
    private final Map<T, Double> heuristic;

    private double g;  // g is distance from the source
    private double h;  // h is the heuristic of destination.
    private double f;  // f = g + h 

    public NodeData (T nodeId, Map<T, Double> heuristic) {
        this.nodeId = nodeId;
        this.g = Double.MAX_VALUE; 
        this.heuristic = heuristic;
    }

    public T getNodeId() {
        return nodeId;
    }

    public double getG() {
        return g;
    }

    public void setG(double g) {
        this.g = g;
    }

    public void calcF(T destination) {
        this.h = heuristic.get(destination);
        this.f = g + h;
    } 

    public double getH() {
        return h;
    }

    public double getF() {
        return f;
    }
 }

/**
 * The graph represents an undirected graph. 
 * 
 * @author SERVICE-NOW\ameya.patil
 *
 * @param <T>
 */
final class GraphAStar<T> implements Iterable<T> {
    /*
     * A map from the nodeId to outgoing edge.
     * An outgoing edge is represented as a tuple of NodeData and the edge length
     */
    private final Map<T, Map<NodeData<T>, Double>> graph;
    /*
     * A map of heuristic from a node to each other node in the graph.
     */
    private final Map<T, Map<T, Double>> heuristicMap;
    /*
     * A map between nodeId and nodedata.
     */
    private final Map<T, NodeData<T>> nodeIdNodeData;

    public GraphAStar(Map<T, Map<T, Double>> heuristicMap) {
        if (heuristicMap == null) throw new NullPointerException("The huerisic map should not be null");
        graph = new HashMap<T, Map<NodeData<T>, Double>>();
        nodeIdNodeData = new HashMap<T, NodeData<T>>();
        this.heuristicMap = heuristicMap;
    } 

    /**
     * Adds a new node to the graph.
     * Internally it creates the nodeData and populates the heuristic map concerning input node into node data.
     * 
     * @param nodeId the node to be added
     */
    public void addNode(T nodeId) {
        if (nodeId == null) throw new NullPointerException("The node cannot be null");
        if (!heuristicMap.containsKey(nodeId)) throw new NoSuchElementException("This node is not a part of hueristic map");

        graph.put(nodeId, new HashMap<NodeData<T>, Double>());
        nodeIdNodeData.put(nodeId, new NodeData<T>(nodeId, heuristicMap.get(nodeId)));
    }

    /**
     * Adds an edge from source node to destination node.
     * There can only be a single edge from source to node.
     * Adding additional edge would overwrite the value
     * 
     * @param nodeIdFirst   the first node to be in the edge
     * @param nodeIdSecond  the second node to be second node in the edge
     * @param length        the length of the edge.
     */
    public void addEdge(T nodeIdFirst, T nodeIdSecond, double length) {
        if (nodeIdFirst == null || nodeIdSecond == null) throw new NullPointerException("The first nor second node can be null.");

        if (!heuristicMap.containsKey(nodeIdFirst) || !heuristicMap.containsKey(nodeIdSecond)) {
            throw new NoSuchElementException("Source and Destination both should be part of the part of hueristic map");
        }
        if (!graph.containsKey(nodeIdFirst) || !graph.containsKey(nodeIdSecond)) {
            throw new NoSuchElementException("Source and Destination both should be part of the part of graph");
        }

        graph.get(nodeIdFirst).put(nodeIdNodeData.get(nodeIdSecond), length);
        graph.get(nodeIdSecond).put(nodeIdNodeData.get(nodeIdFirst), length);
    }

    /**
     * Returns immutable view of the edges
     * 
     * @param nodeId    the nodeId whose outgoing edge needs to be returned
     * @return          An immutable view of edges leaving that node
     */
    public Map<NodeData<T>, Double> edgesFrom (T nodeId) {
        if (nodeId == null) throw new NullPointerException("The input node should not be null.");
        if (!heuristicMap.containsKey(nodeId)) throw new NoSuchElementException("This node is not a part of hueristic map");
        if (!graph.containsKey(nodeId)) throw new NoSuchElementException("The node should not be null.");

        return Collections.unmodifiableMap(graph.get(nodeId));
    }

    /**
     * The nodedata corresponding to the current nodeId.
     * 
     * @param nodeId    the nodeId to be returned
     * @return          the nodeData from the 
     */ 
    public NodeData<T> getNodeData (T nodeId) {
        if (nodeId == null) { throw new NullPointerException("The nodeid should not be empty"); }
        if (!nodeIdNodeData.containsKey(nodeId))  { throw new NoSuchElementException("The nodeId does not exist"); }
        return nodeIdNodeData.get(nodeId);
    }

    /**
     * Returns an iterator that can traverse the nodes of the graph
     * 
     * @return an Iterator.
     */
    @Override public Iterator<T> iterator() {
        return graph.keySet().iterator();
    }
}

public class AStar<T> {

    private final GraphAStar<T> graph;


    public AStar (GraphAStar<T> graphAStar) {
        this.graph = graphAStar;
    }

    // extend comparator.
    public class NodeComparator implements Comparator<NodeData<T>> {
        public int compare(NodeData<T> nodeFirst, NodeData<T> nodeSecond) {
            if (nodeFirst.getF() > nodeSecond.getF()) return 1;
            if (nodeSecond.getF() > nodeFirst.getF()) return -1;
            return 0;
        }
    } 

    /**
     * Implements the A-star algorithm and returns the path from source to destination
     * 
     * @param source        the source nodeid
     * @param destination   the destination nodeid
     * @return              the path from source to destination
     */
    public List<T> astar(T source, T destination) {
        /**
         * http://stackoverflow.com/questions/20344041/why-does-priority-queue-has-default-initial-capacity-of-11
         */
        final Queue<NodeData<T>> openQueue = new PriorityQueue<NodeData<T>>(11, new NodeComparator()); 

        NodeData<T> sourceNodeData = graph.getNodeData(source);
        sourceNodeData.setG(0);
        sourceNodeData.calcF(destination);
        openQueue.add(sourceNodeData);

        final Map<T, T> path = new HashMap<T, T>();
        final Set<NodeData<T>> closedList = new HashSet<NodeData<T>>();

        while (!openQueue.isEmpty()) {
            final NodeData<T> nodeData = openQueue.poll();

            if (nodeData.getNodeId().equals(destination)) { 
                return path(path, destination);
            }

            closedList.add(nodeData);

            for (Entry<NodeData<T>, Double> neighborEntry : graph.edgesFrom(nodeData.getNodeId()).entrySet()) {
                NodeData<T> neighbor = neighborEntry.getKey();

                if (closedList.contains(neighbor)) continue;

                double distanceBetweenTwoNodes = neighborEntry.getValue();
                double tentativeG = distanceBetweenTwoNodes + nodeData.getG();

                if (tentativeG < neighbor.getG()) {
                    neighbor.setG(tentativeG);
                    neighbor.calcF(destination);

                    path.put(neighbor.getNodeId(), nodeData.getNodeId());
                    if (!openQueue.contains(neighbor)) {
                        openQueue.add(neighbor);
                    }
                }
            }
        }

        return null;
    }


    private List<T> path(Map<T, T> path, T destination) {
        assert path != null;
        assert destination != null;

        final List<T> pathList = new ArrayList<T>();
        pathList.add(destination);
        while (path.containsKey(destination)) {
            destination = path.get(destination);
            pathList.add(destination);
        }
        Collections.reverse(pathList);
        return pathList;
    }


    public static void main(String[] args) {
        Map<String, Map<String, Double>> hueristic = new HashMap<String, Map<String, Double>>();
        // map for A    
        Map<String, Double> mapA = new HashMap<String, Double>();
        mapA.put("A",  0.0);
        mapA.put("B", 10.0);
        mapA.put("C", 20.0);
        mapA.put("E", 100.0);
        mapA.put("F", 110.0);


        // map for B
        Map<String, Double> mapB = new HashMap<String, Double>();
        mapB.put("A", 10.0);
        mapB.put("B",  0.0);
        mapB.put("C", 10.0);
        mapB.put("E", 25.0);
        mapB.put("F", 40.0);


        // map for C
        Map<String, Double> mapC = new HashMap<String, Double>();
        mapC.put("A", 20.0);
        mapC.put("B", 10.0);
        mapC.put("C",  0.0);
        mapC.put("E", 10.0);
        mapC.put("F", 30.0);


        // map for X
        Map<String, Double> mapX = new HashMap<String, Double>();
        mapX.put("A", 100.0);
        mapX.put("B", 25.0);
        mapX.put("C", 10.0);
        mapX.put("E",  0.0);
        mapX.put("F", 10.0);

        // map for X
        Map<String, Double> mapZ = new HashMap<String, Double>();
        mapZ.put("A", 110.0);
        mapZ.put("B",  40.0);
        mapZ.put("C",  30.0);
        mapZ.put("E", 10.0);
        mapZ.put("F",  0.0);

        hueristic.put("A", mapA);
        hueristic.put("B", mapB);
        hueristic.put("C", mapC);
        hueristic.put("E", mapX);
        hueristic.put("F", mapZ);

        GraphAStar<String> graph = new GraphAStar<String>(hueristic);
        graph.addNode("A");
        graph.addNode("B");
        graph.addNode("C");
        graph.addNode("E");
        graph.addNode("F");

        graph.addEdge("A", "B",  10);
        graph.addEdge("A", "E", 100);
        graph.addEdge("B", "C", 10);
        graph.addEdge("C", "E", 10);
        graph.addEdge("C", "F", 30);
        graph.addEdge("E", "F", 10);

        AStar<String> aStar = new AStar<String>(graph);

        for (String path : aStar.astar("A", "F")) {
            System.out.println(path);
        }
    }
}

Answer 1

This is quite good and professional-looking code. There are many small aspects which I really like:

• Using Collections.unmodifiableMap instead of a shallow copy is brilliant.
• The use of a generic nodeId is clever and makes for elegant code.
• There are input checks in all public methods (well, there are some exceptions: only the AStar and NodeData constructors, NodeComparator#compare, and AStar#astar are not checked).
• You make perfect use of empty lines to separate unrelated blocks.

But there were some aspects that made your code sometimes a bit harder to follow:

• Lack of encapsulation of some parts like heuristic: What is this Map<T, Map<T, Double>>? Can't I have nice self-documenting accessors for a Heuristic<T> instance?

• Sequential coupling in NodeData: whenever I setG I also have to call calcF. You could have made this easier by slapping a return this in there instead of void methods, but the real solution is to get rid of public calcF and make it private instead. The NodeData class is responsible on its own to maintain its invariants, so any call to setG should update dependent fields.

• Bad naming. The letters g, f and h have a specific meaning in the context of A* and are OK here. Of course it would have been better to include a link to this algorithm's Wikipedia page so that a future maintainer can understand why you used g instead of distance.

  But nodes don't have a distance, nodes or edges have weights. In the context of optimization problems it is also common to talk about a cost – a term which does not occur once in your code.

  It's called heuristic, not hueristic. Typos are easy to correct, and should be corrected while they're still young (The origin of the referer field in a HTTP request should be edutaining here).

• There are some formatting “errors” that can be easily rectified by an automatic formatter. E.g. don't use braces when a conditional is on a single line like if (cond) { single_statement; } – removing the braces reduces line noise. Otherwise, you could also put the statement on its own line.

  Some of your lines are excessively long and should be broken up (see also the next tip).

• As laudable as your input checks are, they do add visual clutter. Consider hiding them behind helper methods, e.g. heuristic.assertContains(nodeId) or preferably Assert.nonNull(nodeId, "node") (which assumes a whole class dedicated to input checking). Arguments against this are less useful stack traces and reduced performance (method call overhead, compiler optimizations are more difficult), but pro-arguments include more self-documenting, concise code.

Other notes:

• Your documentation does not state what happens when there is no path from the start to the destination (e.g. if the nodes are in unconnected subgraphs). The implementation is rather clear here: A null is returned instead of a list.

• The Pseudocode given on the A* Wikipedia page uses a slightly different condition for updating the path and possibly adding neighbours to the openQueue:

  if (!openQueue.contains(neighbor) || tentativeG < neighbor.getG())`
  

  whereas you use if (tentativeG < neighbor.getG()). It might be worth checking an authoritative source what the correct condition is.

• You could translate between T instances and a continuous range of ints at your component's boundaries (in other words: internally, every nodeId would be an integer). Using integers allows for more efficient data structures like arrays. This would remove most Map lookups, but also the NodeData class. The disadvantage is that your code would look like C afterwards… I would just try out the transformation and see (a) whether there is a noticeable increase in performance and (b) whether the increased ugliness is really worth it.

Answer 2

The code seems ok, but in my humble opinion, I think there are some things that can improve your code:

1. Why does each node have a heuristic table? It may be better to use just the node to encapsulate the cost and the score, but let the algorithm compute the f cost for you.

2. Similarly, the f computation is embedded in the node but is also in the AStar algorithm. Why not use a different component to compute f? Let the node act just as a "container" of the data. I think is much better to compute the f in the AStar only, instead of having a similar computation in the Node and in the AStar:

           // You are calculating g here, why f is computed in the node?
           // Responsabilities of each component are not clear
           double distanceBetweenTwoNodes = neighborEntry.getValue();
           double tentativeG = distanceBetweenTwoNodes + nodeData.getG();
   
           if (tentativeG < neighbor.getG()) {
               neighbor.setG(tentativeG);
               neighbor.calcF(destination); // confusing
   

3. AStar can be used only with AStarGraph. This code is a little bit tied. You can define instead a function that computes the successors for a state, so instead of using graph.edgesFrom you can use transitions.from(state). This function can be computed using your graph structure or any other thing.

Check this implementation to see some of these ideas; it may give you some hints on implementing an even better version of the A*.

Answer 3

I like you code but I think there is a bug: in neighbor.setG(tentativeG) you update the priority of a node that may already have been inserted in the openQueue. The latter is a java PriorityQueue, which does not support priority updates. In changing the priority of an object in the queue without adjusting it's position (sifting up in your case) you could be breaking its structure. In orderer to do the update safely in a java PriorityQueue you should remove the node (O(N)), update it and insert it back in. In term of performance, you are already paying a O(N) (here N is the typical size of the open set) in openQueue.contains(neighbor), so the removal is going to be comparable to that. I think you can improve that by, for example, avoiding checking on the priority queue but book-keeping on separate hash instead. This though would not solve the O(N) that you should pay when removing a node before updating it. A possible way out could be to just avoid removing the obsolete node and simply add a new one with the updated priority. If you do that, make sure you hash the close set by some id rather than with the object itself, so that when you poll an obsolete node (which will have the same id of the newer one already visited as the latter had higher priority) it will be classified as already visited. The cost of this approach is that you will have in general a longer queue, but operations will all be O(log(N)) at most.

Answer 4

Why:

if (!openQueue.contains(neighbor)) {
     openQueue.add(neighbor);
}

is only if tentativeG < neighbor.getG()?

In a generic A* pseudocode the update is done in any case.

See, e.g. on Wikipedia

I tried running the code, it finds the optimal solution only "thanks" to the other bug mentioned by mzivi.