title: Heuristic for A*-Algorithm with irregular distances between nodes
body: <p>I am currently working on an implementation of the <a href="http://en.wikipedia.org/wiki/A*_Algorithm" rel="nofollow">A* Algorithm</a> 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</p>
  
  <p>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).</p>
  
  <hr>
  
  <p>My implementation of the A* Algorithm consists mainly of 2 classes, the class for the algorithm itself (<code>AStar</code>) and one for the nodes (<code>Node</code>). The code is heavily based on the Wikipedia pseudocode.</p>
  
  <h3>Source code of <code>AStar.java</code></h3>
  
  
  
  <pre><code>public class AStar {
      private AStar() {}
  
      private static Node[] reconstructPath(Map&lt;Node, Node&gt; paths, Node current) {
          List&lt;Node&gt; path = new ArrayList&lt;Node&gt;();
          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&lt;Node&gt; closed = new ArrayList&lt;Node&gt;();
          PriorityQueue&lt;Node&gt; open = new PriorityQueue&lt;Node&gt;();
          Map&lt;Node, Double&gt; g_score = new HashMap&lt;Node, Double&gt;();
          Map&lt;Node, Double&gt; f_score = new HashMap&lt;Node, Double&gt;();
          Map&lt;Node, Node&gt; paths = new HashMap&lt;Node, Node&gt;();
  
          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&lt;Node, Double&gt; entry : f_score.entrySet()) {
                  if (!closed.contains(entry.getKey()) &amp;&amp; entry.getValue() &lt; 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 &lt; 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);
      }
  }
  </code></pre>
  
  <h3>Source code of <code>Node.java</code></h3>
  
  <pre><code>public class Node {
      private Map&lt;Node, Double&gt; distances = new HashMap&lt;Node, Double&gt;();
  
      public final String       name;
  
      public Node(String name) {
          this.name = name;
      }
  
      public Set&lt;Node&gt; 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);
      }
  }
  </code></pre>
  
  <h3>Source code of <code>PriorityQueue.java</code></h3>
  
  <pre><code>public class PriorityQueue&lt;T&gt; {
      transient ArrayList&lt;PriorityEntry&lt;T&gt;&gt; elements     = null;
  
      private static final int              DEFAULT_SIZE = 10;
  
      public PriorityQueue() {
          elements = new ArrayList&lt;PriorityEntry&lt;T&gt;&gt;(DEFAULT_SIZE);
      }
  
      public PriorityQueue(int initialCapacity) {
          elements = new ArrayList&lt;PriorityEntry&lt;T&gt;&gt;(initialCapacity);
      }
  
      public boolean push(T element, double priority) {
          PriorityEntry&lt;T&gt; entry = new PriorityEntry&lt;T&gt;(element, priority);
          if (elements.contains(entry)) return false;
          elements.add(entry);
          elements.sort(null);
          return true;
      }
  
      public void set(T element, double priority) {
          PriorityEntry&lt;T&gt; entry = new PriorityEntry&lt;T&gt;(element, priority);
          int index = elements.indexOf(entry);
          if (index &gt;= 0) {
              elements.get(index).setPriority(priority);
          } else {
              elements.add(entry);
          }
          elements.sort(null);
      }
  
      public T peek() {
          return size() &lt;= 0 ? null : elements.get(0).getValue();
      }
  
      public T pop() {
          return size() &lt;= 0 ? null : elements.remove(0).getValue();
      }
  
      public boolean remove(T element) {
          return elements.remove(new PriorityEntry&lt;T&gt;(element, 0));
      }
  
      public int size() {
          return elements.size();
      }
  
      public boolean isEmpty() {
          return elements.isEmpty();
      }
  
      public boolean contains(T element) {
          return elements.contains(new PriorityEntry&lt;T&gt;(element, 0));
      }
  
      private class PriorityEntry&lt;E&gt; implements Comparable&lt;PriorityEntry&lt;? extends T&gt;&gt; {
          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&lt;?&gt; entry = (PriorityEntry&lt;?&gt;) o;
              return value.equals(entry);
          }
  
          @Override
          public int compareTo(PriorityEntry&lt;? extends T&gt; entry) {
              return (int) (getPriority() - entry.getPriority());
          }
      }
  }
  </code></pre>