A* Algorithm Path Finding running terribly slow on Processing

Processing Coding Questions
1

Hi! I’m trying to implement a A* Path Finding Alghoritm. It’s supposed to iterate over hundreds of Objects to see which one is closest to the target. This implementation works but it takes around 15 seconds for each object which makes it impossible to use. Can somebody help me figure out why my implementation is so slow? Thank you

public ArrayList<ArrayList<Integer>> A_Star(ArrayList<Integer> start, ArrayList<Integer> goal){
    HashMap<ArrayList<Integer>, Integer> openSet = new HashMap<ArrayList<Integer>, Integer>();
    openSet.put(start,1);
    HashMap<ArrayList<Integer>, ArrayList<Integer>> cameFrom = new HashMap<ArrayList<Integer>, ArrayList<Integer>>();    
    HashMap<ArrayList<Integer>, Float> gScore = new HashMap<ArrayList<Integer>, Float>();
    HashMap<ArrayList<Integer>, Float> fScore = new HashMap<ArrayList<Integer>, Float>();    
    // gScore := map with default value of Infinity
    // fScore := map with default value of Infinity
    for (int xI=0;xI<=XSIZE;xI++){
      for (int yI=0;yI<=YSIZE;yI++){
        ArrayList<Integer> tempArray = new ArrayList<Integer>();
        tempArray.add(0,xI);
        tempArray.add(1,yI);
        gScore.put(tempArray, 9998.0);
        fScore.put(tempArray, 9998.0); 
      }
    }
    gScore.replace(start, 0.0);
    fScore.replace(start, dist(start.get(0),start.get(1),goal.get(0),goal.get(1)));    
    while(openSet.size()>0){
      //current := the node in openSet having the lowest fScore[] value
      ArrayList<Integer> current = new ArrayList<Integer>();
      Float currentFscore=9999.0;
      for (HashMap.Entry<ArrayList<Integer>, Integer> set : openSet.entrySet()) {
        ArrayList<Integer> tempKey = set.getKey();
        if (fScore.get(tempKey)<currentFscore){
          currentFscore=fScore.get(tempKey);
          current=tempKey;
        }
      }
      int currentX = current.get(0);
      int currentY = current.get(1);
      if (currentX == goal.get(0) && currentY == goal.get(1)){
        return reconstruct_path(cameFrom, current);
      }      
      openSet.remove(current);  
      
      for(int xI2=(-1);xI2<=1;xI2++){
        for (int yI2=(-1);yI2<=1;yI2++){
          if (xI2!=0 || yI2!=0){
            if ((((currentX)+xI2)>0) && (((currentX)+xI2)<XSIZE)){
              if ((((currentY)+yI2)>0) && (((currentY)+yI2)<YSIZE)){
                boolean obstacleCoord=false;
                for (Obstacle obstacle : Obstacles) {
                  if ((((currentX)+xI2) > obstacle.getObPoint().x) && (((currentX)+xI2) < obstacle.getObPoint().x+(obstacle.getObSize().x))) {
                    if ((((currentY)+yI2) > obstacle.getObPoint().y) && (((currentY)+yI2) < obstacle.getObPoint().y+(obstacle.getObSize().y))) {
                      obstacleCoord=true;
                    }
                  }               
                }
                if(obstacleCoord==false) {
                  ArrayList<Integer> neighbor = new ArrayList<Integer>();
                  neighbor.add(0,(currentX)+xI2);
                  neighbor.add(1,(currentY)+yI2);
                  float tentative_gScore = (gScore.get(current))+dist(neighbor.get(0),neighbor.get(1),currentX,currentY);
                  if (tentative_gScore < gScore.get(neighbor)){                  
                    cameFrom.put(neighbor, current);
                    gScore.replace(neighbor, tentative_gScore);
                    fScore.replace(neighbor, (tentative_gScore+dist(neighbor.get(0),neighbor.get(1),goal.get(0),goal.get(1))));
                    if (openSet.get(neighbor)==null){
                      openSet.put(neighbor,1);
                    }
                  }
                }
              }
            }
          }
        }
      }
    }
    return new ArrayList<ArrayList<Integer>>();
  }
  
  public ArrayList<ArrayList<Integer>> reconstruct_path(HashMap<ArrayList<Integer>,ArrayList<Integer>> cameFrom, ArrayList<Integer> current){
    ArrayList<ArrayList<Integer>> total_path = new ArrayList<ArrayList<Integer>>();
    total_path.add(0, current);
    while (cameFrom.containsKey(current)){
      current = cameFrom.get(current);
      total_path.add(0, current);
    }        
    return total_path;
  }
2

Hi tantrapt welcome to the forum.

Personally, I suggest that you use the Path Finder library which can be installed via the Contributions Manager in Processing IDE. I later incorporated this library into my AI for 2D Games library so although it comes with just one multi-part example you can find out how to use it in the AI library documentation here.

1 Like
3

As far as I know the A* algorithm is intended for searching the path from a single start to a single goal. I had a very simular problem where I had multiple starts and one goal and managed to do it very efficiently using the dijakstra-Algorithm.
A thing that might improve your code in particular would be using PriorityQueues. These basicly basicaly have an auto-sorted List (used more like a Stack). Here would be a implementation of the A* algorithm I put together.

import java.util.*;
public class Node implements Comparable {
  int x, y, g=0;
  float val=0;
  Node parent;
  Node(int x, int y, float val) {
    this.x=x;
    this.y=y;
    this.val=val;
  }
  //Comparator for the Queue
  @Override
    public int compareTo(Object o) {
    Node n=(Node) o;
    return val-n.val>0?1:-1;
  }
  //For debugging
  @Override
    public String toString() {
    if (parent==null) return x+" "+y+" "+g+" | + +";
    return x+" "+y+" "+g+" | "+parent.x+" "+parent.y;
  }
}

public ArrayList<Integer>[] A_star(int startx, int starty, int goalx, int goaly, int[][] grid) {
  //Inits the returned ArrayLists
  ArrayList<Integer>[] ret=new ArrayList[2];
  ret[0]=new ArrayList<Integer>();
  ret[1]=new ArrayList<Integer>();
  int xlen=grid.length;
  int ylen=grid[0].length;
  //Values used later (declared here so they don't need to reinitialized)
  int g;
  float newval;
  //sets up the grid
  Node state[][]=new Node[grid.length][grid[0].length];
  for (int i=0; i<xlen; i++) for (int j=0; j<ylen; j++) {
    state[i][j]=new Node(i, j, Float.MAX_VALUE);
  }
  //Sets up the starting position
  state[startx][starty].x=startx;
  state[startx][starty].y=starty;
  state[startx][starty].val=0;
  state[startx][starty].parent= state[startx][starty];
  state[startx][starty].parent=state[startx][starty];
  //Sets up the Queue of open nodes
  PriorityQueue<Node> pq=new PriorityQueue<Node>();
  //Sets up the currently processed nodes
  Stack<Node> pending=new Stack<Node>();
  //Puts ste starting node in the Queue
  pq.add(state[startx][starty]);
  while (!pq.isEmpty()) {
    //Processes the first node of the Queue (the open one with the lowest distance from start)
    Node current=pq.remove();
    if (current.x!=0) {
      pending.push(state[current.x-1][current.y]);
    }
    if (current.y!=0) {
      pending.push(state[current.x][current.y-1]);
    }
    if (current.x<xlen-1) {
      pending.push(state[current.x+1][current.y]);
    }
    if (current.y<ylen-1) {
      pending.push(state[current.x][current.y+1]);
    }
    while (!pending.isEmpty()) {
      Node successor=pending.pop();
      //Throws out nodes that are obsticals
      if (grid[successor.x][successor.y]==1) continue;
      g=current.g+1;
      newval=((float) g)+dist(successor.x, successor.y, goalx, goaly);
      //If a shorter path is found replaces the saves the new path
      if (newval<successor.val) {
        if (successor.val==Float.MAX_VALUE) {
          successor.parent=current;
          //Ads a node to the open list if its the first time the node is reached
          pq.add(successor);
        }
        successor.val=newval;
        successor.g=g;
        successor.parent=current;
      }
    }
    if(current.x==goalx&&current.y==goaly) break;
  }
  //Once the search is done traces back the path
  Node currentNode=state[goalx][goaly];
  while (currentNode.val!=0) {
    ret[0].add(0, currentNode.x);
    ret[1].add(0, currentNode.y);
    currentNode = currentNode.parent;
  }
  ret[0].add(0, currentNode.x);
  ret[1].add(0, currentNode.y);
  return ret;
}

void setup() {
  int mt[][]={{1, 0, 0, 0, 1}, {1, 0, 1, 0, 1}, {0, 0, 1, 0, 1}, {1, 0, 0, 0, 1}};
  for (int i=0; i<mt.length; i++) {
    for (int j=0; j<mt[0].length; j++)print("("+mt[i][j]+") ");
    println();
  }
  ArrayList<Integer>[] arr=A_star(2, 0, 2, 3, mt);
  print(arr[0]+"\n");
  println(arr[1]);
}

I hope that helps!

1 Like