int end_y = end.getLoc().getY();
//List of Visited Nodes
FastNodeList visited = new FastNodeList(800);//TODO! Add limit to cfg
// List of Nodes to Visit
BinaryNodeHeap to_visit = new BinaryNodeHeap(800);
to_visit.add(start);
int i = 0;
while (i < 800)//TODO! Add limit to cfg
{
Node node;
try
{
node = to_visit.removeFirst();
}
catch (Exception e)
{
// No Path found
return null;
}
if (node.equals(end)) //path found!
return constructPath(node);
visited.add(node);
node.attacheNeighbors();
for (Node n : node.getNeighbors())
{
if (!visited.contains(n) && !to_visit.contains(n))
{
i++;
n.setParent(node);
n.setCost(Math.abs(start_x - n.getLoc().getNodeX())+Math.abs(start_y - n.getLoc().getNodeY())
+Math.abs(end_x - n.getLoc().getNodeX())+Math.abs(end_y - n.getLoc().getNodeY()));
to_visit.add(n);
}
}
}
//No Path found
return null;