source: branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/AStarAlgorithmV2.cs @ 8408

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

    private Dictionary<long, float> distances;
    private Dictionary<long, long> predecessors;
    private List<long> closedList;
    private HashSet<long> closedListLookup;
    private SortedSet<NodeData> openList;
    private Dictionary<long, NodeData> openListLookup;

    public AStarAlgorithmV2(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<long>();
      closedListLookup = new HashSet<long>();
      openList = new SortedSet<NodeData>();
      openListLookup = new Dictionary<long, NodeData>();

      long currentNode = sourceNodeId;

      NodeData nd = new NodeData(currentNode, 0);
      openList.Add(nd);
      openListLookup.Add(currentNode, nd);
      distances.Add(currentNode, 0);

      while (openList.Count > 0) {
        NodeData data = openList.Max;
        openList.Remove(data);
        openListLookup.Remove(data.Node);
        currentNode = data.Node;
        if (currentNode == targetNodeId) {
          return ReconstructPath(currentNode).ToArray();
        }

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

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

          if (openListLookup.ContainsKey(neighbor.Key) && costsFromStart > GetDistance(neighbor.Key))
            continue;

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

          float f = costsFromStart + graph.EstimatedCosts(neighbor.Key, targetNodeId);

          if (openListLookup.ContainsKey(neighbor.Key)) {
            openList.Remove(openListLookup[neighbor.Key]);
            openListLookup.Remove(neighbor.Key);
          }

          NodeData newNode = new NodeData(neighbor.Key, f);
          openList.Add(newNode);
          openListLookup.Add(newNode.Node, newNode);
        }
        closedList.Add(currentNode);
        closedListLookup.Add(currentNode);
      }
      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 NodeData : IComparable<NodeData> {
    public long Node { get; set; }
    public float Costs { get; set; }

    public NodeData(long nodeId, float f) {
      Node = nodeId;
      Costs = f;
    }

    #region IComparable<NodeData> Members

    public int CompareTo(NodeData other) {
      return this.Costs > other.Costs ? 1 : this.Costs < other.Costs ? -1 : 0;
    }

    #endregion
  }
}