# Java A* Algorithm Implementation Performance

I've coded a working implementation of the A* Algorithm however it's not meeting my performance expectations - it explores way too many nodes and takes a lot longer to find a route than I expect.

private Vertex dijkstra(Vertex startLocation, Vertex endLocation, int routetype)
{
Long startTime = System.nanoTime();
Vertex vertexNeighbour;
startLocation.setTentativeDistance(0);
startLocation.setH(heuristic(endLocation, startLocation));
startLocation.setF(startLocation.getH() + startLocation.getTentativeDistance());
startLocation.from = startLocation;

while (!(pqOpen.IsEmpty()))
{
tempVertex = pqOpen.GetNextItem();
if (tempVertex == null || tempVertex.getTentativeDistance() == Double.POSITIVE_INFINITY)
{
//System.out.println("Route calculation time: " + ((System.nanoTime() - startTime)/1000000) + " ms");
return null;
}
else if (tempVertex.city == endLocation.city)
{
for (int i = 0; i < pqClosed.queueSize; i++)
{
for (int z = 0; z < pqClosed.QueueArray[i].neighbors.GetNoOfItems(); z++)
{
pqClosed.QueueArray[i].neighbors.GetItem(z).visited = false;
}
}
//System.out.println("Route calculation time: " + ((System.nanoTime() - startTime)/1000000) + " ms");
return tempVertex;
}
else
{
for (int i = 0; i < tempVertex.neighbors.GetNoOfItems() && tempVertex.neighbors.GetItem(i).visited == false; i++) //for each neighbor of tempVertex
{
tempEdge = tempVertex.neighbors.GetItem(i);
tempVertex.neighbors.GetItem(i).visited = true;

vertexNeighbour = allVertices.GetItem(binarySearch(allVertices, 0, allVertices.GetNoOfItems(), tempEdge.toid));
nodesVisited++;

boolean boolClosed = false;
//if the neighbor is in closed set, move to next neighbor
for (int z = 0; z < pqClosed.GetQueueSize(); z++)
{
if (pqClosed.QueueArray[z].city == vertexNeighbour.city)
{
boolClosed = true;
}
}
if (boolClosed)
{
continue;
}

double temp_g_score = (tempVertex.getTentativeDistance() + tempEdge.distance);
//checks if neighbor is in open set
boolean foundNeighbor = false;

for (int z = 0; z < pqOpen.GetQueueSize() && foundNeighbor == false; z++)
{
if (pqOpen.QueueArray[z].city == vertexNeighbour.city)
{
foundNeighbor = true;
}
}

if (!(foundNeighbor) || temp_g_score < vertexNeighbour.getTentativeDistance())
{
vertexNeighbour.from = tempVertex;
vertexNeighbour.setTentativeDistance(temp_g_score);
//calculate H once, store it and then do an if statement to see if it's been used before - if true, grab from memory, else calculate.
if (vertexNeighbour.getH() == 0)
vertexNeighbour.setH(heuristic(endLocation, vertexNeighbour));

if (routetype == 2)
vertexNeighbour.setF(((vertexNeighbour.getH() + vertexNeighbour.getTentativeDistance())*(0.000621371192) / tempEdge.speedlimit));
else
vertexNeighbour.setF(vertexNeighbour.getH() + vertexNeighbour.getTentativeDistance());

if (!(foundNeighbor)) // if neighbor isn't in open set, add it to open set
{
}
}
}
}
}
return null;
}

private double heuristic(Vertex goal, Vertex next)
{
return (Math.sqrt(Math.pow((goal.x - next.x), 2) + Math.pow((goal.y - next.y), 2)))*1.10;
}


Here is my Vertex class:

public class Vertex {
public int city, x, y;
public boolean visited = false;
public EdgeVector neighbors;
private double f;  // f = tentativeDistance + h
private double tentativeDistance;  // tentativeDistance is distance from the source
private double h;  // h is the heuristic of destination.
public Vertex from;

public Vertex(int city, int x, int y)
{
this.city = city;
this.x = x;
this.y = y;
this.neighbors = new EdgeVector();
this.tentativeDistance = Double.POSITIVE_INFINITY;
this.f = Double.POSITIVE_INFINITY;
}

{
}

public double getTentativeDistance()
{
return tentativeDistance;
}

public void setTentativeDistance(double g)
{
this.tentativeDistance = g;
}

public double getF()
{
return f;
}

public void setF(double f)
{
this.f = f;
}

public double getH()
{
return h;
}

public void setH(double h)
{
this.h = h;
}
}


and my Edge class:

public class Edge {
public String label;
public int fromid, toid, speedlimit;
public double distance;
public boolean visited;

public Edge(String label, int fromid, int toid, double distance, int speedlimit)
{
this.label = label;
this.fromid = fromid;
this.toid = toid;
this.distance = distance;
this.speedlimit = speedlimit;
this.visited = false;
}
}


Lastly, I use my own Vector class:

    public class Vector {
private static final int MAXDISPLAY=20;

private int growby;
private int noofitems;
private Object[] data;

private static final int MINGROW=10;

public Vector()
{
Init(10);
}

public Vector(int initsize)
{
Init(initsize);
}

private void Init(int initsize)
{
growby=initsize;

if (growby<MINGROW)
growby=MINGROW;

noofitems=0;
data=new Object[initsize];
}

public int GetNoOfItems()
{
return noofitems;
}

public Object GetItem(int index)
{
return (Object)(index>=0 && index<noofitems?data[index]:null);
}

{
if (noofitems==data.length)
GrowDataStore();
data[noofitems++]=item;
}

public boolean InsertItem(int index, Object item)
{
if (index>=0 && index<=noofitems)
{
if (noofitems==data.length)
GrowDataStore();
for (int i=noofitems; i>index; i--)
data[i]=data[i-1];
data[index]=item;
++noofitems;
return true;
}
else
return false;
}

public boolean DeleteItem(int index)
{
if (index>=0 && index<noofitems)
{
--noofitems;
for (int i=index; i<noofitems; i++)
data[i]=data[i+1];
return true;
}
else
return false;
}

public void ResetData(int[] items)
{
if (items!=null && items.length==noofitems)
System.arraycopy(items, 0, data, 0, noofitems);
}

public void Swap(int index1, int index2)
{
if (index1>=0 && index1<noofitems && index2>=0 && index2<noofitems)
{
Object tmp=data[index1];
data[index1]=data[index2];
data[index2]=tmp;
}
}

private void GrowDataStore()
{
Object[] tmp=new Object[noofitems+growby];
System.arraycopy(data, 0, tmp, 0, noofitems);
data=tmp;
}

public void Randomise()
{
for (int i=0; i<noofitems; i++)
{
int pos=(int) (Math.random()*noofitems);
Swap(i, pos);
}
}

public String toString()
{
StringBuilder str=new StringBuilder();
str.append('[');
if (noofitems>0)
str.append(data[0]);

int max=(noofitems<MAXDISPLAY?noofitems:MAXDISPLAY);
for (int i=1; i<max; i++)
{
str.append(", ");
str.append(data[i]);
}
if (noofitems>MAXDISPLAY)
{
str.append(", ...(");
str.append(noofitems-MAXDISPLAY);
str.append(')');
}
str.append(']');
return str.toString();
}

}


Please note, that I have EdgeVector and VertexVector classes that are simply alterations of the above Vector class (replace Object for Edge or Vertex) - I didn't post them two as it would take up a lot of the post space. I do this as it is more efficient than casting an Object to either Vertex or Edge in the algorithm.

I'm hoping that somebody could take a quick look at my code and see where I may be going wrong - the code does work and produces correct results however it does explore significantly more nodes than I expect and want which therefore increases run-time substantially.

It look my a long time to code this as I'm a novice and I've spent the best part of a full day trying to see what I've done incorrect which is causing the problem but I can't spot anything.

My own implementation of PQ:

public class PriorityQueue
{
Vertex[] QueueArray = new Vertex[10];
int queueSize = 0;

// Default Constructor
public PriorityQueue()
{
}

// Returns true if the priority queue is empty, else false
public boolean IsEmpty()
{
if (queueSize == 0)
{
return true;
}
else
{
return false;
}
}

// Returns the number of items in the queue
public int GetQueueSize()
{
return queueSize;
}

public Vertex removeVertex(int info)
{
int i = 0;
boolean found = false;
Vertex data = null;
for (i = 0; i < QueueArray.length && found == false; i++)
{
if(QueueArray[i].city == info)
{
found = true;
data = QueueArray[i];
}
}

if (found == true)
{
for (i = i; i < QueueArray.length-1; i++)
{
QueueArray[i] = QueueArray[i+1];
}
QueueArray[queueSize] = null;
queueSize--;
}

return data;
}

{
if (queueSize == QueueArray.length)
{
Vertex[] QueueArray2 = new Vertex[queueSize*2];
System.arraycopy(QueueArray, 0, QueueArray2, 0, queueSize);
QueueArray = QueueArray2;
}

if (queueSize == 0)
{
QueueArray[queueSize] = item; // insert at 0
queueSize++;
}
else
{
int index=queueSize;
//Vertex newNode = new Vertex(item, priority);
QueueArray[index] = item;
queueSize++;

int parent=(index-1)/2;
while (index!=0 && QueueArray[index].getF()<QueueArray[parent].getF())
{
// swap parent and index items
Vertex temp = QueueArray[parent];
QueueArray[parent] = QueueArray[index];
QueueArray[index] = temp;

index=parent;
parent=(index-1)/2;
}
}
}

public Vertex GetNextItem()
{
if (queueSize == 0)
{
return null;
}
Vertex temp = QueueArray[0];
--queueSize;
if (queueSize > 0)
{
QueueArray[0] = QueueArray[queueSize];
swapNodes(0);
}
QueueArray[queueSize] = null;
return temp;
}

public void swapNodes(int root)
{
int child;
if ((2*root+1) >= queueSize)
{
child = root;        //no children
}
else
if ((2*root)+2 == queueSize)
{
child = (2*root)+1;
}
else
if (QueueArray[(2*root)+1].getF()< QueueArray[(2*root)+2].getF())
{
child = (2*root)+1;   //left child
}
else
{
child = (2*root)+2;     //right child
}
//swap the nodes around
if (QueueArray[root].getF() > QueueArray[child].getF())
{
Vertex temp = QueueArray[root];
QueueArray[root] = QueueArray[child];
QueueArray[child] = temp;
swapNodes(child);
}
}

public void siftUp(int root)
{
while (root > 0 && root < queueSize && (QueueArray[root].getF() < QueueArray[(root-1)/2].getF()))
{
Vertex temp = QueueArray[root];
QueueArray[root] = QueueArray[(root-1)/2];
QueueArray[(root-1)/2] = temp;

root = (root-1)/2;
}
}

public String toString()
{
return super.toString();
}

}


The program is a sat-nav, there are many vertex's which can have edges connecting them. Each vertex's neighbors are stored in a VortexVector within the Vertex class itself. It would probably be easier if I could upload a zip of my program because I test my program by reading Edge and Vertex data from two .bat files (they contain 250,000 pieces of data each), however I presume there are rules against that for safety of users?

• and pqOpen is a home brew priority queue? – ratchet freak Apr 14 '15 at 18:02
• Oops yeah it is, I forgot to include that - I'll edit my post with the implementation of that – Craig Apr 14 '15 at 18:03
• What kind of problems are you trying to solve with this code? Can you give us some example data (or a program that computes some example data) that illustrates the performance problem? – Gareth Rees Apr 14 '15 at 18:03
• @GarethRees I've updated my post with a bit of information about it - the test data can be very simple such as creating vetex's with incrementing data, however my program's performance lacks when there are say a few thousand vertex's and edges. The performance becomes extremely slow (compared to what I need) when I enter the 100's of thousands. – Craig Apr 14 '15 at 18:12

When you update the vertex' estimated distance you should signal the openQueue to update it's position in the backing array.

if (!(foundNeighbor)) // if neighbor isn't in open set, add it to open set
{
}
else
{
pqOpen.UpdateItem(vertexNeighbour);
}


This means that you'd need to find it again in the priority queue and shift it up if needed.

So you may as well remove the for loop and change the name to AddOrUpdateItem

This can be more efficient by first collecting all changed neightbours to outside the loop over all neighbours. And then submitting the collection of changed vertices.

This saves you the loop over all vertices in the open set to once per pqOpen.GetNextItem().

Even that loop can be saved by using a secondary index (like a HashMap) that will let you find the index of each changed vertex in constant time.

• Ahh thank you, I shall change this when I get back home. You said "So you may as well remove the for loop and change the name to AddOrUpdateItem", which for loop do you mean? also, change the name of what to AddOrUpdateItem? Thank you – Craig Apr 15 '15 at 10:32
• @Craig where you set foundNeighbor to true. And I meant AddItem if you add functionality to update if it already is contained. – ratchet freak Apr 15 '15 at 10:36

I don't know if this is the main source of your problem, but this part definitely jumps out at me:

//if the neighbor is in closed set, move to next neighbor
for (int z = 0; z < pqClosed.GetQueueSize(); z++)
{
if (pqClosed.QueueArray[z].city == vertexNeighbour.city)
{
boolClosed = true;
}
}


Since pqClosed grows as you traverse the graph, this loop will become more and more expensive. Consider using a Set or a Map instead of whatever pqClosed is.

• Thank you for pointing that out, I have updated my code so that it now adds the visited Vertices to a Vector and then I order the vector using quick sort. I then call a simple binary search on the vector and if the result is greater than -1 (it's been found) I move on to the next neighbour. However, the initial problem still remains of it exploring too many vertices. – Craig Apr 15 '15 at 8:07
• Sorting the list every time you add something? That's not any better. Why are you trying to reinvent the wheel? Use HashSet<Vertex> or HashMap<Integer,Vertex> (map of city to vertex). – Misha Apr 15 '15 at 10:37
• I'm not familiar with hash maps, however I will learn them once I get back home and implement that - I guess my attempt earlier was due to my lack of knowledge. Thank you for the advice :) – Craig Apr 15 '15 at 10:44
• Just to keep you updated, I have been able to implement a hashmap which means my code runs much more efficiently! I've also been able to use a hash map for my collection of Edges, again meaning my start-up time is more efficient so thank you! – Craig Apr 16 '15 at 19:32

There are several issues with this code (amid being hard to read for me):

1. When SetF updates estimation priority queue do not always update (particulary if node was already there), this could make make more nodes traversed then needed.

2. I can guarantee that code do not always produce correct result, because hueristics can yeild larger value than actual path length.

3. F calculation for routetype == 2 (??) just make no sense.

Issues above can cause more nodes to be traversed or incorrect result yeilded. But there are more issues you should consider to fix:

1. Writing your own containers is a waste of time (unless you have a data structures class, which is obviosly not the case here). Use standard Java classes for this, they are generic, so one can have List and List instead of copies of the same code for different elements.

2. Some issues with naming. Particulary, dijkstra not being dijkstra and pqOpen with wierd prefix disclosing implementation detail.

3. Consider to split dijkstra function (after renaming) to several more concise ones. Right now it is a mess.

4. Looks like you do much more searching then needed, though I do not have complete knowledge about your domain. Consider to put Vertex references to edges. And consider adding bool fields (like visited) to Vertex, so you can ask Vertex if it is put into closed or open already.

• Thanks for the feedback, for 1) are you referring to what Ratchet Freak said in their answer? 2) I don't quite understand what you mean by this, my function heuristic calculates that - and I was told to always overestimate the distance so I added a small adjustment - even if I remove this adjustment I get the same result. Could you possibly expand that sentence so I can understand it better? 3) I have two route calculation types - if routetype == 1 it's based on shortest distance, 2 == shortest time, hence the calculation involving speed and distance – Craig Apr 15 '15 at 10:35
• the other issues you mentioned, 1) unfortunately I do have to write all my containers myself - this is a piece of work and I'm not allowed to use the Java standard library. 2) I understand the confusion, I'll try and stick to a standard naming convention from now on - I use the name dijkstra because I modified my implementation of that and forgot to edit the name. 3) That's a good idea, i'll do that later on! 4) I'll definitely do that - I can see i do loop through my data structures a lot. – Craig Apr 15 '15 at 10:37
• 1) Yes, this is the same as Ratchet Freak said. Haven't seen his answer when began to write my own. – GeniusIsme Apr 15 '15 at 11:29
• 2) A algorithm is only guaranteed to find a shortest path if heuristics used is admissable - ie. provides values less or equal to an actual distance. This small adjustment you've made may not cause trouble on particular data, but it is not guaranteed to do so. – GeniusIsme Apr 15 '15 at 11:39
• 3) This still doesn't make sense. Magic number do not change anything at all and speedLimit will ruin everything if it is differs along edges. Consider all edges have speedLimit == 1 and one very short edge have speedLimit == 100. Path found will contain this edge while time walking it may be quite large. – GeniusIsme Apr 15 '15 at 11:39