I am currently working on an implementation of the A* Algorithm with irregular distances between two nodes. The graph containing the nodes is a directed and weighted graph. Every node is connected to at least one other node, there may also be symmetrical connections with different distances. A node is nothing more than a label and doesn't contain any special information
What I need is a heuristic to determine the shortest path from any node A to another node B as accurate as possible. I tried to use a heuristic that returns the distance to the nearest neighbor of a node, but of course that wasn't as effective as no heuristic at all (= Dijkstra).
My implementation of the A* Algorithm consists mainly of 2 classes, the class for the algorithm itself (AStar) and one for the nodes (Node). The code is heavily based on the Wikipedia pseudocode.
Source code of AStar.java
public class AStar {
private AStar() {}
private static Node[] reconstructPath(Map<Node, Node> paths, Node current) {
List<Node> path = new ArrayList<Node>();
path.add(0, current);
while (paths.containsKey(current)) {
current = paths.get(current);
path.add(0, current);
}
return path.toArray(new Node[0]);
}
public static Node[] calculate(Node start, Node target, IHeuristic heuristic) {
List<Node> closed = new ArrayList<Node>();
PriorityQueue<Node> open = new PriorityQueue<Node>();
Map<Node, Double> g_score = new HashMap<Node, Double>();
Map<Node, Double> f_score = new HashMap<Node, Double>();
Map<Node, Node> paths = new HashMap<Node, Node>();
g_score.put(start, 0d);
f_score.put(start, g_score.get(start) + heuristic.estimateDistance(start, target));
open.set(start, f_score.get(start));
while (!open.isEmpty()) {
Node current = null;
// find the node with lowest f_score value
double min_f_score = Double.POSITIVE_INFINITY;
for (Entry<Node, Double> entry : f_score.entrySet()) {
if (!closed.contains(entry.getKey()) && entry.getValue() < min_f_score) {
min_f_score = entry.getValue();
current = entry.getKey();
}
}
if (current.equals(target)) return reconstructPath(paths, target);
open.remove(current);
closed.add(current);
for (Node neighbor : current.getAdjacentNodes()) {
if (closed.contains(neighbor)) {
continue;
}
double tentative_g_score = g_score.get(current) + current.getDistance(neighbor);
if (!open.contains(neighbor) || tentative_g_score < g_score.get(neighbor)) {
paths.put(neighbor, current);
g_score.put(neighbor, tentative_g_score);
f_score.put(neighbor, g_score.get(neighbor) + heuristic.estimateDistance(neighbor, target));
if (!open.contains(neighbor)) {
open.set(neighbor, f_score.get(neighbor));
}
}
}
}
throw new RuntimeException("no path between " + start + " and " + target);
}
}
Source code of Node.java
public class Node {
private Map<Node, Double> distances = new HashMap<Node, Double>();
public final String name;
public Node(String name) {
this.name = name;
}
public Set<Node> getAdjacentNodes() {
return Collections.unmodifiableSet(distances.keySet());
}
public double getDistance(Node node) {
return distances.get(node);
}
public void setDistance(Node node, double distance) {
distances.put(node, distance);
}
@Override
public String toString() {
return (name == null ? "Node@" + Integer.toHexString(hashCode()) : name);
}
}
Source code of PriorityQueue.java
public class PriorityQueue<T> {
transient ArrayList<PriorityEntry<T>> elements = null;
private static final int DEFAULT_SIZE = 10;
public PriorityQueue() {
elements = new ArrayList<PriorityEntry<T>>(DEFAULT_SIZE);
}
public PriorityQueue(int initialCapacity) {
elements = new ArrayList<PriorityEntry<T>>(initialCapacity);
}
public boolean push(T element, double priority) {
PriorityEntry<T> entry = new PriorityEntry<T>(element, priority);
if (elements.contains(entry)) return false;
elements.add(entry);
elements.sort(null);
return true;
}
public void set(T element, double priority) {
PriorityEntry<T> entry = new PriorityEntry<T>(element, priority);
int index = elements.indexOf(entry);
if (index >= 0) {
elements.get(index).setPriority(priority);
} else {
elements.add(entry);
}
elements.sort(null);
}
public T peek() {
return size() <= 0 ? null : elements.get(0).getValue();
}
public T pop() {
return size() <= 0 ? null : elements.remove(0).getValue();
}
public boolean remove(T element) {
return elements.remove(new PriorityEntry<T>(element, 0));
}
public int size() {
return elements.size();
}
public boolean isEmpty() {
return elements.isEmpty();
}
public boolean contains(T element) {
return elements.contains(new PriorityEntry<T>(element, 0));
}
private class PriorityEntry<E> implements Comparable<PriorityEntry<? extends T>> {
private final E value;
private double priority = Double.MIN_VALUE;
public PriorityEntry(E value, double priority) {
this.value = value;
this.priority = priority;
}
public E getValue() {
return value;
}
public double getPriority() {
return priority;
}
public void setPriority(double priority) {
this.priority = priority;
}
@Override
@SuppressWarnings("unchecked")
public boolean equals(Object o) {
if (!(o instanceof PriorityEntry)) return false;
PriorityEntry<?> entry = (PriorityEntry<?>) o;
return value.equals(entry);
}
@Override
public int compareTo(PriorityEntry<? extends T> entry) {
return (int) (getPriority() - entry.getPriority());
}
}
}
To add to @kiheru comment above. Your solution will only be as good as the heuristic provided.
If the following line and the heuristic.estimate, has too narrow of a scope. The algorithm will quickly reach a local minimum. Alternatively, if the heuristic isn't admissible the algorithm will result in either no solution or an incorrect random solution.
Take a close look at your heuristic and confirm it's admissible. If it is admissible, it may need to be improved in order to provide a more accurate estimate.