Free cookie consent management tool by TermsFeed Policy Generator

source: branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/AStarAlgorithmV3.cs @ 9277

Last change on this file since 9277 was 8640, checked in by spimming, 12 years ago

#1894:

  • Included costCalculator in a star search
  • restructured and renamed Dijkstra algorithm
  • Added code comments to FibonacciHeap
File size: 4.6 KB
RevLine 
[8481]1
2using System;
3using System.Collections.Generic;
[8516]4using HeuristicLab.Algorithms.GraphRouting.Interfaces;
5using HeuristicLab.Problems.RoutePlanning.Interfaces;
[8640]6using HeuristicLab.Problems.RoutePlanning.RoutingGraph;
[8481]7namespace HeuristicLab.Algorithms.GraphRouting {
8  public class AStarAlgorithmV3 : IRouter {
9    private IGraph graph;
[8640]10    private ICostCalculator costCalculator;
[8481]11
12    private Dictionary<long, float> distances;
13    private Dictionary<long, long> predecessors;
14    private List<long> closedList;
15    private HashSet<long> closedListLookup;
16    private SortedSet<NodeData> openList;
17    private Dictionary<long, NodeData> openListLookup;
18
[8640]19    public AStarAlgorithmV3(IGraph graph)
20      : this(graph, new TravelTimeCostCalculator()) {
21    }
22
23    public AStarAlgorithmV3(IGraph graph, ICostCalculator costCalc) {
[8481]24      this.graph = graph;
[8640]25      this.costCalculator = costCalc;
[8481]26    }
27
28    #region IRouter Members
29
30    public long[] Calculate(long sourceNodeId, long targetNodeId) {
31      distances = new Dictionary<long, float>();
32      predecessors = new Dictionary<long, long>();
33      closedList = new List<long>();
34      closedListLookup = new HashSet<long>();
35      openList = new SortedSet<NodeData>();
36      openListLookup = new Dictionary<long, NodeData>();
37
38      long currentNode = sourceNodeId;
39
40      NodeData nd = new NodeData(currentNode, 0);
41      openList.Add(nd);
42      openListLookup.Add(currentNode, nd);
43      distances.Add(currentNode, 0);
44
[8640]45      int i = 0;
[8481]46      while (openList.Count > 0) {
[8640]47
[8481]48        NodeData data = openList.Min;
[8640]49        //Console.Write(i + ": (" + openList.Count + "/");
[8481]50        openList.Remove(data);
[8640]51        //Console.Write(openList.Count + ") - (" + openListLookup.Count + "/");
[8481]52        openListLookup.Remove(data.Node);
[8640]53        //Console.WriteLine(openListLookup.Count + ")");
[8481]54        currentNode = data.Node;
55        if (currentNode == targetNodeId) {
56          return ReconstructPath(currentNode).ToArray();
57        }
58
[8640]59        //if (openList.Count != openListLookup.Count) {
60        //  Console.WriteLine();
61        //} else {
62        //  Console.Write(".");
63        //}
64
[8481]65        Dictionary<long, float> currentNeighbors = graph.GetNeighborsWithWeight(currentNode);
66        foreach (KeyValuePair<long, float> neighbor in currentNeighbors) {
67          if (closedListLookup.Contains(neighbor.Key))
68            continue;
69
70          float costsFromStart = GetDistance(currentNode) + neighbor.Value;
71
72          if (openListLookup.ContainsKey(neighbor.Key) && costsFromStart > GetDistance(neighbor.Key))
73            continue;
74
75          distances[neighbor.Key] = costsFromStart;
76          predecessors[neighbor.Key] = currentNode;
77
[8640]78          Vertex vertNeigh = graph.GetVertex(neighbor.Key);
79          Vertex vertTarget = graph.GetVertex(targetNodeId);
80          float f = costsFromStart + costCalculator.EstimatedCosts(vertNeigh, vertTarget);
[8481]81
82          if (openListLookup.ContainsKey(neighbor.Key)) {
83            openList.Remove(openListLookup[neighbor.Key]);
84            openListLookup.Remove(neighbor.Key);
85          }
86
87          NodeData newNode = new NodeData(neighbor.Key, f);
[8640]88          openList.Add(newNode);  // wenn f bereits vorhanden -> kein add: openList.Contains(new NodeData(123, 0.2973786f))
[8481]89          openListLookup.Add(newNode.Node, newNode);
90        }
91        closedList.Add(currentNode);
92        closedListLookup.Add(currentNode);
[8640]93        i++;
[8481]94      }
95      throw new Exception(string.Format("No route found from {0} to {1}", sourceNodeId, targetNodeId));
96    }
97
98    #endregion
99
100    private float GetDistance(long id) {
101      float d;
102      if (distances.TryGetValue(id, out d)) {
103        return d;
104      } else { return float.MaxValue; }
105    }
106
107    private List<long> ReconstructPath(long nodeId) {
108      List<long> route = new List<long>();
109      long current = nodeId;
110      long next;
111
112      if (!predecessors.TryGetValue(current, out next)) {
113        return null;
114      }
115      route.Add(current);
116      while (predecessors.TryGetValue(current, out next)) {
117        current = predecessors[current];
118        route.Add(current);
119      }
120      route.Reverse();
121      return route;
122    }
123
124    private class NodeData : IComparable<NodeData> {
125      public long Node { get; set; }
126      public float Costs { get; set; }
127
128      public NodeData(long nodeId, float f) {
129        Node = nodeId;
130        Costs = f;
131      }
132
133      #region IComparable<NodeData> Members
134
135      public int CompareTo(NodeData other) {
136        return this.Costs > other.Costs ? 1 : this.Costs < other.Costs ? -1 : 0;
137      }
138
139      #endregion
140    }
141  }
142}
Note: See TracBrowser for help on using the repository browser.