Index: branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/AStarAlgorithmV2.cs
===================================================================
--- branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/AStarAlgorithmV2.cs	(revision 8408)
+++ branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/AStarAlgorithmV2.cs	(revision 8408)
@@ -0,0 +1,120 @@
+﻿
+using System;
+using System.Collections.Generic;
+using HeuristicLab.Problems.RoutePlanning.Graph;
+namespace HeuristicLab.Algorithms.GraphRouting {
+  public class AStarAlgorithmV2 : IRouter {
+    private Graph graph;
+
+    private Dictionary<long, float> distances;
+    private Dictionary<long, long> predecessors;
+    private List<long> closedList;
+    private HashSet<long> closedListLookup;
+    private SortedSet<NodeData> openList;
+    private Dictionary<long, NodeData> openListLookup;
+
+    public AStarAlgorithmV2(Graph graph) {
+      this.graph = graph;
+    }
+
+    #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 SortedSet<NodeData>();
+      openListLookup = new Dictionary<long, NodeData>();
+
+      long currentNode = sourceNodeId;
+
+      NodeData nd = new NodeData(currentNode, 0);
+      openList.Add(nd);
+      openListLookup.Add(currentNode, nd);
+      distances.Add(currentNode, 0);
+
+      while (openList.Count > 0) {
+        NodeData data = openList.Max;
+        openList.Remove(data);
+        openListLookup.Remove(data.Node);
+        currentNode = data.Node;
+        if (currentNode == targetNodeId) {
+          return ReconstructPath(currentNode).ToArray();
+        }
+
+        Dictionary<long, float> currentNeighbors = graph.GetNeighbors(currentNode);
+        foreach (KeyValuePair<long, float> neighbor in currentNeighbors) {
+          if (closedListLookup.Contains(neighbor.Key))
+            continue;
+
+          float costsFromStart = GetDistance(currentNode) + neighbor.Value;
+
+          if (openListLookup.ContainsKey(neighbor.Key) && costsFromStart > GetDistance(neighbor.Key))
+            continue;
+
+          distances[neighbor.Key] = costsFromStart;
+          predecessors[neighbor.Key] = currentNode;
+
+          float f = costsFromStart + graph.EstimatedCosts(neighbor.Key, targetNodeId);
+
+          if (openListLookup.ContainsKey(neighbor.Key)) {
+            openList.Remove(openListLookup[neighbor.Key]);
+            openListLookup.Remove(neighbor.Key);
+          }
+
+          NodeData newNode = new NodeData(neighbor.Key, f);
+          openList.Add(newNode);
+          openListLookup.Add(newNode.Node, newNode);
+        }
+        closedList.Add(currentNode);
+        closedListLookup.Add(currentNode);
+      }
+      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;
+    }
+  }
+
+  class NodeData : IComparable<NodeData> {
+    public long Node { get; set; }
+    public float Costs { get; set; }
+
+    public NodeData(long nodeId, float f) {
+      Node = nodeId;
+      Costs = f;
+    }
+
+    #region IComparable<NodeData> Members
+
+    public int CompareTo(NodeData other) {
+      return this.Costs > other.Costs ? 1 : this.Costs < other.Costs ? -1 : 0;
+    }
+
+    #endregion
+  }
+}
Index: branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/HeuristicLab.Algorithms.GraphRouting.csproj
===================================================================
--- branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/HeuristicLab.Algorithms.GraphRouting.csproj	(revision 8369)
+++ branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/HeuristicLab.Algorithms.GraphRouting.csproj	(revision 8408)
@@ -58,4 +58,5 @@
   <ItemGroup>
     <Compile Include="AStarAlgorithm.cs" />
+    <Compile Include="AStarAlgorithmV2.cs" />
     <Compile Include="DijkstraAlgorithm.cs" />
     <Compile Include="GraphRoutingAlgorithm.cs" />
Index: branches/RoutePlanning/HeuristicLab.Problems.RoutePlanning.Test/Program.cs
===================================================================
--- branches/RoutePlanning/HeuristicLab.Problems.RoutePlanning.Test/Program.cs	(revision 8369)
+++ branches/RoutePlanning/HeuristicLab.Problems.RoutePlanning.Test/Program.cs	(revision 8408)
@@ -11,4 +11,8 @@
   class Program {
     static void Main(string[] args) {
+      // For testing openstreetmap data is used
+      // http://www.openstreetmap.org
+      // Map data © OpenStreetMap contributors, CC BY-SA
+
       string file1 = @"..\..\OsmTestFiles\test.osm";
       string file2 = @"..\..\OsmTestFiles\testNode1.osm";
@@ -18,13 +22,62 @@
       string file6 = @"C:\dev\osmfiles\oberosterreich.highway.osm";
 
-      Test(file5, 529102170, 1001363194, true, false);
-      //Test(file6, 529102170, 1001363194, true, false);
-      //Test(file6, 529102170, 31372732, false, true); // inz - hgb
+      //TestD(file5, 529102170, 1001363194, true, false); // inz - inz
+      //Test(file5, 529102170, 1001363194, true, false); // inz - inz
+      //Test(file6, 529102170, 1001363194, true, false); // inz - inz
+
+      IDataSource ds = TestLoad(file6);
+      Graph.Graph graph = new Graph.Graph(ds);
+      TestRouter(new AStarAlgorithm(graph), 529102170, 31372732, false); // inz - hgb
+      TestRouter(new AStarAlgorithmV2(graph), 529102170, 31372732, false); // inz - hgb
+
+      //TestLoadAndRouter(file6, typeof(DijkstraAlgorithm), 529102170, 31372732, false, false); // inz - hgb
+      //TestLoadAndRouter(file6, typeof(AStarAlgorithm), 529102170, 31372732, false, false); // inz - hgb
+      //TestLoadAndRouter(file6, typeof(AStarAlgorithmV2), 529102170, 31372732, false, false); // inz - hgb
+
       System.Console.Read();
     }
 
-    private static void Test(string filepath, long sourceNodeId, long targetNodeId, bool showResult, bool writeResultFile) {
+    private static long[] TestRouter(IRouter router, long sourceNodeId, long targetNodeId, bool showResult) {
+      Console.WriteLine("Test Router BEGIN ---------------------------------");
+      Console.WriteLine("Type: " + router.GetType());
+
+      Console.Write("Calculate route ... ");
+      var sw = Stopwatch.StartNew();
+      long[] result = router.Calculate(sourceNodeId, targetNodeId);
+      sw.Stop();
+      Console.WriteLine("done.");
+      Console.WriteLine("Execution Time: {0}", sw.Elapsed);
+      Console.WriteLine();
+      if (showResult) {
+        Console.WriteLine("Result: ");
+        foreach (long i in result) System.Console.Write(i + "; ");
+        Console.WriteLine();
+        Console.WriteLine();
+      }
+      Console.WriteLine("===================================================");
+      Console.WriteLine();
+      return result;
+    }
+
+    private static IDataSource TestLoad(string filepath) {
       FileInfo file = new FileInfo(filepath);
-      Console.WriteLine("Test BEGIN ------------------------------");
+      Console.WriteLine("Test Load Data BEGIN ------------------------------");
+      Console.WriteLine("File name: {0}", file.Name);
+      Console.WriteLine();
+      Console.Write("Loading data ... ");
+      var sw = Stopwatch.StartNew();
+      XmlDataSource xmlDs = new XmlDataSource(filepath);
+      sw.Stop();
+      Console.WriteLine("done.");
+      Console.WriteLine("Time: {0}", sw.Elapsed);
+      Console.WriteLine();
+      Console.WriteLine("===================================================");
+      Console.WriteLine();
+      return xmlDs;
+    }
+
+    private static void TestLoadAndRouter(string filepath, Type routerType, long sourceNodeId, long targetNodeId, bool showResult, bool writeResultFile) {
+      FileInfo file = new FileInfo(filepath);
+      Console.WriteLine("Test Load and Route BEGIN -------------------------");
       Console.WriteLine("File name: {0}", file.Name);
       Console.WriteLine();
@@ -37,6 +90,5 @@
       Console.WriteLine();
       Graph.Graph graph = new Graph.Graph(xmlDs);
-      //IRouter router = new DijkstraAlgorithm(graph);
-      IRouter router = new AStarAlgorithm(graph);
+      IRouter router = (IRouter)Activator.CreateInstance(routerType, graph);
       Console.Write("Calculate route ... ");
       sw = Stopwatch.StartNew();
@@ -59,5 +111,5 @@
         WriteGPXFile(graph, result, resultFile);
       }
-      Console.WriteLine("=========================================");
+      Console.WriteLine("===================================================");
       Console.WriteLine();
     }
Index: branches/RoutePlanning/HeuristicLab.Problems.RoutePlanning/3.3/Graph/Graph.cs
===================================================================
--- branches/RoutePlanning/HeuristicLab.Problems.RoutePlanning/3.3/Graph/Graph.cs	(revision 8369)
+++ branches/RoutePlanning/HeuristicLab.Problems.RoutePlanning/3.3/Graph/Graph.cs	(revision 8408)
@@ -61,59 +61,5 @@
     public float GetWeight(Way edge, Vertex<Node> source, Vertex<Node> target) {
       double distance = Utils.Distance(source.Node.Point, target.Node.Point);
-      int speed = 0;
-
-      //TODO: 
-      HighwayType highwayType = edge.HighwayTag;
-      switch (highwayType) {
-        case HighwayType.bridleway:
-        case HighwayType.bus_guideway:
-        case HighwayType.raceway:
-        case HighwayType.cycleway:
-        case HighwayType.footway:
-        case HighwayType.steps:
-        case HighwayType.null_type:
-          speed = 1;
-          break;
-
-        case HighwayType.path:
-        case HighwayType.service:
-        case HighwayType.pedestrian:
-        case HighwayType.living_street:
-          speed = 15;
-          break;
-
-        case HighwayType.road:
-        case HighwayType.track:
-          speed = 30;
-          break;
-
-        case HighwayType.tertiary:
-        case HighwayType.tertiary_link:
-        case HighwayType.secondary:
-        case HighwayType.secondary_link:
-          speed = 80;
-          break;
-
-        case HighwayType.unclassified:
-        case HighwayType.residential:
-          speed = 50;
-          break;
-
-        case HighwayType.trunk:
-        case HighwayType.trunk_link:
-        case HighwayType.primary:
-        case HighwayType.primary_link:
-          speed = 100;
-          break;
-        case HighwayType.motorway:
-        case HighwayType.motorway_link:
-          speed = 130;
-          break;
-
-        default:
-          speed = 1;
-          break;
-      }
-
+      int speed = edge.GetMaxSpeed();
       double weight = (distance / speed) * 3.6;
       return (float)weight;
Index: branches/RoutePlanning/HeuristicLab.Problems.RoutePlanning/3.3/HeuristicLab.Problems.RoutePlanning.csproj
===================================================================
--- branches/RoutePlanning/HeuristicLab.Problems.RoutePlanning/3.3/HeuristicLab.Problems.RoutePlanning.csproj	(revision 8369)
+++ branches/RoutePlanning/HeuristicLab.Problems.RoutePlanning/3.3/HeuristicLab.Problems.RoutePlanning.csproj	(revision 8408)
@@ -81,5 +81,4 @@
     <None Include="Properties\AssemblyInfo.cs.frame" />
   </ItemGroup>
-  <ItemGroup />
   <Import Project="$(MSBuildToolsPath)\Microsoft.CSharp.targets" />
   <PropertyGroup>
Index: branches/RoutePlanning/HeuristicLab.Problems.RoutePlanning/3.3/Osm/Way.cs
===================================================================
--- branches/RoutePlanning/HeuristicLab.Problems.RoutePlanning/3.3/Osm/Way.cs	(revision 8369)
+++ branches/RoutePlanning/HeuristicLab.Problems.RoutePlanning/3.3/Osm/Way.cs	(revision 8408)
@@ -94,4 +94,62 @@
 
     //public bool CanBeTraversed(Vehicle) {}
+
+    public int GetMaxSpeed() {
+      int speed = 0;
+
+      //TODO: 
+      HighwayType highwayType = HighwayTag;
+      switch (highwayType) {
+        case HighwayType.bridleway:
+        case HighwayType.bus_guideway:
+        case HighwayType.raceway:
+        case HighwayType.cycleway:
+        case HighwayType.footway:
+        case HighwayType.steps:
+        case HighwayType.null_type:
+          speed = 1;
+          break;
+
+        case HighwayType.path:
+        case HighwayType.service:
+        case HighwayType.pedestrian:
+        case HighwayType.living_street:
+          speed = 15;
+          break;
+
+        case HighwayType.road:
+        case HighwayType.track:
+          speed = 30;
+          break;
+
+        case HighwayType.tertiary:
+        case HighwayType.tertiary_link:
+        case HighwayType.secondary:
+        case HighwayType.secondary_link:
+          speed = 80;
+          break;
+
+        case HighwayType.unclassified:
+        case HighwayType.residential:
+          speed = 50;
+          break;
+
+        case HighwayType.trunk:
+        case HighwayType.trunk_link:
+        case HighwayType.primary:
+        case HighwayType.primary_link:
+          speed = 100;
+          break;
+        case HighwayType.motorway:
+        case HighwayType.motorway_link:
+          speed = 130;
+          break;
+
+        default:
+          speed = 1;
+          break;
+      }
+      return speed;
+    }
   }
 }
