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_cost2 + vert_cost2) + 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_cost2 + vert_cost2) + 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).