Infinite Loop in AStar (A*) Algorithm

Question

Alright, so I've run into a problem where when trying to find a path that you cannot reach because it is blocked, it will continually loop and loop and loop (the 'open' list is ALWAYS filled). I understand the open set cannot contain things that the closed set contains. I cannot understand where the open set is getting things from the closed set.

I make sure to check if the neighbor is within the closed set, so it shouldn't be added to the open set. I'm confused...

    public static LinkedList<Node> findPath(World world, Node start, Node goal) {
        final PriorityQueue<Node> open = new PriorityQueue<>();
        final HashSet<Node> closed = new HashSet<>();

        open.add(start.setHeuristic(start.heuristic(goal)));
        Node current = null;
        Node best = start;
        while (!open.isEmpty()) {
            current = open.poll();
            if (current.equals(goal))
                return construct(start, current);
            closed.add(current);
            Tile tile = world.getTile(current.getX(), current.getY(), current.getZ());
            ArrayList<Tile> neighbors = tile.getNeighbors();
            for (Tile n : neighbors) {
                if (n == null)
                    continue;
                Node neighbor = new Node(n.getGlobalX(), n.getGlobalY(), n.getZ(), current);
                neighbor.setCost(neighbor.distance(current));
                neighbor.setHeuristic(neighbor.heuristic(goal));
                if (closed.contains(neighbor))
                    continue;
                closed.add(neighbor);
                if (!n.isWalkable())
                    continue;
                if (neighbor.equals(goal))
                    return construct(start, neighbor);
                double tentative_cost = current.getCost() + current.distance(neighbor);
                if (!open.contains(neighbor) || tentative_cost < neighbor.getCost()) {
                    best = neighbor;
                    neighbor.setCost(tentative_cost);
                    open.add(neighbor);
                }
            }
        }
        return construct(start, best);
    }

    private static LinkedList<Node> construct(Node start, Node goal) {
        LinkedList<Node> path = new LinkedList<>();
        Node last = goal;
        while (!last.equals(start)) {
            path.addFirst(last);
            last = last.getParent();
        }
        path.addFirst(start);
        return path;
    }

Node class

package com.entity.world.path;

import java.util.Objects;

/**
 * @author Albert Beaupre
 * 
 */
public class Node implements Comparable<Node> {

    private final int x, y, z;
    private final Node parent;
    private double cost, heuristic;

    public Node(int x, int y, int z) {
        this.x = x;
        this.y = y;
        this.z = z;
        parent = this;
    }

    public Node(int x, int y, int z, Node parent) {
        this.x = x;
        this.y = y;
        this.z = z;
        this.parent = parent;
    }

    public double distance(Node from) {
        double dx = Math.pow(Math.abs(from.x - x), 2);
        double dy = Math.pow(Math.abs(from.y - y), 2);
        return Math.sqrt(dx + dy);
    }

    public int getX() {
        return x;
    }

    public int getY() {
        return y;
    }

    public int getZ() {
        return z;
    }

    public Node getParent() {
        return parent;
    }

    public double getCost() {
        return cost;
    }

    public Node setCost(double cost) {
        this.cost = cost;
        return this;
    }

    public double getHeuristic() {
        return heuristic;
    }

    public Node setHeuristic(double heuristic) {
        this.heuristic = heuristic;
        return this;
    }

    public double getF() {
        return cost + heuristic;
    }

    public double heuristic(Node goal) {
        double dx = Math.abs(x - goal.x);
        double dy = Math.abs(y - goal.y);
        return Math.sqrt(dx + dy);
    }

    @Override
    public int hashCode() {
        return Objects.hash(x, y, z);
    }

    /**
     * {@inheritDoc}
     */
    public boolean equals(Object o) {
        if (o instanceof Node) {
            Node n = (Node) o;
            return x == n.x && y == n.y && z == n.z;
        }
        return false;
    }

    /**
     * {@inheritDoc}
     */
    public String toString() {
        return "[x=" + x + ", y=" + y + "]";
    }

    /*
     * (non-Javadoc)
     * 
     * @see java.lang.Comparable#compareTo(java.lang.Object)
     */
    public int compareTo(Node o) {
        return Double.compare(getF(), o.getF());
    }

}
```

Answer 1

There are a few misunderstandings of A* here.

1. You add every node to the closed set before you add it to the open set, but that's strictly speaking premature: at this point, we know we've found a route here, but we don't yet know whether it's the shortest route possible.

   We know we've found the shortest route to current, since it was at the head of the priority queue. If there were any shorter route available, we'd have processed it earlier. But we can't say what might be next in queue. Let's say current has a link of length 2 to reach neighbor. If the very next node in the queue is no further than current, and has a link of length 1 to reach neighbor, then there's still a shorter route to be found later on. So we can't close it off just yet.

   For the same reason, we can't return a path to the goal from inside the neighbour enumeration loop - we might return a longer-than-optimal route if we do.

   You can make these assumptions if all your links have the same length, but if that's the case then you might do better with breadth-first search instead of the complexity of A*.

2. What you're doing with tentative_cost isn't meaningful.

   Given that you compute both tentative_cost and your first pass at node.setCost() using the same distance calculation, but you add current_cost to tentative_cost, it will never be less than node.getCost().

   It looks like you might be trying to selectively replace a node in the open set queue if you've found a faster route there. You can do this by...

   • calculating the correct cost in the first place (including current.getCost()),
   • walking the open set queue to find the current instance of the node if there is one
     • if there is, and it has cost less than or equal to the one you're considering, continue without adding a copy
     • if there is, and it has more cost than than the one you're considering, remove it from the queue
   • finally, add your new node, if you haven't already earlied-out

   Below, I'll show a simpler approach where we just let bad nodes accumulate in the queue, knowing we'll look at the best ones first anyway. But this can strictly be improved upon.

   If you're trying to find the path that gets closest in the event that none reach the goal, then you need to keep track of the closest you've seen in your whole search and compare against that.

3. Your heuristic can be improved. Currently you're paying the expensive part of a Euclidean heuristic (the square root) but getting a substantial under-estimate because you're missing the squares on each term inside. This will make every direction look closer than it really is (assuming your distances are >= 1, otherwise this heuristic is inadmissible), weakening A*'s ability to select the closest routes.

   public double heuristic(Node goal) {
       double dx = (x - goal.x);
       double dy = (y - goal.y);
       return Math.sqrt(dx*dx + dy*dy);
   }

   You can apply this same form to your distance function to skip the abs and pow

4. You can rearrange your early-outs to better prune out unnecessary work.

Here's a rewrite of your core loop fixing some of these issues:

double leastHeuristic = Double.POSITIVE_INFINITY;

while (!open.isEmpty()) {
    current = open.poll();

    // We already found a faster route here. Discard a bad dupe.
    if (closed.contains(current))
        continue;

    if (current.equals(goal))
        return construct(start, current);

    // No shorter route can be found to this node. Close it out.
    closed.add(current);
    // Log best only as we close-off nodes (happens less often than adding dupes).
    if(current.getHeuristic() < leastHeuristic) {
        leastHeuristic = current.getHeuristic;
        best = current;
    }

    Tile tile = world.getTile(current.getX(), current.getY(), current.getZ());
    ArrayList<Tile> neighbors = tile.getNeighbors();

    for (Tile n : neighbors) {
        // We don't care if we can't walk on it.
        if (n == null || !n.isWalkable())
            continue;

        Node neighbor = new Node(n.getGlobalX(), n.getGlobalY(), n.getZ(), current);

        // We already found the shortest possible route to this node.
        if (closed.contains(neighbor))
            continue;

        neighbor.setCost(current.cost + neighbor.distance(current));
        neighbor.setHeuristic(neighbor.heuristic(goal));

        open.add(neighbor);
    }
}