1 |
|
---|
2 | using System;
|
---|
3 | using System.Collections.Generic;
|
---|
4 | using HeuristicLab.Problems.RoutePlanning.Graph;
|
---|
5 | namespace HeuristicLab.Algorithms.GraphRouting {
|
---|
6 | public class AStarAlgorithmV3 : IRouter {
|
---|
7 | private IGraph graph;
|
---|
8 |
|
---|
9 | private Dictionary<long, float> distances;
|
---|
10 | private Dictionary<long, long> predecessors;
|
---|
11 | private List<long> closedList;
|
---|
12 | private HashSet<long> closedListLookup;
|
---|
13 | private SortedSet<NodeData> openList;
|
---|
14 | private Dictionary<long, NodeData> openListLookup;
|
---|
15 |
|
---|
16 | public AStarAlgorithmV3(IGraph graph) {
|
---|
17 | this.graph = graph;
|
---|
18 | }
|
---|
19 |
|
---|
20 | #region IRouter Members
|
---|
21 |
|
---|
22 | public long[] Calculate(long sourceNodeId, long targetNodeId) {
|
---|
23 | distances = new Dictionary<long, float>();
|
---|
24 | predecessors = new Dictionary<long, long>();
|
---|
25 | closedList = new List<long>();
|
---|
26 | closedListLookup = new HashSet<long>();
|
---|
27 | openList = new SortedSet<NodeData>();
|
---|
28 | openListLookup = new Dictionary<long, NodeData>();
|
---|
29 |
|
---|
30 | long currentNode = sourceNodeId;
|
---|
31 |
|
---|
32 | NodeData nd = new NodeData(currentNode, 0);
|
---|
33 | openList.Add(nd);
|
---|
34 | openListLookup.Add(currentNode, nd);
|
---|
35 | distances.Add(currentNode, 0);
|
---|
36 |
|
---|
37 | while (openList.Count > 0) {
|
---|
38 | NodeData data = openList.Min;
|
---|
39 | openList.Remove(data);
|
---|
40 | openListLookup.Remove(data.Node);
|
---|
41 | currentNode = data.Node;
|
---|
42 | if (currentNode == targetNodeId) {
|
---|
43 | return ReconstructPath(currentNode).ToArray();
|
---|
44 | }
|
---|
45 |
|
---|
46 | Dictionary<long, float> currentNeighbors = graph.GetNeighborsWithWeight(currentNode);
|
---|
47 | foreach (KeyValuePair<long, float> neighbor in currentNeighbors) {
|
---|
48 | if (closedListLookup.Contains(neighbor.Key))
|
---|
49 | continue;
|
---|
50 |
|
---|
51 | float costsFromStart = GetDistance(currentNode) + neighbor.Value;
|
---|
52 |
|
---|
53 | if (openListLookup.ContainsKey(neighbor.Key) && costsFromStart > GetDistance(neighbor.Key))
|
---|
54 | continue;
|
---|
55 |
|
---|
56 | distances[neighbor.Key] = costsFromStart;
|
---|
57 | predecessors[neighbor.Key] = currentNode;
|
---|
58 |
|
---|
59 | float f = costsFromStart + graph.EstimatedCosts(neighbor.Key, targetNodeId);
|
---|
60 |
|
---|
61 | if (openListLookup.ContainsKey(neighbor.Key)) {
|
---|
62 | openList.Remove(openListLookup[neighbor.Key]);
|
---|
63 | openListLookup.Remove(neighbor.Key);
|
---|
64 | }
|
---|
65 |
|
---|
66 | NodeData newNode = new NodeData(neighbor.Key, f);
|
---|
67 | openList.Add(newNode);
|
---|
68 | openListLookup.Add(newNode.Node, newNode);
|
---|
69 | }
|
---|
70 | closedList.Add(currentNode);
|
---|
71 | closedListLookup.Add(currentNode);
|
---|
72 | }
|
---|
73 | throw new Exception(string.Format("No route found from {0} to {1}", sourceNodeId, targetNodeId));
|
---|
74 | }
|
---|
75 |
|
---|
76 | #endregion
|
---|
77 |
|
---|
78 | private float GetDistance(long id) {
|
---|
79 | float d;
|
---|
80 | if (distances.TryGetValue(id, out d)) {
|
---|
81 | return d;
|
---|
82 | } else { return float.MaxValue; }
|
---|
83 | }
|
---|
84 |
|
---|
85 | private List<long> ReconstructPath(long nodeId) {
|
---|
86 | List<long> route = new List<long>();
|
---|
87 | long current = nodeId;
|
---|
88 | long next;
|
---|
89 |
|
---|
90 | if (!predecessors.TryGetValue(current, out next)) {
|
---|
91 | return null;
|
---|
92 | }
|
---|
93 | route.Add(current);
|
---|
94 | while (predecessors.TryGetValue(current, out next)) {
|
---|
95 | current = predecessors[current];
|
---|
96 | route.Add(current);
|
---|
97 | }
|
---|
98 | route.Reverse();
|
---|
99 | return route;
|
---|
100 | }
|
---|
101 |
|
---|
102 | private class NodeData : IComparable<NodeData> {
|
---|
103 | public long Node { get; set; }
|
---|
104 | public float Costs { get; set; }
|
---|
105 |
|
---|
106 | public NodeData(long nodeId, float f) {
|
---|
107 | Node = nodeId;
|
---|
108 | Costs = f;
|
---|
109 | }
|
---|
110 |
|
---|
111 | #region IComparable<NodeData> Members
|
---|
112 |
|
---|
113 | public int CompareTo(NodeData other) {
|
---|
114 | return this.Costs > other.Costs ? 1 : this.Costs < other.Costs ? -1 : 0;
|
---|
115 | }
|
---|
116 |
|
---|
117 | #endregion
|
---|
118 | }
|
---|
119 | }
|
---|
120 | }
|
---|