source: branches/1894_RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/DijkstraAlgorithm.cs @ 16371

using System;
using System.Collections.Generic;
using HeuristicLab.Algorithms.GraphRouting.Interfaces;
using HeuristicLab.Algorithms.GraphRouting.PriorityQueues;
using HeuristicLab.Problems.RoutePlanning.Interfaces;
namespace HeuristicLab.Algorithms.GraphRouting {
  public class DijkstraAlgorithm : IRouter {
    private IGraph graph;
    private IHeap<float, long> unvisitedNodes;
    private Dictionary<long, float> distances;
    private Dictionary<long, long> predecessors;

    public DijkstraAlgorithm(IGraph graph) {
      this.graph = graph;
    }

    #region IRouter Members

    public long[] Calculate(long sourceNodeId, long targetNodeId) {
      unvisitedNodes = new BinaryHeap<float, long>(1000000);      // unvisited
      //unvisitedNodes = new BinomialHeap<float, long>();    // unvisited
      //unvisitedNodes = new FibonacciHeap<float, long>();   // unvisited
      distances = new Dictionary<long, float>();
      predecessors = new Dictionary<long, long>();

      List<long> resultRoute = new List<long>();
      if (sourceNodeId == targetNodeId) {
        resultRoute.Add(sourceNodeId);
        return resultRoute.ToArray();
      }

      long currentNode = sourceNodeId;
      float currentWeight = 0;
      unvisitedNodes.Insert(currentWeight, currentNode);

      while (unvisitedNodes.Size > 0) {
        KeyValuePair<float, long> data = unvisitedNodes.PeekMin();
        unvisitedNodes.RemoveMin();
        currentNode = data.Value;
        currentWeight = data.Key;

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

        if (currentNode == targetNodeId) {
          break;
        }

        distances[currentNode] = currentWeight;
        RelaxNeighbors(currentNode, currentWeight);


        //--------------
        //Logger.LogToFile("");
        //--------------
      }

      if (currentNode != targetNodeId) {
        throw new Exception(string.Format("No route found from {0} to {1}", sourceNodeId, targetNodeId));
      }
      return GetRoute(targetNodeId).ToArray();
    }

    #endregion

    public void RelaxNeighbors(long nodeId, float weight) {
      Dictionary<long, float> currentNeighbors = graph.GetNeighborsWithWeight(nodeId);
      foreach (KeyValuePair<long, float> neighbor in currentNeighbors) {
        float totalWeight = weight + neighbor.Value;
        if (totalWeight < GetDistance(neighbor.Key)) {

          if (GetDistance(neighbor.Key) == float.MaxValue) {
            unvisitedNodes.Insert(totalWeight, neighbor.Key);
          } else {
            unvisitedNodes.DecreaseKey(totalWeight, neighbor.Key);
          }

          distances[neighbor.Key] = totalWeight;
          predecessors[neighbor.Key] = nodeId;

          //--------------
          //Logger.LogToFile("    neigb:" + neighbor.Key + " (" + totalWeight + ")");
          //--------------
        }
      }
    }

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

    private List<long> GetRoute(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;
    }
  }
}