I am currently working on an implementation of the A * algorithm with irregular distances between two nodes. A graph containing nodes is a directed and weighted graph. Each 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 does not contain any special information.
I need a heuristic to determine the shortest path from any node A to another node B as accurately as possible. I tried using a heuristic that returns the distance to the nearest neighbor of the node, but of course it was not as efficient as no heuristic at all (= Dijkstra).
My implementation of the A * algorithm consists mainly of 2 classes, a class for the algorithm itself ( AStar ) and one for nodes ( Node ). The code is largely based on the Wikipedia pseudo-code.
Source Code 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;
Node.java Source 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 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()); } } }