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;
pqOpen.AddItem(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
{
pqClosed.AddItem(tempVertex);
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;
resultEdges.AddItem(tempEdge);
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
{
pqOpen.AddItem(vertexNeighbour);
}
}
}
}
}
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 void addEdge(Edge city)
{
this.neighbors.AddItem(city);
}
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);
}
public void AddItem(Object item)
{
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;
}
public void AddItem(Vertex item)
{
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?
pqOpen
is a home brew priority queue? \$\endgroup\$