Index: /branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/AStarAlgorithmV2.cs
===================================================================
--- /branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/AStarAlgorithmV2.cs	(revision 8480)
+++ /branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/AStarAlgorithmV2.cs	(revision 8481)
@@ -99,22 +99,22 @@
       return route;
     }
-  }
 
-  class NodeData : IComparable<NodeData> {
-    public long Node { get; set; }
-    public float Costs { get; set; }
+    private class NodeData : IComparable<NodeData> {
+      public long Node { get; set; }
+      public float Costs { get; set; }
 
-    public NodeData(long nodeId, float f) {
-      Node = nodeId;
-      Costs = f;
+      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
     }
-
-    #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/AStarAlgorithmV3.cs
===================================================================
--- /branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/AStarAlgorithmV3.cs	(revision 8481)
+++ /branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/AStarAlgorithmV3.cs	(revision 8481)
@@ -0,0 +1,120 @@
+﻿
+using System;
+using System.Collections.Generic;
+using HeuristicLab.Problems.RoutePlanning.Graph;
+namespace HeuristicLab.Algorithms.GraphRouting {
+  public class AStarAlgorithmV3 : IRouter {
+    private IGraph 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 AStarAlgorithmV3(IGraph 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.Min;
+        openList.Remove(data);
+        openListLookup.Remove(data.Node);
+        currentNode = data.Node;
+        if (currentNode == targetNodeId) {
+          return ReconstructPath(currentNode).ToArray();
+        }
+
+        Dictionary<long, float> currentNeighbors = graph.GetNeighborsWithWeight(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;
+    }
+
+    private 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/BidirectionalDijkstraAlgorithmV2.cs
===================================================================
--- /branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/BidirectionalDijkstraAlgorithmV2.cs	(revision 8481)
+++ /branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/BidirectionalDijkstraAlgorithmV2.cs	(revision 8481)
@@ -0,0 +1,149 @@
+﻿
+using System;
+using System.Collections.Generic;
+using HeuristicLab.Problems.RoutePlanning.Graph;
+namespace HeuristicLab.Algorithms.GraphRouting {
+  public class BidrectionalDijkstraAlgorithmV2 : IRouter {
+    private IGraph graph;
+
+    private HashSet<long> visitedNodesForward;
+    private HashSet<long> visitedNodesBackward;
+    private HashSet<long> unvisitedNodesForward;
+    private HashSet<long> unvisitedNodesBackward;
+    private Dictionary<long, float> distancesForward;
+    private Dictionary<long, long> predecessorsForward;
+    private Dictionary<long, float> distancesBackward;
+    private Dictionary<long, long> predecessorsBackward;
+
+    public BidrectionalDijkstraAlgorithmV2(IGraph graph) {
+      this.graph = graph;
+    }
+
+    #region IRouter Members
+
+    public long[] Calculate(long sourceNodeId, long targetNodeId) {
+      visitedNodesForward = new HashSet<long>();
+      visitedNodesBackward = new HashSet<long>();
+
+      unvisitedNodesForward = new HashSet<long>();
+      unvisitedNodesBackward = new HashSet<long>();
+
+      distancesForward = new Dictionary<long, float>();
+      predecessorsForward = new Dictionary<long, long>();
+
+      distancesBackward = new Dictionary<long, float>();
+      predecessorsBackward = new Dictionary<long, long>();
+
+      List<long> resultRouteForward = new List<long>();
+      List<long> resultRouteBackward = new List<long>();
+
+      if (sourceNodeId == targetNodeId) {
+        resultRouteForward.Add(sourceNodeId);
+        return resultRouteForward.ToArray();
+      }
+
+      long currentNodeForward = sourceNodeId;
+      distancesForward.Add(currentNodeForward, 0);
+      unvisitedNodesForward.Add(currentNodeForward);
+
+      long currentNodeBackward = targetNodeId;
+      distancesBackward.Add(currentNodeBackward, 0);
+      unvisitedNodesBackward.Add(currentNodeBackward);
+
+      while (unvisitedNodesForward.Count > 0 && unvisitedNodesBackward.Count > 0) {
+        currentNodeForward = GetMinimumDistance(distancesForward, unvisitedNodesForward);
+        visitedNodesForward.Add(currentNodeForward);
+        unvisitedNodesForward.Remove(currentNodeForward);
+
+        currentNodeBackward = GetMinimumDistance(distancesBackward, unvisitedNodesBackward);
+        visitedNodesBackward.Add(currentNodeBackward);
+        unvisitedNodesBackward.Remove(currentNodeBackward);
+
+        if (visitedNodesForward.Contains(currentNodeBackward)) {
+          return GetRoute(currentNodeBackward).ToArray();
+        }
+
+        if (visitedNodesBackward.Contains(currentNodeForward)) {
+          return GetRoute(currentNodeForward).ToArray();
+        }
+
+        FindMinimalWeightForward(currentNodeForward);
+        FindMinimalWeightBackward(currentNodeBackward);
+      }
+
+      throw new Exception(string.Format("No route found from {0} to {1}", sourceNodeId, targetNodeId));
+    }
+
+    #endregion
+
+    private long GetMinimumDistance(Dictionary<long, float> distances, HashSet<long> nodeIds) {
+      long minimum = -1;
+      foreach (long id in nodeIds) {
+        if (minimum == -1) minimum = id;
+        else {
+          if (GetDistance(distances, id) < GetDistance(distances, minimum)) {
+            minimum = id;
+          }
+        }
+      }
+      return minimum;
+    }
+
+    private float GetDistance(Dictionary<long, float> distances, long id) {
+      float d;
+      if (distances.TryGetValue(id, out d)) {
+        return d;
+      } else { return float.MaxValue; }
+    }
+
+    private void FindMinimalWeightForward(long nodeId) {
+      Dictionary<long, float> currentNeighbors = graph.GetNeighborsWithWeight(nodeId);
+      foreach (KeyValuePair<long, float> neighbor in currentNeighbors) {
+        // update relax
+        float totalWeight = GetDistance(distancesForward, nodeId) + neighbor.Value;
+        if (GetDistance(distancesForward, neighbor.Key) > totalWeight) {
+          distancesForward[neighbor.Key] = totalWeight;
+          predecessorsForward[neighbor.Key] = nodeId;
+          unvisitedNodesForward.Add(neighbor.Key);
+        }
+      }
+    }
+
+    private void FindMinimalWeightBackward(long nodeId) {
+      Dictionary<long, float> currentNeighbors = graph.GetNeighborsWithWeightReversed(nodeId);
+      foreach (KeyValuePair<long, float> neighbor in currentNeighbors) {
+        // update relax
+        float totalWeight = GetDistance(distancesBackward, nodeId) + neighbor.Value;
+        if (GetDistance(distancesBackward, neighbor.Key) > totalWeight) {
+          distancesBackward[neighbor.Key] = totalWeight;
+          predecessorsBackward[neighbor.Key] = nodeId;
+          unvisitedNodesBackward.Add(neighbor.Key);
+        }
+      }
+    }
+
+    private List<long> GetRoute(long nodeId) {
+      List<long> route = new List<long>();
+      long current = nodeId;
+      long next;
+
+      if (!predecessorsForward.TryGetValue(current, out next)) {
+        return null;
+      }
+      route.Add(current);
+      while (predecessorsForward.TryGetValue(current, out next)) {
+        current = predecessorsForward[current];
+        route.Add(current);
+      }
+      route.Reverse();
+
+      current = nodeId;
+      while (predecessorsBackward.TryGetValue(current, out next)) {
+        current = predecessorsBackward[current];
+        route.Add(current);
+      }
+
+      return route;
+    }
+  }
+}
Index: /branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/DijkstraAlgorithmV2.cs
===================================================================
--- /branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/DijkstraAlgorithmV2.cs	(revision 8480)
+++ /branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/DijkstraAlgorithmV2.cs	(revision 8481)
@@ -76,4 +76,7 @@
 
     private void FindMinimalWeight(long nodeId) {
+      //if (nodeId == 274265932) {
+      //  Console.WriteLine();
+      //}
       Dictionary<long, float> currentNeighbors = graph.GetNeighborsWithWeight(nodeId);
       foreach (KeyValuePair<long, float> neighbor in currentNeighbors) {
Index: /branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/HeuristicLab.Algorithms.GraphRouting.csproj
===================================================================
--- /branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/HeuristicLab.Algorithms.GraphRouting.csproj	(revision 8480)
+++ /branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/HeuristicLab.Algorithms.GraphRouting.csproj	(revision 8481)
@@ -60,4 +60,6 @@
     <Compile Include="AStarAlgorithmV2.cs" />
     <Compile Include="BidirectionalDijkstraAlgorithm.cs" />
+    <Compile Include="AStarAlgorithmV3.cs" />
+    <Compile Include="BidirectionalDijkstraAlgorithmV2.cs" />
     <Compile Include="DijkstraAlgorithmV2.cs" />
     <Compile Include="DijkstraAlgorithm.cs" />
Index: /branches/RoutePlanning/HeuristicLab.Problems.RoutePlanning.Test/Program.cs
===================================================================
--- /branches/RoutePlanning/HeuristicLab.Problems.RoutePlanning.Test/Program.cs	(revision 8480)
+++ /branches/RoutePlanning/HeuristicLab.Problems.RoutePlanning.Test/Program.cs	(revision 8481)
@@ -5,4 +5,5 @@
 using System.Xml;
 using HeuristicLab.Algorithms.GraphRouting;
+using HeuristicLab.Problems.RoutePlanning.DIMACS;
 using HeuristicLab.Problems.RoutePlanning.Graph;
 using HeuristicLab.Problems.RoutePlanning.Osm;
@@ -23,16 +24,25 @@
       string file7 = @"C:\dev\osmfiles\austria.highway.osm";
 
-      IDataSource ds = TestLoad(file5);
-      OsmGraph graph = new OsmGraph(ds);
-      IGraph graphNew = TestGetRoutingGraph(ds);
-      TestRouter(new DijkstraAlgorithm(graph), 529102170, 1001363194, true);
-      TestRouter(new DijkstraAlgorithmV2(graphNew), 529102170, 1001363194, true);
+      string fileNYArcs = @"C:\dev\DIMACSfiles\USA-road-t.NY.gr.gz";
+      string fileNYCoords = @"C:\dev\DIMACSfiles\USA-road-d.NY.co.gz";
+
+      //IDataSource ds = TestLoad(file5);
+      //OsmGraph graph = new OsmGraph(ds);
+      //IGraph graphNew = TestGetRoutingGraph(ds);
+
+      IDataSource ds = new DIMACSDataSource(fileNYCoords, fileNYArcs);
+
+      //TestRouter(new DijkstraAlgorithm(graph), 529102170, 1001363194, true);
+      //TestRouter(new DijkstraAlgorithmV2(graphNew), 529102170, 1001363194, true);
       //TestRouter(new DijkstraAlgorithm(graph), 529102170, 31372732, false); // inz - hgb
       //TestRouter(new DijkstraAlgorithmV2(graphNew), 529102170, 31372732, false); // inz - hgb
+      //TestRouter(new BidrectionalDijkstraAlgorithm(graph), 529102170, 31372732, false); // inz - hgb
+      //TestRouter(new BidrectionalDijkstraAlgorithmV2(graphNew), 529102170, 31372732, false); // inz - hgb
       //TestRouter(new AStarAlgorithm(graph), 529102170, 31372732, false); // inz - hgb
       //TestRouter(new AStarAlgorithmV2(graph), 529102170, 31372732, false); // inz - hgb
-      //TestRouter(new BidrectionalDijkstraAlgorithm(graph), 529102170, 31372732, false); // inz - hgb
+      //TestRouter(new AStarAlgorithmV3(graphNew), 529102170, 31372732, false); // inz - hgb
 
       //TestRouter(new AStarAlgorithmV2(graph), 346151602, 33196510, false); // bregenz (bahnhofstr) - wien (stepansplatz)
+      //TestRouter(new AStarAlgorithmV3(graphNew), 346151602, 33196510, false); // bregenz (bahnhofstr) - wien (stepansplatz)
 
       //TestLoadAndRouter(file6, typeof(DijkstraAlgorithm), 529102170, 31372732, false, false); // inz - hgb
@@ -50,7 +60,7 @@
       //TestLoadAndRouter(file6, typeof(BidrectionalDijkstraAlgorithm), 529102170, 31372732, false, true); // inz - hgb
 
-
-
-
+      //TestLoadAndRouterNew(file6, typeof(DijkstraAlgorithmV2), 529102170, 31372732, false, true);
+      //TestLoadAndRouterNew(file6, typeof(BidrectionalDijkstraAlgorithmV2), 529102170, 31372732, false, true);
+      //TestLoadAndRouterNew(file6, typeof(AStarAlgorithmV3), 529102170, 31372732, false, true);
 
       System.Console.Read();
@@ -129,4 +139,42 @@
         Console.WriteLine("Result file: {0}", resultFile);
         WriteGPXFile(graph, result, resultFile);
+      }
+      Console.WriteLine("===================================================");
+      Console.WriteLine();
+    }
+
+    private static void TestLoadAndRouterNew(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();
+      Console.Write("Loading data ... ");
+      var sw = Stopwatch.StartNew();
+      XmlDataSource xmlDs = new XmlDataSource(filepath);
+      sw.Stop();
+      Console.WriteLine("done.");
+      Console.WriteLine("Loading Time: {0}", sw.Elapsed);
+      Console.WriteLine();
+      IGraph graph = xmlDs.GetRoutingGraph();
+      IRouter router = (IRouter)Activator.CreateInstance(routerType, graph);
+      Console.Write("Calculate route ... ");
+      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();
+      }
+      if (writeResultFile) {
+        string dt = DateTime.Now.ToString("yyyyMMddhhmmss_");
+        string fn = Path.GetFileNameWithoutExtension(file.Name);
+        string resultFile = dt + fn + "_" + sourceNodeId + "-" + targetNodeId + ".gpx";
+        Console.WriteLine("Result file: {0}", resultFile);
+        WriteGPXFileNew(graph, result, resultFile);
       }
       Console.WriteLine("===================================================");
@@ -180,4 +228,50 @@
     }
 
+    private static void WriteGPXFileNew(IGraph graph, long[] route, string file) {
+      XmlWriterSettings settings = new XmlWriterSettings();
+      settings.Indent = true;
+      settings.IndentChars = ("   ");
+
+      using (XmlWriter writer = XmlWriter.Create(file, settings)) {
+        writer.WriteStartElement("gpx", "http://www.topografix.com/GPX/1/0");
+        writer.WriteAttributeString("version", "1.1");
+        writer.WriteAttributeString("creator", "cloudia");
+
+        foreach (long nodeId in route) {
+          Vertex node = graph.GetVertex(nodeId);
+          writer.WriteStartElement("wpt");
+          writer.WriteAttributeString("lat", XmlConvert.ToString(node.Latitude));
+          writer.WriteAttributeString("lon", XmlConvert.ToString(node.Logitude));
+          writer.WriteEndElement(); // wpt
+        }
+
+        //writer.WriteStartElement("rte");
+        //foreach (long nodeId in route) {
+        //  Vertex<Node> node = graph.GetVertex(nodeId);
+        //  writer.WriteStartElement("rtept");
+        //  writer.WriteAttributeString("lat", XmlConvert.ToString(node.Node.Latitude));
+        //  writer.WriteAttributeString("lon", XmlConvert.ToString(node.Node.Longitude));
+        //  writer.WriteEndElement(); // rtept
+        //}
+        //writer.WriteEndElement(); // rte
+
+        //writer.WriteStartElement("trk");
+        //writer.WriteElementString("name", "RoutePath");
+        ////writer.WriteEndElement(); // name
+        //writer.WriteStartElement("trkseq");
+        //foreach (long nodeId in route) {
+        //  Vertex<Node> node = graph.GetVertex(nodeId);
+        //  writer.WriteStartElement("trkpt");
+        //  writer.WriteAttributeString("lat", XmlConvert.ToString(node.Node.Latitude));
+        //  writer.WriteAttributeString("lon", XmlConvert.ToString(node.Node.Longitude));
+        //  writer.WriteEndElement(); // trkpt
+        //}
+        //writer.WriteEndElement(); // trkseq
+        //writer.WriteEndElement(); // trk
+
+        writer.WriteEndElement(); // gpx
+      }
+    }
+
     private static IGraph TestGetRoutingGraph(IDataSource ds) {
       Console.WriteLine("Test GetRoutingGraph BEGIN ------------------------");
