source: branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/AStarAlgorithm.cs @ 8350

using System;
using System.Collections.Generic;
using HeuristicLab.Problems.RoutePlanning.Graph;
namespace HeuristicLab.Algorithms.GraphRouting {
  public class AStarAlgorithm : IRouter {
    private Graph graph;

    private Dictionary<long, float> distances;
    private Dictionary<long, long> predecessors;
    private List<float> closedList = new List<float>();
    //private PriorityQueueOld<float, long> openList;
    private PriorityQueue<float, long> openList;

    public AStarAlgorithm(Graph graph) {
      this.graph = graph;
    }

    #region IRouter Members

    public long[] Calculate(long sourceNodeId, long targetNodeId) {
      distances = new Dictionary<long, float>();
      predecessors = new Dictionary<long, long>();
      closedList = new List<float>();
      //openList = new PriorityQueueOld<float, long>();
      openList = new PriorityQueue<float, long>(new MyComparer());

      long currentNode = sourceNodeId;

      //openList.Enqueue(currentNode, 0);
      openList.Enqueue(0, currentNode);
      distances.Add(currentNode, 0);

      while (openList.Count > 0) {
        //currentNode = openList.Dequeue();
        currentNode = openList.DequeueValue();
        if (currentNode == targetNodeId) {
          return ReconstructPath(currentNode).ToArray();
        }

        Dictionary<long, float> currentNeighbors = graph.GetNeighbors(currentNode);
        foreach (KeyValuePair<long, float> neighbor in currentNeighbors) {
          if (closedList.Contains(neighbor.Key))
            continue;

          float costsFromStart = GetDistance(currentNode) + neighbor.Value;

          if (openList.Contains(neighbor.Key) && costsFromStart > GetDistance(neighbor.Key))
            continue;

          distances[neighbor.Key] = costsFromStart;
          predecessors[neighbor.Key] = currentNode;

          float f = costsFromStart + graph.EstimatedCosts(neighbor.Key, targetNodeId);
          if (openList.Contains(neighbor.Key)) {
            openList.Remove(neighbor.Key);
          }
          //openList.Enqueue(neighbor.Key, f);
          openList.Enqueue(f, neighbor.Key);
        }
      }
      throw new Exception(string.Format("No route found from {0} to {1}", sourceNodeId, targetNodeId));
    }

    #endregion

    private float GetDistance(long id) {
      float d;
      if (distances.TryGetValue(id, out d)) {
        return d;
      } else { return float.MaxValue; }

    }

    private List<long> ReconstructPath(long nodeId) {
      List<long> route = new List<long>();
      long current = nodeId;
      long next;

      if (!predecessors.TryGetValue(current, out next)) {
        return null;
      }
      route.Add(current);
      while (predecessors.TryGetValue(current, out next)) {
        current = predecessors[current];
        route.Add(current);
      }
      route.Reverse();
      return route;
    }
  }

  class MyComparer : IComparer<float> {
    public int Compare(float item1, float item2) {
      return item1 > item2 ? 1 : item1 < item2 ? -1 : 0;

      // inverted comparison
      //return item1 < item2 ? 1 : item1 > item2 ? -1 : 0;
    }
  }
}