source: branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/AStarAlgorithmV4.cs @ 11303

using System;
using System.Collections.Generic;
using HeuristicLab.Algorithms.GraphRouting.Interfaces;
using HeuristicLab.Problems.RoutePlanning.Interfaces;
using HeuristicLab.Problems.RoutePlanning.RoutingGraph;
namespace HeuristicLab.Algorithms.GraphRouting {
  public class AStarAlgorithmV4 : IRouter {
    private IGraph graph;
    private ICostCalculator costCalculator;

    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 AStarAlgorithmV4(IGraph graph)
      : this(graph, new TravelTimeCostCalculator()) {
    }

    public AStarAlgorithmV4(IGraph graph, ICostCalculator costCalc) {
      this.graph = graph;
      this.costCalculator = costCalc;
    }

    #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.Min;
        openList.Remove(data);
        openListLookup.Remove(data.Node);
        currentNode = data.Node;
        if (currentNode == targetNodeId) {
          return ReconstructPath(currentNode).ToArray();
        }

        //--------------
        //Logger.LogToFile("cn = " + currentNode + " (" + data.Costs + ")");
        //--------------

        Dictionary<long, float> currentNeighbors = graph.GetNeighborsWithWeight(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;

          Vertex vertNeigh = graph.GetVertex(neighbor.Key);
          Vertex vertTarget = graph.GetVertex(targetNodeId);
          float f = costsFromStart + costCalculator.EstimatedCosts(vertNeigh, vertTarget);

          //--------------
          //Logger.LogToFile("    neigb:" + neighbor.Key + " (" + costsFromStart + " / " + f + ")");
          //--------------

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

          NodeData newNode = new NodeData(neighbor.Key, f);
          while (!openList.Add(newNode)) {
            newNode.Costs = newNode.Costs + 0.0000001f;
          }
          openListLookup.Add(newNode.Node, newNode);
        }
        closedList.Add(currentNode);
        closedListLookup.Add(currentNode);
        //--------------
        //Logger.LogToFile("");
        //--------------
      }
      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;
    }

    private 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
    }
  }
}