I'm trying to implement A-Star in Java based on OSM Data. My problem is that my implementation is not working correctly. First of all the path is not the shortest. Second the closedlist contains more 1/3 more nodes in the end as Dijkstra. Thats actuall not that what I expected.
Here is my A-Star code which is based on Wikipedia Pseudocode
public Object[] executeAstar(ArrayList<Arclistentry> data, NodeD start, NodeD dest,long[] nodenur)
{
openlist = new PriorityQueue<NodeD>(1,comp);
closedlist.clear();
openlist.offer(start);
start.setg(0);
start.seth(calccost(start, dest));
start.setf(start.getg()+start.geth());
while(!openlist.isEmpty())
{
NodeD currentnode = openlist.poll();
if(currentnode.getnodenumber() == dest.getpredessor())
{
closedlist.add(currentnode);
return drawway(closedlist, start, dest);
}
closedlist.add(currentnode);
ArrayList<Arclistentry> entries = neighbors.get((int)currentnode.getnodenumber()-1);
for(Arclistentry aentry:entries)
{
NodeD successor = new NodeD(aentry.getnode(),aentry.getstart(), aentry.getcoorddest());
float tentative_g = currentnode.getg()+calccost(currentnode,successor);//+aentry.getcost();
if(contains(successor, closedlist))
{
continue;
}
if((contains(successor,openlist))&& tentative_g >= aentry.getcost())
{
continue;
}
if(!contains(successor, openlist))
{
successor.setpredessor(currentnode.getnodenumber());
successor.setg(tentative_g);
successor.seth(calccost(successor, dest));
successor.setf(successor.getg()+successor.geth());
openlist.offer(successor);
}
else
{
openlist.remove(successor);
successor.setpredessor(currentnode.getnodenumber());
successor.setg(tentative_g);
successor.seth(calccost(successor, dest));
successor.setf(successor.getg()+successor.geth());
openlist.offer(successor);
}
}
}
return drawway(closedlist,start, dest);
}
My Heuristics will be calculated by using the euclidian distance. But to consider also the cost of the node, the costs are multiplied with the heuristics result. My Data structure contains the following:
private long nodenumber;
private long predessor;
private float label;
private float f;
private float g;
private float h;
private double[] coord = new double[2];
public NodeD(long nodenr, long predessor, double[] coor)
{
this.nodenumber = nodenr;
this.predessor = predessor;
this.coord = coor;
}
public NodeD(long nodenr, long predessor, float label)
{
this.nodenumber = nodenr;
this.predessor = predessor;
this.label = label;
}
and for the arclist I use the following:
private long start;
private long dest_node;
private float cost_;
private double[]coordstart = new double[2];
private double[]coorddest = new double[2];
Contains Function for Priority Queue:
public boolean contains(NodeD o, PriorityQueue<NodeD> al)
{
Iterator<NodeD> e = al.iterator();
if (o==null)
{
while (e.hasNext())
{
if (e.next()==null)
{
return true;
}
}
}
else
{
while (e.hasNext())
{
NodeD t = e.next();
if(t.equals(null))
{
return false;
}
if (((o.getnodenumber()==t.getnodenumber()) & (o.getpredessor()==t.getpredessor()))||(o.getnodenumber()==t.getpredessor() & o.getpredessor()==t.getnodenumber()))
{
return true;
}
}
return false;
}
return false;
}
and contains for ArrayList (because it was not detecting right with the ArrayList.contains function
public boolean contains(NodeD o, ArrayList<NodeD> al) {
return indexOf(o,al) >= 0;
}
public int indexOf(NodeD o, ArrayList<NodeD> al) {
if (o == null) {
for (int i = 0; i < al.size(); i++)
if (al.get(i)==null)
return i;
} else {
for (int i = 0; i < al.size(); i++)
{
if ((o.getpredessor()==al.get(i).getpredessor())) //(o.getnodenumber()==al.get(i).getnodenumber()) &&
{
return i;
}
else if((o.getpredessor()==al.get(i).getnodenumber())&&(o.getnodenumber()==al.get(i).getpredessor()))
{
return i;
}
}
}
return -1;
}
The problem is that the algorithm is visiting all nodes. The other problem is the sorted openlist, which is pushing neighbors of the currentnode up, because they have a lower f value. So what I'm duing wrong by implementing this algorithm?