Index: /branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/AStarAlgorithm.cs
===================================================================
--- /branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/AStarAlgorithm.cs	(revision 8349)
+++ /branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/AStarAlgorithm.cs	(revision 8350)
@@ -4,5 +4,5 @@
 using HeuristicLab.Problems.RoutePlanning.Graph;
 namespace HeuristicLab.Algorithms.GraphRouting {
-  class AStarAlgorithm : IRouter {
+  public class AStarAlgorithm : IRouter {
     private Graph graph;
 
@@ -10,4 +10,5 @@
     private Dictionary<long, long> predecessors;
     private List<float> closedList = new List<float>();
+    //private PriorityQueueOld<float, long> openList;
     private PriorityQueue<float, long> openList;
 
@@ -22,13 +23,16 @@
       predecessors = new Dictionary<long, long>();
       closedList = new List<float>();
-      openList = new PriorityQueue<float, long>();
+      //openList = new PriorityQueueOld<float, long>();
+      openList = new PriorityQueue<float, long>(new MyComparer());
 
       long currentNode = sourceNodeId;
 
-      openList.Enqueue(currentNode, 0);
+      //openList.Enqueue(currentNode, 0);
+      openList.Enqueue(0, currentNode);
       distances.Add(currentNode, 0);
 
       while (openList.Count > 0) {
-        currentNode = openList.Dequeue();
+        //currentNode = openList.Dequeue();
+        currentNode = openList.DequeueValue();
         if (currentNode == targetNodeId) {
           return ReconstructPath(currentNode).ToArray();
@@ -52,5 +56,6 @@
             openList.Remove(neighbor.Key);
           }
-          openList.Enqueue(neighbor.Key, f);
+          //openList.Enqueue(neighbor.Key, f);
+          openList.Enqueue(f, neighbor.Key);
         }
       }
@@ -85,3 +90,12 @@
     }
   }
+
+  class MyComparer : IComparer<float> {
+    public int Compare(float item1, float item2) {
+      return item1 > item2 ? 1 : item1 < item2 ? -1 : 0;
+
+      // inverted comparison
+      //return item1 < item2 ? 1 : item1 > item2 ? -1 : 0;
+    }
+  }
 }
Index: /branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/HeuristicLab.Algorithms.GraphRouting.csproj
===================================================================
--- /branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/HeuristicLab.Algorithms.GraphRouting.csproj	(revision 8349)
+++ /branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/HeuristicLab.Algorithms.GraphRouting.csproj	(revision 8350)
@@ -64,4 +64,5 @@
     <Compile Include="Plugin.cs" />
     <Compile Include="PriorityQueue.cs" />
+    <Compile Include="PriorityQueueOld.cs" />
     <Compile Include="Properties\AssemblyInfo.cs" />
   </ItemGroup>
Index: /branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/PriorityQueue.cs
===================================================================
--- /branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/PriorityQueue.cs	(revision 8349)
+++ /branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/PriorityQueue.cs	(revision 8350)
@@ -1,48 +1,181 @@
 ﻿
 using System;
+using System.Collections;
 using System.Collections.Generic;
 namespace HeuristicLab.Algorithms.GraphRouting {
-  public class PriorityQueue<P, T> where P : IComparable {
-    SortedList<Pair<P>, T> list;
-    P dummy;
+  public class PriorityQueue<TPriority, TValue> : ICollection<KeyValuePair<TPriority, TValue>> {
+    private List<KeyValuePair<TPriority, TValue>> heap;
+    private Dictionary<TValue, KeyValuePair<TPriority, TValue>> values;
+    private IComparer<TPriority> comparer;
 
-    public PriorityQueue() {
-      list = new SortedList<Pair<P>, T>(new PairComparer<P>());
+    #region Constructors
+
+    public PriorityQueue(IComparer<TPriority> comparer) {
+      if (comparer == null)
+        throw new ArgumentNullException();
+
+      heap = new List<KeyValuePair<TPriority, TValue>>();
+      values = new Dictionary<TValue, KeyValuePair<TPriority, TValue>>();
+      this.comparer = comparer;
+    }
+
+    #endregion
+
+    #region Priority queue methods
+
+    public void Enqueue(TPriority priority, TValue value) {
+      Insert(priority, value);
+    }
+
+    public KeyValuePair<TPriority, TValue> Dequeue() {
+      if (Count != 0) {
+        KeyValuePair<TPriority, TValue> item = heap[0];
+        RemoveRoot();
+        return item;
+      } else {
+        throw new InvalidOperationException("Priority queue is empty");
+      }
+    }
+
+    public TValue DequeueValue() {
+      return Dequeue().Value;
+    }
+
+    #endregion
+
+    #region ICollection members
+
+    public void Add(KeyValuePair<TPriority, TValue> item) {
+      Enqueue(item.Key, item.Value);
+    }
+
+    public void Clear() {
+      heap.Clear();
+      values.Clear();
+    }
+
+    public bool Contains(KeyValuePair<TPriority, TValue> item) {
+      return heap.Contains(item);
+    }
+
+    public bool Contains(TValue value) {
+      return values.ContainsKey(value);
+    }
+
+    public void CopyTo(KeyValuePair<TPriority, TValue>[] array, int arrayIndex) {
+      heap.CopyTo(array, arrayIndex);
     }
 
     public int Count {
-      get { return list.Count; }
+      get { return heap.Count; }
     }
 
-    public void Enqueue(T item, P priority) {
-      list.Add(new Pair<P>(priority, dummy), item);
-      //count++;
+    public bool IsReadOnly {
+      get { return false; }
     }
 
-    public T Dequeue() {
-      T item = list[list.Keys[0]];
-      list.RemoveAt(0);
-      return item;
+    public bool Remove(KeyValuePair<TPriority, TValue> item) {
+      int idx = heap.IndexOf(item);
+      if (idx < 0) {
+        return false;
+      }
+
+      values.Remove(item.Value);
+
+      heap[idx] = heap[heap.Count - 1];
+      heap.RemoveAt(heap.Count - 1);
+
+      HeapUp(idx);
+      HeapDown(idx);
+
+      return true;
     }
 
-    public bool Contains(T item) {
-      return list.ContainsValue(item);
+    public bool Remove(TValue value) {
+      KeyValuePair<TPriority, TValue> item = values[value];
+      return Remove(item);
     }
 
-    public bool Remove(T item) {
-      bool delete = false;
-      KeyValuePair<Pair<P>, T> toDelete = new KeyValuePair<Pair<P>, T>();
-      foreach (var pair in list) {
-        if (pair.Value.Equals(item)) {
-          delete = true;
-          toDelete = pair;
+    #endregion
+
+    #region IEnumerable Members
+
+    public IEnumerator<KeyValuePair<TPriority, TValue>> GetEnumerator() {
+      return heap.GetEnumerator();
+    }
+
+    IEnumerator IEnumerable.GetEnumerator() {
+      return this.GetEnumerator();
+    }
+
+    #endregion
+
+    #region Private methods
+
+    private void SwapElements(int pos1, int pos2) {
+      KeyValuePair<TPriority, TValue> element = heap[pos1];
+      heap[pos1] = heap[pos2];
+      heap[pos2] = element;
+    }
+
+    private void Insert(TPriority priority, TValue value) {
+      KeyValuePair<TPriority, TValue> element = new KeyValuePair<TPriority, TValue>(priority, value);
+      heap.Add(element);
+      values.Add(value, element);
+
+      HeapUp(heap.Count - 1);
+    }
+
+    private void RemoveRoot() {
+      if (heap.Count <= 1) {
+        heap.Clear();
+        values.Clear();
+      } else {
+        KeyValuePair<TPriority, TValue> item = heap[0];
+        values.Remove(item.Value);
+
+        heap[0] = heap[heap.Count - 1];
+        heap.RemoveAt(heap.Count - 1);
+
+        HeapDown(0);
+      }
+    }
+
+    private void HeapUp(int pos) {
+      if (pos < heap.Count) {
+        while (pos > 0) {
+          int parent = (pos - 1) / 2;
+          if (comparer.Compare(heap[parent].Key, heap[pos].Key) > 0) {
+            SwapElements(parent, pos);
+            pos = parent;
+          } else {
+            break;
+          }
         }
       }
-      if (delete) {
-        return list.Remove(toDelete.Key);
-      }
-      return false;
     }
 
+    private void HeapDown(int pos) {
+      while (true) {
+        int smallest = pos;
+        int left = 2 * pos + 1;
+        int right = 2 * pos + 2;
+        if (left < heap.Count && comparer.Compare(heap[smallest].Key, heap[left].Key) > 0) {
+          smallest = left;
+        }
+        if (right < heap.Count && comparer.Compare(heap[smallest].Key, heap[right].Key) > 0) {
+          smallest = right;
+        }
+
+        if (smallest != pos) {
+          SwapElements(smallest, pos);
+          pos = smallest;
+        } else {
+          break;
+        }
+      }
+    }
+
+    #endregion
   }
 }
Index: /branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/PriorityQueueOld.cs
===================================================================
--- /branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/PriorityQueueOld.cs	(revision 8350)
+++ /branches/RoutePlanning/HeuristicLab.Algorithms.GraphRouting/3.3/PriorityQueueOld.cs	(revision 8350)
@@ -0,0 +1,48 @@
+﻿
+using System;
+using System.Collections.Generic;
+namespace HeuristicLab.Algorithms.GraphRouting {
+  public class PriorityQueueOld<P, T> where P : IComparable {
+    SortedList<Pair<P>, T> list;
+    P dummy;
+
+    public PriorityQueueOld() {
+      list = new SortedList<Pair<P>, T>(new PairComparer<P>());
+    }
+
+    public int Count {
+      get { return list.Count; }
+    }
+
+    public void Enqueue(T item, P priority) {
+      list.Add(new Pair<P>(priority, dummy), item);
+      //count++;
+    }
+
+    public T Dequeue() {
+      T item = list[list.Keys[0]];
+      list.RemoveAt(0);
+      return item;
+    }
+
+    public bool Contains(T item) {
+      return list.ContainsValue(item);
+    }
+
+    public bool Remove(T item) {
+      bool delete = false;
+      KeyValuePair<Pair<P>, T> toDelete = new KeyValuePair<Pair<P>, T>();
+      foreach (var pair in list) {
+        if (pair.Value.Equals(item)) {
+          delete = true;
+          toDelete = pair;
+        }
+      }
+      if (delete) {
+        return list.Remove(toDelete.Key);
+      }
+      return false;
+    }
+
+  }
+}
Index: /branches/RoutePlanning/HeuristicLab.Problems.RoutePlanning.Test/HeuristicLab.Problems.RoutePlanning.Test.csproj
===================================================================
--- /branches/RoutePlanning/HeuristicLab.Problems.RoutePlanning.Test/HeuristicLab.Problems.RoutePlanning.Test.csproj	(revision 8349)
+++ /branches/RoutePlanning/HeuristicLab.Problems.RoutePlanning.Test/HeuristicLab.Problems.RoutePlanning.Test.csproj	(revision 8350)
@@ -16,5 +16,5 @@
   </PropertyGroup>
   <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|x86' ">
-    <PlatformTarget>x86</PlatformTarget>
+    <PlatformTarget>AnyCPU</PlatformTarget>
     <DebugSymbols>true</DebugSymbols>
     <DebugType>full</DebugType>
@@ -33,4 +33,36 @@
     <ErrorReport>prompt</ErrorReport>
     <WarningLevel>4</WarningLevel>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)' == 'Debug|AnyCPU'">
+    <DebugSymbols>true</DebugSymbols>
+    <OutputPath>bin\Debug\</OutputPath>
+    <DefineConstants>DEBUG;TRACE</DefineConstants>
+    <DebugType>full</DebugType>
+    <PlatformTarget>AnyCPU</PlatformTarget>
+    <CodeAnalysisLogFile>bin\Debug\HeuristicLab.Problems.RoutePlanning.Test.exe.CodeAnalysisLog.xml</CodeAnalysisLogFile>
+    <CodeAnalysisUseTypeNameInSuppression>true</CodeAnalysisUseTypeNameInSuppression>
+    <CodeAnalysisModuleSuppressionsFile>GlobalSuppressions.cs</CodeAnalysisModuleSuppressionsFile>
+    <ErrorReport>prompt</ErrorReport>
+    <CodeAnalysisRuleSet>MinimumRecommendedRules.ruleset</CodeAnalysisRuleSet>
+    <CodeAnalysisRuleSetDirectories>;C:\Program Files (x86)\Microsoft Visual Studio 10.0\Team Tools\Static Analysis Tools\\Rule Sets</CodeAnalysisRuleSetDirectories>
+    <CodeAnalysisIgnoreBuiltInRuleSets>true</CodeAnalysisIgnoreBuiltInRuleSets>
+    <CodeAnalysisRuleDirectories>;C:\Program Files (x86)\Microsoft Visual Studio 10.0\Team Tools\Static Analysis Tools\FxCop\\Rules</CodeAnalysisRuleDirectories>
+    <CodeAnalysisIgnoreBuiltInRules>true</CodeAnalysisIgnoreBuiltInRules>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)' == 'Release|AnyCPU'">
+    <OutputPath>bin\Release\</OutputPath>
+    <DefineConstants>TRACE</DefineConstants>
+    <Optimize>true</Optimize>
+    <DebugType>pdbonly</DebugType>
+    <PlatformTarget>AnyCPU</PlatformTarget>
+    <CodeAnalysisLogFile>bin\Release\HeuristicLab.Problems.RoutePlanning.Test.exe.CodeAnalysisLog.xml</CodeAnalysisLogFile>
+    <CodeAnalysisUseTypeNameInSuppression>true</CodeAnalysisUseTypeNameInSuppression>
+    <CodeAnalysisModuleSuppressionsFile>GlobalSuppressions.cs</CodeAnalysisModuleSuppressionsFile>
+    <ErrorReport>prompt</ErrorReport>
+    <CodeAnalysisRuleSet>MinimumRecommendedRules.ruleset</CodeAnalysisRuleSet>
+    <CodeAnalysisRuleSetDirectories>;C:\Program Files (x86)\Microsoft Visual Studio 10.0\Team Tools\Static Analysis Tools\\Rule Sets</CodeAnalysisRuleSetDirectories>
+    <CodeAnalysisIgnoreBuiltInRuleSets>true</CodeAnalysisIgnoreBuiltInRuleSets>
+    <CodeAnalysisRuleDirectories>;C:\Program Files (x86)\Microsoft Visual Studio 10.0\Team Tools\Static Analysis Tools\FxCop\\Rules</CodeAnalysisRuleDirectories>
+    <CodeAnalysisIgnoreBuiltInRules>true</CodeAnalysisIgnoreBuiltInRules>
   </PropertyGroup>
   <ItemGroup>
Index: /branches/RoutePlanning/HeuristicLab.Problems.RoutePlanning.Test/Program.cs
===================================================================
--- /branches/RoutePlanning/HeuristicLab.Problems.RoutePlanning.Test/Program.cs	(revision 8349)
+++ /branches/RoutePlanning/HeuristicLab.Problems.RoutePlanning.Test/Program.cs	(revision 8350)
@@ -37,5 +37,6 @@
       Console.WriteLine();
       Graph.Graph graph = new Graph.Graph(xmlDs);
-      IRouter router = new DijkstraAlgorithm(graph);
+      //IRouter router = new DijkstraAlgorithm(graph);
+      IRouter router = new AStarAlgorithm(graph);
       Console.Write("Calculate route ... ");
       sw = Stopwatch.StartNew();
Index: /branches/RoutePlanning/RoutePlanning.sln
===================================================================
--- /branches/RoutePlanning/RoutePlanning.sln	(revision 8349)
+++ /branches/RoutePlanning/RoutePlanning.sln	(revision 8350)
@@ -40,5 +40,6 @@
 		{0C6B9F99-DBC1-49BE-B90A-36CB86DD1D0C}.Release|Mixed Platforms.Build.0 = Release|Any CPU
 		{0C6B9F99-DBC1-49BE-B90A-36CB86DD1D0C}.Release|x86.ActiveCfg = Release|Any CPU
-		{004241D1-2127-4489-9DFB-9F7707983371}.Debug|Any CPU.ActiveCfg = Debug|x86
+		{004241D1-2127-4489-9DFB-9F7707983371}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
+		{004241D1-2127-4489-9DFB-9F7707983371}.Debug|Any CPU.Build.0 = Debug|Any CPU
 		{004241D1-2127-4489-9DFB-9F7707983371}.Debug|Mixed Platforms.ActiveCfg = Debug|x86
 		{004241D1-2127-4489-9DFB-9F7707983371}.Debug|Mixed Platforms.Build.0 = Debug|x86
