Performance Enhancement of A Star Path Finder algorithm with 1024 multidimentional array

Question

I have the below code for a A* pathfinder, however it is taking upwards of 10 minutes to find a solution using a simple 1024 x 1024 array.

I had to comment out //Collections.sort(this.openList); as it was throwing a comparison method violates its general contract! error when running.

Is the algorithm correct and any idea why the bottleneck? Some people using C++ are getting a response time of 40ms, not 10+ mins!

import java.util.List;

import javax.imageio.ImageIO;

import java.awt.Canvas;
import java.awt.Color;
import java.awt.GraphicsConfiguration;
import java.awt.Paint;
import java.awt.image.BufferedImage;
import java.io.Console;
import java.io.File;
import java.io.IOException;
import java.util.ArrayList;
import java.util.Collection;
import java.util.Collections;
import java.util.Comparator;

class AStarPathfinding {

    // Closed list, open list and calculatedPath lists
    private final List<Node> openList;
    private final List<Node> closedList;
    private final List<Node> calcPath;

    // Collision Map to store tha map in
    private final int[][] collisionMap;

    // Current node the  program is executing
    private Node currentNode;

    // Define the start and end coords
    private final int xstart;
    private final int ystart;
    private int xEnd, yEnd;

    // Node class for convenience
    static class Node implements Comparable {
        public Node parent;
        public int x, y;
        public double g;
        public double h;
        Node(Node parent, int xpos, int ypos, double g, double h) {
            this.parent = parent;
            this.x = xpos;
            this.y = ypos;
            this.g = g;
            this.h = h;
       }

       // Compare f value (g + h)
       @Override
       public int compareTo(Object o) {
           Node that = (Node) o;
           return (int)((this.g + this.h) - (that.g + that.h));
       }
   }

    // construct and initialise 
    AStarPathfinding(int[][] collisionMap, int xstart, int ystart) {
        this.openList = new ArrayList<>();
        this.closedList = new ArrayList<>();
        this.calcPath = new ArrayList<>();
        this.collisionMap = collisionMap;
        this.currentNode = new Node(null, xstart, ystart, 0, 0);
        this.xstart = xstart;
        this.ystart = ystart;

    }

    // returns a List<> of nodes to target
    public List<Node> findPathTo(int xTo, int yTo) {

        this.xEnd = xTo;
        this.yEnd = yTo;

        // Add this to the closed list
        this.closedList.add(this.currentNode);

        // Add neighbours to openList for iteration
        addNeigborsToOpenList();

        // Whilst not at our target
        while (this.currentNode.x != this.xEnd || this.currentNode.y != this.yEnd) {

            // If nothing in the open list then return with null - handled in error message in main calling func
            if (this.openList.isEmpty()) {
                return null;
            }

            // get the lowest f value and add it to the closed list, f calculated when neighbours are sorted
            this.currentNode = this.openList.get(0);
            this.openList.remove(0); 
            this.closedList.add(this.currentNode); 

            addNeigborsToOpenList();

        }

        // add this node to the calculated path
        this.calcPath.add(0, this.currentNode);

        while (this.currentNode.x != this.xstart || this.currentNode.y != this.ystart) {

            this.currentNode = this.currentNode.parent;

            this.calcPath.add(0, this.currentNode);

        }

        return this.calcPath;
    }



    // Searches the current list for neighbouring nodes returns bool
    private static boolean checkNeighbourHasBeenSearched(List<Node> array, Node node) {
        return array.stream().anyMatch((n) -> (n.x == node.x && n.y == node.y));
    }


    // Calculate distance from current node to the target
    private double distance(int dx, int dy) {
        return Math.hypot(this.currentNode.x + dx - this.xEnd, this.currentNode.y + dy - this.yEnd); // return hypothenuse
    }

    // Add neighbouring nodes to the open list to iterate through next
    @SuppressWarnings("unchecked")
    private void addNeigborsToOpenList() {

        Node node;

        for (int x = -1; x <= 1; x++) {

            for (int y = -1; y <= 1; y++) {

                node = new Node(this.currentNode, this.currentNode.x + x, this.currentNode.y + y, this.currentNode.g, this.distance(x, y));

                // if we are not on the current node
                if ((x != 0 || y != 0) 
                    && this.currentNode.x + x >= 0 && this.currentNode.x + x < this.collisionMap[0].length // check collision map boundaries
                    && this.currentNode.y + y >= 0 && this.currentNode.y + y < this.collisionMap.length
                    && this.collisionMap[this.currentNode.y + y][this.currentNode.x + x] != -1) { // check if tile is walkable (-1)

                    // and finally check we haven't already searched the nodes
                    if(!checkNeighbourHasBeenSearched(this.openList, node) && !checkNeighbourHasBeenSearched(this.closedList, node)){
                        node.g = node.parent.g + 1.; // Horizontal/vertical cost = 1.0
                        node.g += collisionMap[this.currentNode.y + y][this.currentNode.x + x]; // add movement cost for this square


                        // Add diagonal movement cost sqrt(hor_cost² + vert_cost²) + 0.4
                        if (x != 0 && y != 0) {
                            node.g += .4;   
                        }

                        // Add the node to the List<>
                        this.openList.add(node);
                    }

                }
            }
        }

        // sort in ascending order
        //Collections.sort(this.openList);
    }


    public static void main(String[] args) {

        // Define the size of the grid
        final int sizeOf = 1024;

        int[][] collisionMap = new int[sizeOf][];

        for(int i=0;i < sizeOf; i++) {
            // -1 = blocked
            // 0+ = cost
            collisionMap[i] = new int[sizeOf];
        }

        // set the value of the nodes
        for (int k = 0; k < sizeOf; k++) {
            for (int j = 0; j < sizeOf; j++) {
                if(j == 0 && k < 100) {
                    collisionMap[k][j] = -1;
                } else if (j == 50 && k > 230) {
                    collisionMap[k][j] = -1;
                }else {
                    collisionMap[k][j] = 0;
                }

            }
        }        

        AStarPathfinding as = new AStarPathfinding(collisionMap, 103, 300);
        List<Node> path = as.findPathTo(0,0);

        if(path == null) {
            System.out.println("Unable to reach target");
        }


        // create image buffer to write output to
        BufferedImage img = new BufferedImage(sizeOf, sizeOf, BufferedImage.TYPE_INT_RGB);

        // Set colours
        int r = 255;
        int g = 0;
        int b = 0; 

        int colRed = (r << 16) | (g << 8) | b;

        r = 0;
        g = 255;
        b = 0; 

        int colGreen = (r << 16) | (g << 8) | b;

        r = 0;
        g = 0;
        b = 255; 

        int colBlue = (r << 16) | (g << 8) | b;

        r = 255;
        g = 255;
        b = 255; 

        int colWhite = (r << 16) | (g << 8) | b;

        int i = 0;
        int j = 0;

        if (path != null) {

            path.forEach((n) -> {
                System.out.print("[" + n.x + ", " + n.y + "] ");
                collisionMap[n.y][n.x] = 1;
            });

            System.out.printf("\nTotal cost: %.02f\n", path.get(path.size() - 1).g);

            for (int[] maze_row : collisionMap) {
                for (int maze_entry : maze_row) {
                    switch (maze_entry) {

                        // normal tile
                        case 0:
                            img.setRGB(j, i, colWhite);
                            break;

                        // final path
                        case 1:
                            img.setRGB(j, i, colBlue);
                            break;

                        // Object to avoid
                        case -1:
                            img.setRGB(j, i, colRed);
                            break;

                        // Any other value
                        default:
                            img.setRGB(j, i, colGreen);
                    }

                    j++;
                }
                // count j - reset as if it were a for loop
                if(i != sizeOf-1) {
                    j=0;
                }
                i++;
                System.out.println();
            }
        }

        // output file
        File f = new File("aStarPath.png");

        try {
            ImageIO.write(img, "PNG", f);
        } catch (IOException e) {
            // TODO Auto-generated catch block
            e.printStackTrace();
        }

        System.out.println("i: " + i + ", j: " + j);
    }
}

Answer 1

Single Responsibility

your code is a little bit*) monolithic, you try to do all in one class

• creating a map
• calculating path
• drawing images

this makes it really hard to understand your code! it would greatly improve readability if you would split your code into proper classes

*) honestly: the whole code is just ONE BIG monolith

programm flaws

you have already identified the source of your problems, it happens here

// get the lowest f value and add it to the closed list, f calculated when neighbours are sorted
this.currentNode = this.openList.get(0);

this does not work because you do not sort the list by the f-value, as already declared in your question. But that part is essential to have an optimized path-finding algorithm, and that is why you have performance issues. See this answer for some more hints on how to sort a list according to custom properties.

instead of following your heuristic you now have programmed a Flood-Fill-Algorithm that will inspect all fields (untill it finds the lucky target one)

programm flaws 2

when you expand your node (addNeigborsToOpenList()) and check the candidates, the algorithm says:

if the path (g) is better than any previous, then add it

but you don't check that condition:

node.g = node.parent.g + 1.; // Horizontal/vertical cost = 1.0
node.g += collisionMap[this.currentNode.y + y][this.currentNode.x + x]; // add movement cost for this square

// Add diagonal movement cost sqrt(hor_cost² + vert_cost²) + 0.4
if (x != 0 && y != 0) {
    node.g += .4;   
}

// Add the node to the List <--WRONG HERE:
this.openList.add(node);

summary code flaws

These bugs in your code prevent the efficent execution of your astar search, if you solve them your code will gain the desired performance.

additional performance hints

on top of this if you would also apply the guidance from coderodde to optimize the new flawless code.

other issues

some very basic issues

• use a formatter and apply the java code style rules
• apply java naming conventions (e.g. boolean checkNeighbourHasBeenSearched() should be renamed into hasNeighbourHasBeenSearched())
• primitive obseesion - why is collisionMap an array of int - should it not be an array of Field where each Field has the attribute isAccessable
• why do you suppress warnings when you could use proper data types (@SuppressWarnings("unchecked"))?
• tell - don't ask (instead of writing comments give your methods proper names)
• magic numbers

Answer 2

Wrong data structures

First of all, your poor efficiency is due to the fact that you use array lists for the open and closed sets. Change the open set to PriorityQueue and your closed set to HashSet. In order to work, you will need to overwrite equals, hashCode and compareTo for your class Node.

If you don't mind "picking", you can find something relevant here (see AStarPathfinder.java).