source: branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/AStarAlgorithmV5.cs @ 14604

using System;
using System.Collections.Generic;
using HeuristicLab.Algorithms.GraphRouting.Interfaces;
using HeuristicLab.Algorithms.GraphRouting.PriorityQueues;
using HeuristicLab.Problems.RoutePlanning.Interfaces;
using HeuristicLab.Problems.RoutePlanning.RoutingGraph;
namespace HeuristicLab.Algorithms.GraphRouting {
  public class AStarAlgorithmV5 : 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 HashSet<long> openListLookup;
    private IHeap<float, long> openList;

    public AStarAlgorithmV5(IGraph graph)
      : this(graph, new TravelTimeCostCalculator()) {
    }

    public AStarAlgorithmV5(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 BinHeap<float, long>(300000);
      openList = new BinomialHeap<float, long>();
      //openList = new NaivePriorityQueue<float, long>();
      openListLookup = new HashSet<long>();

      long currentNode = sourceNodeId;

      openList.Insert(0, currentNode);
      openListLookup.Add(currentNode);
      distances.Add(currentNode, 0);

      while (openList.Size > 0) {
        // get the node form the open list having the lowest score

        //long dataId = openList.PeekMinValue();
        //float dataF = openList.PeekMinKey();

        KeyValuePair<float, long> data = openList.PeekMin();
        long dataId = data.Value;
        float dataF = data.Key;
        currentNode = dataId;

        if (currentNode == targetNodeId) {
          return ReconstructPath(currentNode).ToArray();
        }

        // remove current from open list
        openList.RemoveMin();
        openListLookup.Remove(currentNode);

        // add current to closed list
        closedList.Add(currentNode);
        closedListLookup.Add(currentNode);

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

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

          Vertex vn = graph.GetVertex(neighbor.Key);
          Vertex vt = graph.GetVertex(targetNodeId);
          float costsFromStart = GetDistance(currentNode) + neighbor.Value;
          float f = costsFromStart + costCalculator.EstimatedCosts(vn, vt);

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

          bool isInOpenList = openListLookup.Contains(neighbor.Key);
          if (!isInOpenList || costsFromStart < GetDistance(neighbor.Key)) {
            if (!isInOpenList) {
              openList.Insert(f, neighbor.Key);
              openListLookup.Add(neighbor.Key);
            } else {
              openList.DecreaseKey(f, neighbor.Key);
            }
            distances[neighbor.Key] = costsFromStart;
            predecessors[neighbor.Key] = 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;
    }
  }
}