source: branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/DijkstraAlgorithm.cs @ 8481

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

    private HashSet<long> visitedNodes;
    private HashSet<long> unvisitedNodes;
    private Dictionary<long, float> distances;
    private Dictionary<long, long> predecessors;

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

    #region IRouter Members

    public long[] Calculate(long sourceNodeId, long targetNodeId) {
      visitedNodes = new HashSet<long>();        // visited
      unvisitedNodes = new HashSet<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;
      distances.Add(currentNode, 0);
      unvisitedNodes.Add(currentNode);

      while (unvisitedNodes.Count > 0) {
        currentNode = GetMinimumDistance(unvisitedNodes);
        visitedNodes.Add(currentNode);
        unvisitedNodes.Remove(currentNode);
        if (currentNode == targetNodeId) {
          break;
        }
        FindMinimalWeight(currentNode);
      }

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

    #endregion

    private long GetMinimumDistance(HashSet<long> nodeIds) {
      long minimum = -1;
      foreach (long id in nodeIds) {
        if (minimum == -1) minimum = id;
        else {
          if (GetDistance(id) < GetDistance(minimum)) {
            minimum = id;
          }
        }
      }
      return minimum;
    }

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

    }

    private void FindMinimalWeight(long nodeId) {
      Dictionary<long, float> currentNeighbors = graph.GetNeighbors(nodeId);
      foreach (KeyValuePair<long, float> neighbor in currentNeighbors) {
        // update relax
        float totalWeight = GetDistance(nodeId) + neighbor.Value;
        if (GetDistance(neighbor.Key) > totalWeight) {
          distances[neighbor.Key] = totalWeight;
          predecessors[neighbor.Key] = nodeId;
          unvisitedNodes.Add(neighbor.Key);
        }
      }
    }

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