Problems with pathfinding. Weird paths being returned

Question

I have been reading some articles to learn A* pathfinding and I was able to make a test program based on it, but the the program I made is giving strange paths which are not a shortest path to the target node.

In the images, the green square unit represents the starting node, the red unit represents target node, blue units are impassable tiles (walls) and the purple units represent the path found from starting node to target node

https://i.sstatic.net/xwhuk.jpg

https://i.sstatic.net/B0WxU.jpg

If anybody could find a problem with the pathfinding source code I would be much thankful. I'm burned out from trying to know what caused it to act strange.

Its allowed to cut corners and go diagonal

package com.streak324.pathfinding;

import java.util.Comparator;
import java.util.HashSet;
import java.util.PriorityQueue;

import com.badlogic.gdx.utils.Array;

public class PathFinder {

    private boolean foundTarget;
    private int width, height;

    //list of nodes that leads from starting node to target node is stored here
    private Array<PathNode> path;

    //all nodes stored in this array
    private PathNode[][] nodes;

    private PriorityQueue<PathNode> open;

    private HashSet<PathNode> closed;

    //nodes that must be referenced
    private PathNode start, target, current;

    //how far the current node can reach for other nodes from its own position
    private int range = 1;

    public PathFinder(int width, int height, boolean map[][]) {
        this.width = width;
        this.height = height;
        nodes = new PathNode[width][height];

        for(int i=0; i<width; i++) {
            for(int j=0; j<height; j++) {
                nodes[i][j] = new PathNode(i, j);
                //if wall tile is spotted, mark the node unwalkable
                if(map[i][j] != true) { nodes[i][j].passable = false; }
            }
        }

        open = new PriorityQueue<PathNode>(new CostComparator());
        closed = new HashSet<PathNode>();
    }

    public Array<PathNode> getPath(int sx, int sy ,int tx, int ty) {
        path = new Array<PathNode>();
        open.clear();
        closed.clear();

        start = nodes[sx][sy];
        start.movementCost = 0;
        addToOpenList(start);

        target = nodes[tx][ty];

        while(foundTarget != true) {
            if(open.size() == 0) { return null; }

            current = open.poll();
            addToClosedList(current);

            checkNeighbors(current);
        }

        traceBack();

        return path;
    }
    // makes its way back adding the parent node until start
    private void traceBack() {
        while(current != start) {
            path.add(current);
            current = current.parent;
        }
    }

    //checks for nodes within certain range
    private void checkNeighbors(PathNode node) {
        //continues loop if i or j goes out of bounds of nodes array
        for(int i = node.x - range; i <= (node.x + range); i++) {

            if(i >= width || i < 0) { continue; }
            for(int j = node.y - range; j <= (node.y + range); j++) {

                if( j >= height || j < 0) { continue; }

                if((i == node.x && j == node.y) )  { continue; }

                PathNode neighbor = nodes[i][j];

                identifyNode(neighbor);

            }
        }
    }

    //if node is not on open list, add node and calculate it
    private void identifyNode(PathNode node) {
        if(!node.passable || closed.contains(node) ) return;

        if(node == target) {
            foundTarget = true;
            System.out.println("Target Found: " + node.x + ", " + node.y);
            return;
        }
        else if(!open.contains(node)) {
            addToOpenList(node);
            calcHeuristic(node);
            updateNode(node, current);
        }
        else {
            checkForReparenting(node);
        }
    }

    //is the movement cost less to go from the current node?
    private void checkForReparenting(PathNode node) {
        float cost = node.movementCost;
        float reCost = calcMovementCost(node, current);

        if(reCost < cost) {
            System.out.println("Reparenting");
            updateNode(node, current);
        }
    }

    //updates parent and cost
    private void updateNode(PathNode child, PathNode parent) {
        child.parent = parent;
        child.movementCost = calcMovementCost(child, parent);       
        child.totalCost = child.movementCost + child.heuristic;
    }

    private float calcMovementCost(PathNode n1, PathNode n2) {
        float dx = n1.x - n2.x;
        float dy = n1.y - n2.y;
        return (float) Math.sqrt( (dx*dx + dy*dy) ) + n2.movementCost;
    }

    private float calcHeuristic(PathNode node) {
        float dx = node.x - target.x;
        float dy = node.y - target.y;
        return (float) Math.sqrt( (dx*dx + dy*dy) );
    }

    private void addToOpenList(PathNode node) {
        if(!open.contains(node) && !closed.contains(node)) {
            open.add(node);
        }
    }

    private void addToClosedList(PathNode node) {
        if(!closed.contains(node)) {
            closed.add(node);
        }
    }

    public class PathNode {
        public int x, y;
        public PathNode parent;
        //g, h and f
        public float movementCost, heuristic, totalCost;
        public boolean passable;

        public PathNode(int x, int y) {
            this.x = x;
            this.y = y;
            passable = true;
        }

    }

    private class CostComparator implements Comparator<PathNode> {

        @Override
        public int compare(PathNode a, PathNode b) {

            if(a.totalCost < b.totalCost) return 1;
            else return -1;
        }

    }

}

no comments http://pastebin.com/rSv7pUrB

I'm guessing something is wrong in the way that the priority queue is ordering the elements or I may have not properly calculated the totalCost, movementCost, and heuristic variables, but I see nothing wrong with it.

Someone that could point me to the right direction of a probable problem or solution is much appreciated

Answer 1

Estimated Streak324:

Altough your implementation of A* is now working properly, I recommend you to do a quick search in Internet for java search libraries. Your code will look much more simple, scalable and modular, and the implementations are very efficient and well tested. This will be your code using Hipster:

//HERE YOU DEFINE THE SEARCH PROBLEM
// The maze is a 2D map, where each tile defined by 2D coordinates x and y
// can be empty or occupied by an obstacle. We have to define de transition
// function that tells the algorithm which are the available movements from
// a concrete tile point.
SearchProblem p = ProblemBuilder.create()
   .initialState(origin)
   .defineProblemWithoutActions()
   .useTransitionFunction(new StateTransitionFunction<Point>() {
      @Override
      public Iterable<Point> successorsOf(Point state) {
         // The transition function returns a collection of transitions.
         // A transition is basically a class Transition with two attributes:
         // source point (from) and destination point (to). Our source point
         // is the current point argument. We have to compute which are the
         // available movements (destination points) from the current point.
         // Class Maze has a helper method that tell us the empty points
         // (where we can move) available:
         //TODO: FILL WITH YOUR CODE GENERATING THE NEIGHBORS, FILTERING
         //THOSE WHICH ARE NOT ACCESIBLE DUE TO OBSTACLES IN YOUR MAP
        return [...]
      }
   })
   .useCostFunction(new CostFunction<Void, Point, Double>() {
      // We know now how to move (transitions) from each tile. We need to define the cost
      // of each movement. A diagonal movement (for example, from (0,0) to (1,1)) is longer
      // than a top/down/left/right movement. Although this is straightforward, if you don't
      // know why, read this http://www.policyalmanac.org/games/aStarTutorial.htm.
      // For this purpose, we define a CostFunction that computes the cost of each movement.
      // The CostFunction is an interface with two generic types: S - the state, and T - the cost
      // type. We use Points as states in this problem, and for example doubles to compute the distances:
      @Override
      public Double evaluate(Transition<Void, Point> transition) {
         Point source = transition.getFromState();
         Point destination = transition.getState();
         // The distance from the source to de destination is the euclidean
         // distance between these two points http://en.wikipedia.org/wiki/Euclidean_distance
         return source.distance(destination);
      }
   })
   .build();

//HERE YOU INSTANTIATE THE ALGORITHM AND EXECUTE THE SEARCH
//MazeSearch.printSearch(Hipster.createAStar(p).iterator(), maze);
System.out.println(Hipster.createAStar(p).search(goal));

As you can see, you only need to define the components to be used in the search problem, and then execute the algorithm. The library will do the rest of the operations for you.

Also, the library is open-source and licsensed Apache2. You have access to several examples that will help you to start working with the library.

In your case, as you are using a custom 2D grid, the only thing you need to adapt is the transition function which checks your grid to filter those neighbors not accesible due to obstacles.

The inmediate benefit of using this implementation is, apart of the scalability and modularity of the code, avoid instantiating all the nodes in the path, as the library will do it dynamically for you, reducing memory and increasing performance (specially in cases of huge grids).

I hope my answer helps,

Answer 2

There are several issues with your code:

1. You never really use the heuristic. The following statement (the only call to calcHeuristic) just "throws the result away".

   calcHeuristic(node);
   

   That alone can't be the error here, since it's a valid admissible heuristic to guess the distance to the target to be 0. However the algorithm degenerates that way (to what I think is the Dijkstra algorithm).

2. You never update the position of the node in the priority queue. That means a node with updated totalDistance will never move up in the proirity queue, even if it's totalCost becomes less than the totalCost of another node. You have to remove the node and add it again to do that with a PriorityQueue:

   open.remove(node);
   // ... update totalDistance
   open.add(node);
   

3. You terminate too early for general A* (however that wouldn't be an issue here, since totalDistance is equal to the real distance, for expanded neighbors of the target IF you use the heuristic; here the distance real distance is different by either sqrt(2) or 1). In general the distance heuristic for the last step can be arbitrary bad (and here it's bad, see (1.)) and you can only be sure you found the real solution, if you run the algorithm to the point where you would expand the target node.