diff --git a/Modules/GraphAlgorithms/CMakeLists.txt b/Modules/GraphAlgorithms/CMakeLists.txt
new file mode 100644
index 0000000000..43896afc7e
--- /dev/null
+++ b/Modules/GraphAlgorithms/CMakeLists.txt
@@ -0,0 +1,4 @@
+# CREATE MODULE HERE
+MITK_CREATE_MODULE(MitkGraphAlgorithms
+
+)
diff --git a/Modules/GraphAlgorithms/files.cmake b/Modules/GraphAlgorithms/files.cmake
new file mode 100644
index 0000000000..c4c60e7126
--- /dev/null
+++ b/Modules/GraphAlgorithms/files.cmake
@@ -0,0 +1,11 @@
+SET(CPP_FILES
+  itkShortestPathNode.cpp  
+)
+
+SET(H_FILES
+  itkShortestPathCostFunction.h  
+  itkShortestPathCostFunctionTbss.h
+  itkShortestPathNode.h
+  itkShortestPathImageFilter.h
+  
+)
diff --git a/Modules/GraphAlgorithms/itkShortestPathCostFunction.h b/Modules/GraphAlgorithms/itkShortestPathCostFunction.h
new file mode 100644
index 0000000000..a16d260bee
--- /dev/null
+++ b/Modules/GraphAlgorithms/itkShortestPathCostFunction.h
@@ -0,0 +1,85 @@
+#ifndef __itkShortestPathCostFunction_h
+#define __itkShortestPathCostFunction_h
+
+#include "itkObject.h"
+#include "itkObjectFactory.h"
+#include "itkShapedNeighborhoodIterator.h"
+#include <itkImageRegionConstIterator.h>
+
+#include <MitkGraphAlgorithmsExports.h>
+
+
+namespace itk
+{
+
+  // \brief this is a pure virtual superclass for all cost function for the itkShortestPathImageFilter
+  template <class TInputImageType>            
+  class ITK_EXPORT ShortestPathCostFunction : public Object 
+  {
+  public:
+
+    /** Standard class typedefs. */
+    typedef ShortestPathCostFunction Self;
+    typedef Object  Superclass;
+    typedef SmartPointer<Self>   Pointer;
+    typedef SmartPointer<const Self>  ConstPointer;
+    typedef ShapedNeighborhoodIterator< TInputImageType > itkShapedNeighborhoodIteratorType;
+
+    /** Run-time type information (and related methods). */
+    itkTypeMacro(ShortestPathCostFunction, Object);
+
+    /** Type definition for the input image. */
+    typedef TInputImageType  ImageType;
+
+    // More typdefs for convenience
+    typedef typename TInputImageType::Pointer               ImagePointer;
+    typedef typename TInputImageType::ConstPointer          ImageConstPointer;
+
+
+    typedef typename TInputImageType::PixelType             PixelType;    
+
+    typedef typename TInputImageType::IndexType             IndexType;
+
+    /** Set the input image. */
+    itkSetConstObjectMacro(Image,TInputImageType);
+
+    // \brief calculates the costs for going from pixel1 to pixel2
+    virtual double GetCost( IndexType p1, IndexType p2) = 0;
+
+    // \brief returns the minimal costs possible (needed for A*)
+    virtual double GetMinCost() = 0;
+
+    // \brief Initialize the metric
+    virtual void Initialize () = 0;
+
+    // \brief Set Starpoint for Path
+    void SetStartIndex (const IndexType & StartIndex);
+
+    // \brief Set Endpoint for Path
+    void SetEndIndex(const IndexType & EndIndex);
+
+
+    ShortestPathCostFunction();
+
+  protected:
+
+    virtual ~ShortestPathCostFunction() {};
+    void PrintSelf(std::ostream& os, Indent indent) const;
+    ImageConstPointer    m_Image;
+    IndexType m_StartIndex, m_EndIndex; 
+
+  private:
+
+    ShortestPathCostFunction(const Self&); //purposely not implemented
+    void operator=(const Self&); //purposely not implemented
+
+  };
+
+} // end namespace itk
+
+
+#ifndef ITK_MANUAL_INSTANTIATION
+#include "itkShortestPathCostFunction.txx"
+#endif
+
+#endif /* __itkShortestPathCostFunction_h */
diff --git a/Modules/GraphAlgorithms/itkShortestPathCostFunction.txx b/Modules/GraphAlgorithms/itkShortestPathCostFunction.txx
new file mode 100644
index 0000000000..f3e9896bd9
--- /dev/null
+++ b/Modules/GraphAlgorithms/itkShortestPathCostFunction.txx
@@ -0,0 +1,51 @@
+#ifndef __itkShortestPathCostFunction_txx
+#define __itkShortestPathCostFunction_txx
+
+#include "itkShortestPathCostFunction.h"
+
+
+namespace itk
+{ 
+
+  // Constructor
+  template<class TInputImageType>
+  ShortestPathCostFunction<TInputImageType>
+    ::ShortestPathCostFunction()
+  {
+  }
+
+
+  template<class TInputImageType>
+  void
+    ShortestPathCostFunction<TInputImageType>
+    ::PrintSelf( std::ostream& os, Indent indent ) const
+  {
+    Superclass::PrintSelf(os,indent);
+  }
+
+  
+  template <class TInputImageType>
+  void ShortestPathCostFunction<TInputImageType>::
+    SetStartIndex (const typename TInputImageType::IndexType &StartIndex)
+  {
+    for (unsigned int i=0;i<TInputImageType::ImageDimension;++i)
+    {
+      m_StartIndex[i] = StartIndex[i];
+    }
+  }
+
+
+  template <class TInputImageType>
+  void ShortestPathCostFunction<TInputImageType>::
+    SetEndIndex (const typename TInputImageType::IndexType &EndIndex)
+  {
+    for (unsigned int i=0;i<TInputImageType::ImageDimension;++i)
+    {
+      m_EndIndex[i] = EndIndex[i];
+    }
+  }
+
+
+} // end namespace itk
+
+#endif // __itkShortestPathCostFunction_txx
diff --git a/Modules/GraphAlgorithms/itkShortestPathCostFunctionTbss.h b/Modules/GraphAlgorithms/itkShortestPathCostFunctionTbss.h
new file mode 100644
index 0000000000..b6c9715342
--- /dev/null
+++ b/Modules/GraphAlgorithms/itkShortestPathCostFunctionTbss.h
@@ -0,0 +1,75 @@
+#ifndef __itkShortestPathCostFunctionTbss_h
+#define __itkShortestPathCostFunctionTbss_h
+
+#include "itkObject.h"
+#include "itkObjectFactory.h"
+#include "itkShapedNeighborhoodIterator.h"
+#include "itkShortestPathCostFunction.h" // Superclass of Metrics
+#include <itkImageRegionConstIterator.h>
+
+
+
+namespace itk
+{
+
+  template <class TInputImageType>            
+  class ITK_EXPORT ShortestPathCostFunctionTbss : public ShortestPathCostFunction<TInputImageType>
+  {
+  public:
+    /** Standard class typedefs. */
+    typedef ShortestPathCostFunctionTbss               Self;
+    typedef ShortestPathCostFunction<TInputImageType>  Superclass;
+    typedef SmartPointer<Self>                         Pointer;
+    typedef SmartPointer<const Self>                   ConstPointer;
+    typedef itk::ImageRegionConstIterator<TInputImageType>  ConstIteratorType;
+    typedef typename TInputImageType::IndexType             IndexType;
+
+
+    typedef itk::Image<float,3> FloatImageType;
+
+    /** Method for creation through the object factory. */
+    itkNewMacro(Self);
+
+    /** Run-time type information (and related methods). */
+    itkTypeMacro(ShortestPathCostFunctionTbss, Object);
+   
+    // \brief calculates the costs for going from p1 to p2
+    virtual double GetCost( IndexType p1, IndexType p2);
+
+    // \brief Initialize the metric
+    virtual void Initialize ();
+
+    // \brief returns the minimal costs possible (needed for A*)
+    virtual double GetMinCost();
+
+
+    ShortestPathCostFunctionTbss();
+
+    void SetThreshold(double t)
+    {
+      m_Threshold = t;
+    }
+
+
+  protected:
+
+    virtual ~ShortestPathCostFunctionTbss() {};
+
+    double m_Threshold;
+
+
+  private:
+
+
+
+
+  };
+
+} // end namespace itk
+
+
+#ifndef ITK_MANUAL_INSTANTIATION
+#include "itkShortestPathCostFunctionTbss.txx"
+#endif
+
+#endif /* __itkShortestPathCostFunctionTbss_h */
diff --git a/Modules/GraphAlgorithms/itkShortestPathCostFunctionTbss.txx b/Modules/GraphAlgorithms/itkShortestPathCostFunctionTbss.txx
new file mode 100644
index 0000000000..41e6df7fab
--- /dev/null
+++ b/Modules/GraphAlgorithms/itkShortestPathCostFunctionTbss.txx
@@ -0,0 +1,74 @@
+#ifndef __itkShortestPathCostFunctionTbss_txx
+#define __itkShortestPathCostFunctionTbss_txx
+
+#include "itkShortestPathCostFunctionTbss.h"
+#include <limits>
+
+
+
+namespace itk
+{ 
+
+  // Constructor
+  template<class TInputImageType>
+  ShortestPathCostFunctionTbss<TInputImageType>
+    ::ShortestPathCostFunctionTbss()
+  {
+  }
+
+
+ template<class TInputImageType>
+  double ShortestPathCostFunctionTbss<TInputImageType>
+    ::GetCost(IndexType p1 ,IndexType  p2)
+  {
+    // Very simple Metric:
+    // The squared difference of both values is defined as cost
+
+    double a,b,c;    
+    ConstIteratorType it( this->m_Image, this->m_Image->GetRequestedRegion());
+    
+    it.SetIndex(p1);
+    a = it.Get();
+    it.SetIndex(p2);
+    b = it.Get();
+
+
+    if(this->m_Image->GetPixel(p2) < m_Threshold)
+    {
+      c = std::numeric_limits<double>::max( );
+    }
+    else
+    {
+      double dxSqt = (p1[0]-p2[0]) * (p1[0]-p2[0]);// * (p1[0]-p2[0]);
+      double dySqt = (p1[1]-p2[1]) * (p1[1]-p2[1]);
+      double dzSqt = (p1[2]-p2[2]) * (p1[2]-p2[2]);
+
+      double weight = this->m_Image->GetPixel(p2);
+
+      c = (dxSqt + dySqt + dzSqt) + 10000 * (1-weight);
+    }
+
+
+    return c; 
+  }
+
+
+  template<class TInputImageType>
+  void ShortestPathCostFunctionTbss<TInputImageType>
+    ::Initialize()
+  {
+  }
+
+  template<class TInputImageType>
+  double ShortestPathCostFunctionTbss<TInputImageType>
+   ::GetMinCost()
+  {
+    return 1;
+  }
+
+
+
+
+} // end namespace itk
+
+#endif // __itkShortestPathCostFunctionSimple_txx
diff --git a/Modules/GraphAlgorithms/itkShortestPathImageFilter.h b/Modules/GraphAlgorithms/itkShortestPathImageFilter.h
new file mode 100644
index 0000000000..e4f839813a
--- /dev/null
+++ b/Modules/GraphAlgorithms/itkShortestPathImageFilter.h
@@ -0,0 +1,215 @@
+#ifndef __itkShortestPathImageFilter_h_
+#define __itkShortestPathImageFilter_h_
+
+#include "itkImageToImageFilter.h"
+#include "itkShortestPathCostFunction.h"
+#include "itkShortestPathNode.h"
+#include <itkImageRegionIteratorWithIndex.h>
+
+#include <MitkGraphAlgorithmsExports.h>
+
+// ------- INFORMATION ----------
+/// SET FUNCTIONS
+//void SetInput( ItkImage ) // Compulsory
+//void SetStartIndex (const IndexType & StartIndex); // Compulsory
+//void SetEndIndex(const IndexType & EndIndex); // Compulsory
+//void SetFullNeighborsMode(bool) // Optional (default=false), if false N4, if true N26
+//void SetActivateTimeOut(bool) // Optional (default=false), for debug issues: after 30s algorithms terminates. You can have a look at the VectorOrderImage to see how far it came
+//void SetMakeOutputImage(bool) // Optional (default=true), Generate an outputimage of the path. You can also get the path directoy with GetVectorPath()
+//void SetCalcAllDistances(bool) // Optional (default=false), Calculate Distances over the whole image. CAREFUL, algorithm time extends a lot. Necessary for GetDistanceImage
+//void SetStoreVectorOrder(bool) // Optional (default=false), Stores in which order the pixels were checked. Necessary for GetVectorOrderImage
+//void AddEndIndex(const IndexType & EndIndex) //Optional. By calling this function you can add several endpoints! The algorithm will look for several shortest Pathes. From Start to all Endpoints.
+//
+/// GET FUNCTIONS
+//std::vector< itk::Index<3> > GetVectorPath(); // returns the shortest path as vector
+//std::vector< std::vector< itk::Index<3> > GetMultipleVectorPathe(); // returns a vector of shortest Pathes (which are vectors of points)
+//GetDistanceImage // Returns the distance image
+//GetVectorOrderIMage // Returns the Vector Order image
+//
+// EXAMPLE USE
+// pleae see qmitkmitralvalvesegmentation4dtee bundle
+
+
+
+
+
+namespace itk 
+{
+
+   template <class TInputImageType, class TOutputImageType>
+   class ITK_EXPORT ShortestPathImageFilter :
+      public ImageToImageFilter<TInputImageType, TOutputImageType>
+   {
+   public:
+      //Standard Typedefs    
+      typedef ShortestPathImageFilter			                       Self;
+      typedef ImageToImageFilter<TInputImageType,TOutputImageType> Superclass;
+      typedef SmartPointer<Self>                                   Pointer;
+      typedef SmartPointer<const Self>                             ConstPointer;
+
+      // Typdefs for metric
+      typedef ShortestPathCostFunction< TInputImageType >				   CostFunctionType;
+      typedef typename CostFunctionType::Pointer						      CostFunctionTypePointer;        
+
+
+      // More typdefs for convenience
+      typedef TInputImageType                                          InputImageType;
+      typedef typename TInputImageType::Pointer                        InputImagePointer;
+      typedef typename TInputImageType::PixelType                      InputImagePixelType;
+      typedef typename TInputImageType::SizeType                       InputImageSizeType;
+      typedef typename TInputImageType::IndexType                      IndexType;
+      typedef typename itk::ImageRegionIteratorWithIndex< InputImageType >          InputImageIteratorType;
+
+      typedef TOutputImageType                                    OutputImageType;
+      typedef typename TOutputImageType::Pointer                  OutputImagePointer;
+      typedef typename TOutputImageType::PixelType                OutputImagePixelType;
+      typedef typename TOutputImageType::IndexType                OutputImageIndexType;
+      typedef ImageRegionIteratorWithIndex< OutputImageType >    OutputImageIteratorType;
+      typedef itk::ShapedNeighborhoodIterator< TInputImageType >  itkShapedNeighborhoodIteratorType;
+
+
+      // New Macro for smartpointer instantiation
+      itkNewMacro(Self);
+
+      // Run-time type information
+      itkTypeMacro(ShortestPathImageFilter, ImageToImageFilter);
+
+      // Display
+      void PrintSelf( std::ostream& os, Indent indent ) const;
+
+      // Compare function for A_STAR
+      struct CompareNodeStar 
+      {
+         bool operator()(ShortestPathNode *a, ShortestPathNode *b)
+         {
+            return (a->distAndEst > b->distAndEst);
+         }
+      };
+
+        // \brief Set Starpoint for ShortestPath Calculation
+      void SetStartIndex (const IndexType & StartIndex);
+
+      // \brief Adds Endpoint for multiple ShortestPath Calculation
+      void AddEndIndex (const IndexType & index);
+
+      // \brief Set Endpoint for ShortestPath Calculation
+      void SetEndIndex(const IndexType & EndIndex);
+
+
+      // \brief Set FullNeighborsMode. false = no diagonal neighbors, in 2D this means N4 Neigborhood. true = would be N8 in 2D
+      itkSetMacro (FullNeighborsMode, bool); 
+      itkGetMacro (FullNeighborsMode, bool);
+
+      // \brief (default=true), Produce output image, which shows the shortest path. But you can also get the shortest Path directly as vector with the function GetVectorPath
+      itkSetMacro (MakeOutputImage, bool);
+      itkGetMacro (MakeOutputImage, bool);
+
+      // \brief (default=false), Store an Vector of Order, so you can call getVectorOrderImage after update
+      itkSetMacro (StoreVectorOrder, bool);
+      itkGetMacro (StoreVectorOrder, bool);
+
+      // \brief (default=false), // Calculate all Distances to all pixels, so you can call getDistanceImage after update (warning algo will take a long time)
+      itkSetMacro (CalcAllDistances, bool);
+      itkGetMacro (CalcAllDistances, bool);
+
+      // \brief (default=false), for debug issues: after 30s algorithms terminates. You can have a look at the VectorOrderImage to see how far it came
+      itkSetMacro (ActivateTimeOut, bool);
+      itkGetMacro (ActivateTimeOut, bool);
+
+      // \brief returns shortest Path as vector
+      std::vector< itk::Index<3> > GetVectorPath();
+
+      // \brief returns Multiple shortest Paths. You can call this function, when u performed a multiple shortest path search (one start, several ends)
+      std::vector< std::vector< itk::Index<3> > > GetMultipleVectorPaths();
+
+      // \brief returns the vector order image. It shows in which order the pixels were checked. good for debugging. Be sure to have m_StoreVectorOrder=true
+      OutputImagePointer GetVectorOrderImage();
+      
+      // \brief returns the distance image. It shows the distances from the startpoint to all other pixels. Be sure to have m_CalcAllDistances=true
+      OutputImagePointer GetDistanceImage();
+      
+      // \brief cleans up the filter
+      void CleanUp();
+
+      itkSetObjectMacro( CostFunction, CostFunctionType ); // itkSetObjectMacro = set function that uses pointer as parameter
+      itkGetObjectMacro( CostFunction, CostFunctionType );
+
+   protected:
+
+      std::vector< IndexType > m_endPoints; // if you fill this vector, the algo will not rest until all endPoints have been reached
+      std::vector< IndexType > m_endPointsClosed;
+
+      ShortestPathNode* m_Nodes; // main list that contains all nodes
+      NodeNumType m_Graph_NumberOfNodes;
+      NodeNumType m_Graph_StartNode;
+      NodeNumType m_Graph_EndNode;
+      int m_ImageDimensions;
+      bool m_Graph_fullNeighbors;
+      std::vector<ShortestPathNode*> m_Graph_DiscoveredNodeList;
+      ShortestPathImageFilter(Self&);   // intentionally not implemented
+      void operator=(const Self&);      // intentionally not implemented
+      const static int BACKGROUND = 0;
+      const static int FOREGROUND = 255;
+      bool m_FullNeighborsMode;      
+
+      bool m_MakeOutputImage;   
+      bool m_StoreVectorOrder; // Store an Vector of Order, so you can call getVectorOrderImage after update
+      bool m_CalcAllDistances; // Calculate all Distances, so you can call getDistanceImage after update (warning algo will take a long time)
+
+      bool multipleEndPoints;
+
+      bool m_ActivateTimeOut; // if true, then i search max. 30 secs. then abort
+      CostFunctionTypePointer m_CostFunction;
+      IndexType m_StartIndex, m_EndIndex; 
+      std::vector< itk::Index<3> > m_VectorPath;
+      std::vector< std::vector< itk::Index<3> > > m_MultipleVectorPaths;
+
+
+      std::vector< NodeNumType > m_VectorOrder;
+
+
+
+      ShortestPathImageFilter ();
+
+      // \brief Fill m_VectorPath
+      void MakeShortestPathVector();
+
+      // \brief Create all the outputs
+      void MakeOutputs();
+
+      // \brief Generate Data
+      void GenerateData();
+
+      // \brief gets the estimate costs from pixel a to target.
+      double getEstimatedCostsToTarget(const IndexType & a);
+
+      typename InputImageType::Pointer m_magnitudeImage;       
+
+      // \brief Convert a indexnumber of a node in m_Nodes to image coordinates
+      typename TInputImageType::IndexType NodeToCoord(NodeNumType);
+
+      // \brief Convert image coordinate to a indexnumber of a node in m_Nodes
+      unsigned int CoordToNode(IndexType);
+
+      // \brief Returns the neighbors of a node
+      std::vector<ShortestPathNode*> GetNeighbors(NodeNumType nodeNum, bool FullNeighbors);
+
+      // \brief Check if coords are in bounds of image
+      bool CoordIsInBounds(IndexType);
+
+      // \brief Initializes the graph
+      void InitGraph();
+
+      // \brief Start ShortestPathSearch
+      void StartShortestPathSearch();
+
+   };
+
+} // end of namespace itk
+
+
+#ifndef ITK_MANUAL_INSTANTIATION
+#include "itkShortestPathImageFilter.txx"
+#endif
+
+#endif
diff --git a/Modules/GraphAlgorithms/itkShortestPathImageFilter.txx b/Modules/GraphAlgorithms/itkShortestPathImageFilter.txx
new file mode 100644
index 0000000000..14dc080536
--- /dev/null
+++ b/Modules/GraphAlgorithms/itkShortestPathImageFilter.txx
@@ -0,0 +1,917 @@
+#include "itkShortestPathImageFilter.h"
+#include "time.h"
+#include "mitkMemoryUtilities.h"
+#include <iostream>
+#include <algorithm>
+#include <vector>
+
+namespace itk 
+{
+	// Constructor  (initialize standard values)
+	template <class TInputImageType, class TOutputImageType>
+	ShortestPathImageFilter<TInputImageType, TOutputImageType>
+		::ShortestPathImageFilter() :
+		m_FullNeighborsMode(false),   
+		m_MakeOutputImage(true),
+		m_StoreVectorOrder(false),		
+		m_CalcAllDistances(false),
+		m_ActivateTimeOut(false),
+      multipleEndPoints(false),
+      m_Nodes(0),
+      m_Graph_NumberOfNodes(0)
+	{
+      m_endPoints.clear();
+      m_endPointsClosed.clear();
+
+      if (m_MakeOutputImage)
+      {
+         this->SetNumberOfRequiredOutputs(1);
+		   this->SetNthOutput( 0, OutputImageType::New() ); 
+      }
+	} 
+
+	template <class TInputImageType, class TOutputImageType>
+	inline typename ShortestPathImageFilter<TInputImageType, TOutputImageType>::IndexType
+		ShortestPathImageFilter<TInputImageType, TOutputImageType>
+		::NodeToCoord (NodeNumType node)
+	{
+		const InputImageSizeType &size = this->GetInput()->GetRequestedRegion().GetSize();
+		int dim = InputImageType::ImageDimension;
+		IndexType coord;
+		if (dim == 2)
+		{      
+			coord[1] = node / size[0];
+			coord[0] = node % size[0];
+			if ((coord[0] >= size[0]) || (coord[1] >= size[1]))
+			{
+				coord[0] = 0;
+				coord[1] = 0;
+			}
+		}
+		if (dim == 3)
+		{      
+			coord[2] = node / (size[0]*size[1]);
+			coord[1] = (node % (size[0]*size[1])) / size[0];
+			coord[0] = (node % (size[0]*size[1])) % size[0];
+			if ((coord[0] >= size[0]) || (coord[1] >= size[1]) || (coord[2] >= size[2]))
+			{
+				coord[0] = 0;
+				coord[1] = 0;
+				coord[2] = 0;
+			}
+		}
+
+		return coord;    
+	}
+
+	template <class TInputImageType, class TOutputImageType>
+	inline typename itk::NodeNumType
+		ShortestPathImageFilter<TInputImageType, TOutputImageType>::
+		CoordToNode (IndexType coord)
+	{    
+		const InputImageSizeType &size = this->GetInput()->GetRequestedRegion().GetSize();
+		int dim = InputImageType::ImageDimension;
+		NodeNumType node = 0;
+		if (dim == 2)
+		{      
+			node = (coord[1]*size[0]) + coord[0];
+		}
+		if (dim == 3)
+		{
+			node = (coord[2]*size[0]*size[1]) + (coord[1]*size[0]) + coord[0];
+		}
+		if ((m_Graph_NumberOfNodes > 0) && (node >= m_Graph_NumberOfNodes))
+      {
+         MITK_INFO << "WARNING! Coordinates outside image!";
+         MITK_INFO << "Coords = " << coord ;
+         MITK_INFO << "ImageDim = " << dim ;
+         MITK_INFO << "RequestedRegionSize = " << size ;
+			node = 0;
+      }
+
+		return node;
+	}
+
+	template <class TInputImageType, class TOutputImageType>
+	inline bool
+		ShortestPathImageFilter<TInputImageType, TOutputImageType>::
+		CoordIsInBounds  (IndexType coord)
+	{
+		const InputImageSizeType &size = this->GetInput()->GetRequestedRegion().GetSize();
+		int dim = InputImageType::ImageDimension;
+
+		if (dim == 2)
+		{
+			if ((coord[0] >= 0) 
+				&& (coord[0] < size[0])
+				&& (coord[1] >= 0 )
+				&& (coord[1] < size[1] ))
+			{
+				return true;
+			}
+		}
+		if (dim == 3)
+		{
+			if ((coord[0] >= 0) 
+				&& (coord[0] < size[0])
+				&& (coord[1] >= 0 )
+				&& (coord[1] < size[1] )
+				&& (coord[2] >= 0 )
+				&& (coord[2] < size[2] ))
+			{
+				return true;
+			}
+		}
+		return false;
+	}
+
+
+	template <class TInputImageType, class TOutputImageType>
+	inline std::vector< ShortestPathNode* >
+		ShortestPathImageFilter<TInputImageType, TOutputImageType>::
+		GetNeighbors  (unsigned int nodeNum, bool FullNeighbors)
+	{
+		// returns a vector of nodepointers.. these nodes are the neighbors
+		int dim = InputImageType::ImageDimension;
+		IndexType Coord = NodeToCoord(nodeNum);
+		IndexType NeighborCoord;
+		std::vector<ShortestPathNode*> nodeList;
+
+		int neighborDistance = 1; //if i increase that, i might not hit the endnote
+
+      // maybe use itkNeighborhoodIterator here, might be faster
+
+		if ( dim == 2)
+		{
+			// N4
+			NeighborCoord[0] = Coord[0];
+			NeighborCoord[1] = Coord[1]-neighborDistance;
+			if (CoordIsInBounds(NeighborCoord))
+				nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+			NeighborCoord[0] = Coord[0]+neighborDistance;
+			NeighborCoord[1] = Coord[1];
+			if (CoordIsInBounds(NeighborCoord))
+				nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+			NeighborCoord[0] = Coord[0];
+			NeighborCoord[1] = Coord[1]+neighborDistance;
+			if (CoordIsInBounds(NeighborCoord))
+				nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+			NeighborCoord[0] = Coord[0]-neighborDistance;
+			NeighborCoord[1] = Coord[1];
+			if (CoordIsInBounds(NeighborCoord))
+				nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+			if (FullNeighbors)
+			{
+				// N8
+				NeighborCoord[0] = Coord[0]-neighborDistance;
+				NeighborCoord[1] = Coord[1]-neighborDistance;
+				if (CoordIsInBounds(NeighborCoord))
+					nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+				NeighborCoord[0] = Coord[0]+neighborDistance;
+				NeighborCoord[1] = Coord[1]-neighborDistance;
+				if (CoordIsInBounds(NeighborCoord))
+					nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+				NeighborCoord[0] = Coord[0]-neighborDistance;
+				NeighborCoord[1] = Coord[1]+neighborDistance;
+				if (CoordIsInBounds(NeighborCoord))
+					nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+				NeighborCoord[0] = Coord[0]+neighborDistance;
+				NeighborCoord[1] = Coord[1]+neighborDistance;
+				if (CoordIsInBounds(NeighborCoord))
+					nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+			}
+		}
+		if ( dim == 3)
+		{
+			// N6
+			NeighborCoord[0] = Coord[0];
+			NeighborCoord[1] = Coord[1]-neighborDistance;
+			NeighborCoord[2] = Coord[2];
+			if (CoordIsInBounds(NeighborCoord))
+				nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+			NeighborCoord[0] = Coord[0]+neighborDistance;
+			NeighborCoord[1] = Coord[1];
+			NeighborCoord[2] = Coord[2];
+			if (CoordIsInBounds(NeighborCoord))
+				nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+			NeighborCoord[0] = Coord[0];
+			NeighborCoord[1] = Coord[1]+neighborDistance;
+			NeighborCoord[2] = Coord[2];
+			if (CoordIsInBounds(NeighborCoord))
+				nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+			NeighborCoord[0] = Coord[0]-neighborDistance;
+			NeighborCoord[1] = Coord[1];
+			NeighborCoord[2] = Coord[2];
+			if (CoordIsInBounds(NeighborCoord))
+				nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+			NeighborCoord[0] = Coord[0];
+			NeighborCoord[1] = Coord[1];
+			NeighborCoord[2] = Coord[2]+neighborDistance;
+			if (CoordIsInBounds(NeighborCoord))
+				nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+			NeighborCoord[0] = Coord[0];
+			NeighborCoord[1] = Coord[1];
+			NeighborCoord[2] = Coord[2]-neighborDistance;
+			if (CoordIsInBounds(NeighborCoord))
+				nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+			if (FullNeighbors)
+			{
+				// N26
+				// Middle Slice
+				NeighborCoord[0] = Coord[0]-neighborDistance;
+				NeighborCoord[1] = Coord[1]-neighborDistance;
+				NeighborCoord[2] = Coord[2];
+				if (CoordIsInBounds(NeighborCoord))
+					nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+				NeighborCoord[0] = Coord[0]+neighborDistance; 
+				NeighborCoord[1] = Coord[1]-neighborDistance;
+				NeighborCoord[2] = Coord[2];
+				if (CoordIsInBounds(NeighborCoord))
+					nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+				NeighborCoord[0] = Coord[0]-neighborDistance; 
+				NeighborCoord[1] = Coord[1]+neighborDistance;
+				NeighborCoord[2] = Coord[2];
+				if (CoordIsInBounds(NeighborCoord))
+					nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+				NeighborCoord[0] = Coord[0]+neighborDistance; 
+				NeighborCoord[1] = Coord[1]+neighborDistance;
+				NeighborCoord[2] = Coord[2];
+				if (CoordIsInBounds(NeighborCoord))
+					nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+				// BackSlice (Diagonal)
+				NeighborCoord[0] = Coord[0]-neighborDistance;
+				NeighborCoord[1] = Coord[1]-neighborDistance;
+				NeighborCoord[2] = Coord[2]-neighborDistance;
+				if (CoordIsInBounds(NeighborCoord))
+					nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+				NeighborCoord[0] = Coord[0]+neighborDistance; 
+				NeighborCoord[1] = Coord[1]-neighborDistance;
+				NeighborCoord[2] = Coord[2]-neighborDistance;
+				if (CoordIsInBounds(NeighborCoord))
+					nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+				NeighborCoord[0] = Coord[0]-neighborDistance; 
+				NeighborCoord[1] = Coord[1]+neighborDistance;
+				NeighborCoord[2] = Coord[2]-neighborDistance;
+				if (CoordIsInBounds(NeighborCoord))
+					nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+				NeighborCoord[0] = Coord[0]+neighborDistance; 
+				NeighborCoord[1] = Coord[1]+neighborDistance;
+				NeighborCoord[2] = Coord[2]-neighborDistance;
+				if (CoordIsInBounds(NeighborCoord))
+					nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+				//BackSlice (Non-Diag)
+				NeighborCoord[0] = Coord[0];
+				NeighborCoord[1] = Coord[1]-neighborDistance;
+				NeighborCoord[2] = Coord[2]-neighborDistance;
+				if (CoordIsInBounds(NeighborCoord))
+					nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+				NeighborCoord[0] = Coord[0]+neighborDistance;
+				NeighborCoord[1] = Coord[1];
+				NeighborCoord[2] = Coord[2]-neighborDistance;
+				if (CoordIsInBounds(NeighborCoord))
+					nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+				NeighborCoord[0] = Coord[0];
+				NeighborCoord[1] = Coord[1]+neighborDistance;
+				NeighborCoord[2] = Coord[2]-neighborDistance;
+				if (CoordIsInBounds(NeighborCoord))
+					nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+				NeighborCoord[0] = Coord[0]-neighborDistance;
+				NeighborCoord[1] = Coord[1];
+				NeighborCoord[2] = Coord[2]-neighborDistance;
+				if (CoordIsInBounds(NeighborCoord))
+					nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+				// FrontSlice (Diagonal)
+				NeighborCoord[0] = Coord[0]-neighborDistance;
+				NeighborCoord[1] = Coord[1]-neighborDistance;
+				NeighborCoord[2] = Coord[2]+neighborDistance;
+				if (CoordIsInBounds(NeighborCoord))
+					nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+				NeighborCoord[0] = Coord[0]+neighborDistance; 
+				NeighborCoord[1] = Coord[1]-neighborDistance;
+				NeighborCoord[2] = Coord[2]+neighborDistance;
+				if (CoordIsInBounds(NeighborCoord))
+					nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+				NeighborCoord[0] = Coord[0]-neighborDistance; 
+				NeighborCoord[1] = Coord[1]+neighborDistance;
+				NeighborCoord[2] = Coord[2]+neighborDistance;
+				if (CoordIsInBounds(NeighborCoord))
+					nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+				NeighborCoord[0] = Coord[0]+neighborDistance; 
+				NeighborCoord[1] = Coord[1]+neighborDistance;
+				NeighborCoord[2] = Coord[2]+neighborDistance;
+				if (CoordIsInBounds(NeighborCoord))
+					nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+				//FrontSlice(Non-Diag)
+				NeighborCoord[0] = Coord[0];
+				NeighborCoord[1] = Coord[1]-neighborDistance;
+				NeighborCoord[2] = Coord[2]+neighborDistance;
+				if (CoordIsInBounds(NeighborCoord))
+					nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+				NeighborCoord[0] = Coord[0]+neighborDistance;
+				NeighborCoord[1] = Coord[1];
+				NeighborCoord[2] = Coord[2]+neighborDistance;
+				if (CoordIsInBounds(NeighborCoord))
+					nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+				NeighborCoord[0] = Coord[0];
+				NeighborCoord[1] = Coord[1]+neighborDistance;
+				NeighborCoord[2] = Coord[2]+neighborDistance;
+				if (CoordIsInBounds(NeighborCoord))
+					nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+				NeighborCoord[0] = Coord[0]-neighborDistance;
+				NeighborCoord[1] = Coord[1];
+				NeighborCoord[2] = Coord[2]+neighborDistance;
+				if (CoordIsInBounds(NeighborCoord))
+					nodeList.push_back(&m_Nodes[CoordToNode(NeighborCoord)]);
+
+			}
+		}
+		return nodeList;
+	}
+
+
+	template <class TInputImageType, class TOutputImageType>
+	void ShortestPathImageFilter<TInputImageType, TOutputImageType>::
+		SetStartIndex (const typename TInputImageType::IndexType &StartIndex)
+	{
+		for (unsigned int i=0;i<TInputImageType::ImageDimension;++i)
+		{
+			m_StartIndex[i] = StartIndex[i];
+		}    
+      m_Graph_StartNode = CoordToNode(m_StartIndex); 
+      //MITK_INFO << "StartIndex = " << StartIndex;
+      //MITK_INFO << "StartNode = " << m_Graph_StartNode;
+	}
+
+
+	template <class TInputImageType, class TOutputImageType>
+	void ShortestPathImageFilter<TInputImageType, TOutputImageType>::
+		SetEndIndex (const typename TInputImageType::IndexType &EndIndex)
+	{
+		for (unsigned int i=0;i<TInputImageType::ImageDimension;++i)
+		{
+			m_EndIndex[i] = EndIndex[i];
+		}
+      m_Graph_EndNode = CoordToNode(m_EndIndex); 
+      //MITK_INFO << "EndNode = " << m_Graph_EndNode;
+	}
+
+   template <class TInputImageType, class TOutputImageType>
+	void ShortestPathImageFilter<TInputImageType, TOutputImageType>::
+   AddEndIndex (const typename TInputImageType::IndexType &index)
+	{
+      // ONLY FOR MULTIPLE END POINTS SEARCH
+      IndexType newEndIndex;
+		for (unsigned int i=0;i<TInputImageType::ImageDimension;++i)
+		{
+			newEndIndex[i] = index[i];
+		}
+      m_endPoints.push_back(newEndIndex);            
+      SetEndIndex( m_endPoints[0] );      
+      multipleEndPoints = true;
+	}
+
+	template <class TInputImageType, class TOutputImageType>
+	inline double ShortestPathImageFilter<TInputImageType, TOutputImageType>::
+		getEstimatedCostsToTarget (const typename TInputImageType::IndexType &a)
+	{
+		// Returns the minimal possible costs for a path from "a" to targetnode.
+      itk::Vector<float,3> v;
+		v[0] = m_EndIndex[0]-a[0];
+		v[1] = m_EndIndex[1]-a[1];
+		v[2] = m_EndIndex[2]-a[2];
+
+      return  m_CostFunction->GetMinCost() * v.GetNorm();
+	}
+
+
+	template <class TInputImageType, class TOutputImageType>
+	void
+		ShortestPathImageFilter<TInputImageType, TOutputImageType>::
+		InitGraph()
+	{
+		// Clean up previous stuff
+		CleanUp();
+
+		// initalize cost function
+		m_CostFunction->Initialize();
+
+		// Calc Number of nodes
+		m_ImageDimensions = TInputImageType::ImageDimension;
+		const InputImageSizeType &size = this->GetInput()->GetRequestedRegion().GetSize();
+		m_Graph_NumberOfNodes = 1;
+		for (NodeNumType i=0; i<m_ImageDimensions; ++i)
+			m_Graph_NumberOfNodes=m_Graph_NumberOfNodes*size[i];
+
+		// Initialize mainNodeList with that number
+		m_Nodes = new ShortestPathNode[m_Graph_NumberOfNodes];
+
+		// Initialize each node in nodelist    
+		for (NodeNumType i=0; i<m_Graph_NumberOfNodes; i++)
+		{  
+			m_Nodes[i].distAndEst = -1; 
+			m_Nodes[i].distance = -1; 
+			m_Nodes[i].prevNode = -1;
+			m_Nodes[i].mainListIndex=i;
+         m_Nodes[i].closed=false;
+		}
+
+		// In the beginning, the Startnode needs a distance of 0
+		m_Nodes[m_Graph_StartNode].distance = 0;
+      m_Nodes[m_Graph_StartNode].distAndEst = 0;
+	}
+
+	template <class TInputImageType, class TOutputImageType>
+	void
+		ShortestPathImageFilter<TInputImageType, TOutputImageType>::
+		StartShortestPathSearch()
+	{
+      // Setup Timer
+		clock_t startAll = clock();
+		clock_t stopAll = clock();
+
+      // init variables
+		double durationAll = 0;
+		bool timeout = false; 
+		bool makeNewHeapNecessary = false;
+      NodeNumType mainNodeListIndex = 0;
+		DistanceType curNodeDistance = 0;
+		DistanceType curNodeDistAndEst = 0;
+      NodeNumType numberOfNodesChecked = 0;
+
+      // Create Multimap (tree structure for fast searching)
+      std::multimap<double,ShortestPathNode*> myMap;
+      std::pair< std::multimap<double,ShortestPathNode*>::iterator, std::multimap<double,ShortestPathNode*>::iterator> ret;
+      std::multimap<double,ShortestPathNode*>::iterator it;
+
+      // At first, only startNote is discovered.
+      myMap.insert( std::pair<double,ShortestPathNode*> (m_Nodes[m_Graph_StartNode].distAndEst, &m_Nodes[m_Graph_StartNode]) );
+
+		// While there are discovered Nodes, pick the one with lowest distance,
+      // update its neighbors and eventually delete it from the discovered Nodes list.
+		while(!myMap.empty())
+		{     
+         numberOfNodesChecked++;
+         if ( (numberOfNodesChecked % (m_Graph_NumberOfNodes/100)) == 0)
+         {
+            MITK_INFO << "Checked " << ( numberOfNodesChecked / (m_Graph_NumberOfNodes/100) ) << "% List: " << myMap.size() << "\n";            
+         }
+         
+         // Get element with lowest score
+         mainNodeListIndex = myMap.begin()->second->mainListIndex; 
+			curNodeDistAndEst = myMap.begin()->second->distAndEst; 
+			curNodeDistance = myMap.begin()->second->distance; 
+         myMap.begin()->second->closed = true; // close it
+
+         // Debug:         
+         //MITK_INFO << "INFO: size " << myMap.size();
+         /*
+         for (it = myMap.begin(); it != myMap.end(); ++it)
+         {
+            MITK_INFO << "(1) " << it->first << "|" << it->second->distAndEst << "|"<<it->second->mainListIndex;         
+         }
+         */
+         
+			// Kicks out element with lowest score
+         myMap.erase( myMap.begin() );
+
+         // if wanted, store vector order
+			if (m_StoreVectorOrder)
+         {
+				m_VectorOrder.push_back(mainNodeListIndex);
+         }
+
+			// Check neighbors
+			std::vector<ShortestPathNode*> neighborNodes = GetNeighbors(mainNodeListIndex, m_Graph_fullNeighbors);
+			for (NodeNumType i=0; i<neighborNodes.size(); i++)
+			{         
+            if (neighborNodes[i]->closed)
+               continue; // this nodes is already closed, go to next neighbor
+
+				IndexType coordCurNode = NodeToCoord(mainNodeListIndex);
+				IndexType coordNeighborNode = NodeToCoord(neighborNodes[i]->mainListIndex);
+
+				// calculate the new Distance to the current neighbor
+				double newDistance = curNodeDistance
+					+ (m_CostFunction->GetCost(coordCurNode, coordNeighborNode));
+
+				// if it is shorter than any yet known path to this neighbor, than the current path is better. Save that!
+				if ((newDistance < neighborNodes[i]->distance) || (neighborNodes[i]->distance == -1) )
+				{
+					// if that neighbornode is not in discoverednodeList yet, Push it there and update
+					if (neighborNodes[i]->distance == -1)
+					{
+                  
+						neighborNodes[i]->distance = newDistance;
+						neighborNodes[i]->distAndEst = newDistance + getEstimatedCostsToTarget(coordNeighborNode);
+						neighborNodes[i]->prevNode = mainNodeListIndex;                                    
+                  myMap.insert( std::pair<double,ShortestPathNode*> (m_Nodes[neighborNodes[i]->mainListIndex].distAndEst, &m_Nodes[neighborNodes[i]->mainListIndex]) );
+                  /*
+                  MITK_INFO << "Inserted: " << m_Nodes[neighborNodes[i]->mainListIndex].distAndEst << "|" << m_Nodes[neighborNodes[i]->mainListIndex].mainListIndex;
+                  MITK_INFO << "INFO: size " << myMap.size();
+                  for (it = myMap.begin(); it != myMap.end(); ++it)
+                  {
+                     MITK_INFO << "(1) " << it->first << "|" << it->second->distAndEst << "|"<<it->second->mainListIndex;         
+                  }
+                  */
+					}
+					// or if is already in discoverednodelist, update
+					else
+					{
+
+                  /*
+                  it = myMap.find(neighborNodes[i]->distAndEst);
+                  if (it == myMap.end() )
+                  {
+                     MITK_INFO << "Nothing!";
+                     // look further
+                     for (it = myMap.begin(); it != myMap.end(); ++it)
+                     {
+                        if ((*it).second->mainListIndex == lookForId)
+                        {
+                           MITK_INFO << "But it is there!!!";
+                           MITK_INFO << "Searched for: " << lookFor << "   but had: " << (*it).second->distAndEst;
+                        }
+
+                     }
+                  }   
+                  */
+
+                  // 1st : find and delete old element
+                  bool found = false;
+                  double lookFor = neighborNodes[i]->distAndEst;
+                  unsigned int lookForId = neighborNodes[i]->mainListIndex;
+                  ret = myMap.equal_range(neighborNodes[i]->distAndEst);
+                  
+                  if ((ret.first == ret.second))
+                  {
+                     MITK_INFO << "No exact match!"; // if this happens, you are screwed
+                     /*
+                     MITK_INFO << "Was looking for: " << lookFor << "  ID: " << lookForId;
+                     if (ret.first != myMap.end() )
+                     {
+                        it = ret.first;
+                        MITK_INFO << "Found: " << it->first << "  ID: " << it->second->mainListIndex;
+                        ++it;
+                        MITK_INFO << "Found: " << it->first << "  ID: " << it->second->mainListIndex;
+                        --it; 
+                        --it;
+                        MITK_INFO << "Found: " << it->first << "  ID: " << it->second->mainListIndex;
+                     }
+                     
+                     // look if that ID is found in the map
+                     for (it = myMap.begin(); it != myMap.end(); ++it)
+                     {
+                        if ((*it).second->mainListIndex == lookForId)
+                        {
+                           MITK_INFO << "But it is there!!!";
+                           MITK_INFO << "Searched dist: " << lookFor << "  found dist: " << (*it).second->distAndEst;
+                        }
+
+                     }
+                     */
+                  }
+                  else 
+                  {
+                     for (it=ret.first; it!=ret.second; ++it)
+                     {
+                        if (it->second->mainListIndex == neighborNodes[i]->mainListIndex)
+                        {
+                           found = true;                           
+                           myMap.erase(it);
+                           /*
+                           MITK_INFO << "INFO: size " << myMap.size();
+                           MITK_INFO << "Erase: " << it->first << "|" << it->second->mainListIndex;
+                           
+                           MITK_INFO << "INFO: size " << myMap.size();
+                           for (it = myMap.begin(); it != myMap.end(); ++it)
+                           {
+                              MITK_INFO << "(1) " << it->first << "|" << it->second->distAndEst << "|"<<it->second->mainListIndex;         
+                           }
+                           */
+                           break;
+                        }
+                     }
+                  }
+
+                  
+                  if (!found)
+                  {
+                     MITK_INFO << "Could not find it! :(";
+                     continue;
+                  }
+
+                  // 2nd: update and insert new element
+                  neighborNodes[i]->distance = newDistance;
+						neighborNodes[i]->distAndEst = newDistance + getEstimatedCostsToTarget(coordNeighborNode);
+						neighborNodes[i]->prevNode = mainNodeListIndex;                          
+                  //myMap.insert( std::pair<double,ShortestPathNode*> (neighborNodes[i]->distAndEst, neighborNodes[i]));
+                  myMap.insert( std::pair<double,ShortestPathNode*> (m_Nodes[neighborNodes[i]->mainListIndex].distAndEst, &m_Nodes[neighborNodes[i]->mainListIndex]) );
+
+                  //MITK_INFO << "Re-Inserted: " << m_Nodes[neighborNodes[i]->mainListIndex].distAndEst << "|" << m_Nodes[neighborNodes[i]->mainListIndex].mainListIndex;
+                  //MITK_INFO << "INFO: size " << myMap.size();
+                  /*for (it = myMap.begin(); it != myMap.end(); ++it)
+                  {
+                     MITK_INFO << "(1) " << it->first << "|" << it->second->distAndEst << "|"<<it->second->mainListIndex;         
+                  }*/
+                  
+					}
+				}
+			}   
+			// finished with checking all neighbors. 
+
+			// Check Timeout, if activated
+			if (m_ActivateTimeOut)
+			{
+				stopAll = clock();
+				durationAll =  (double)(stopAll - startAll) / CLOCKS_PER_SEC;   
+				if (durationAll >= 30)
+				{
+					MITK_INFO << "TIMEOUT!! Search took over 30 seconds"; 
+					timeout = true ;
+				}
+			}
+
+         // Check end criteria:
+         // For multiple points
+         if ( multipleEndPoints )         
+         {
+            // super slow, make it faster
+            for (int i=0 ;i<m_endPoints.size(); i++)
+            { 
+               if (CoordToNode(m_endPoints[i]) == mainNodeListIndex)
+               {
+                  m_endPointsClosed.push_back(NodeToCoord(mainNodeListIndex));
+                  m_endPoints.erase(m_endPoints.begin()+i);
+                  if (m_endPoints.empty())
+                  {
+                     // Finished! break
+                     return;
+                  }
+                  if (m_Graph_EndNode == mainNodeListIndex)
+                  {
+                     // set new end 
+                     SetEndIndex( m_endPoints[0] );                    
+                  }
+               }
+            }            
+         }			
+         // if single end point, then end, if this one is reached or timeout happened.
+			else if ( ( mainNodeListIndex == m_Graph_EndNode || timeout) && !m_CalcAllDistances)
+			{
+            if (m_StoreVectorOrder)
+					MITK_INFO << "Number of Nodes checked: " << m_VectorOrder.size() ;
+				return;
+			}
+		}
+	}
+
+	template <class TInputImageType, class TOutputImageType>
+	void
+		ShortestPathImageFilter<TInputImageType, TOutputImageType>::
+		MakeOutputs()
+	{
+      MITK_INFO << "Make Output";
+
+      if (m_MakeOutputImage)
+      {
+		   OutputImagePointer output0 = this->GetOutput(0);
+		   output0->SetRegions(  this->GetInput()->GetLargestPossibleRegion() );      
+         output0->Allocate();
+         OutputImageIteratorType shortestPathImageIt (output0, output0->GetRequestedRegion());      	
+         // Create ShortestPathImage (Output 0)    
+		   for (shortestPathImageIt.GoToBegin(); !shortestPathImageIt.IsAtEnd(); ++shortestPathImageIt)
+		   {
+			   // First intialize with background color
+			   shortestPathImageIt.Set( BACKGROUND ) ;           
+		   }
+
+         if (!multipleEndPoints) // Only one path was calculated
+         {
+		      for (int i=0; i< m_VectorPath.size(); i++)
+		      {
+			      shortestPathImageIt.SetIndex( m_VectorPath[i] );
+			      shortestPathImageIt.Set( FOREGROUND ) ;
+		      }
+         }
+         else // multiple pathes has been calculated, draw all
+         {
+            for (int i =0; i<m_MultipleVectorPaths.size(); i++)
+            {
+               for (int j=0; j< m_MultipleVectorPaths[i].size(); j++)
+               {
+                  shortestPathImageIt.SetIndex( m_MultipleVectorPaths[i][j] );
+                  shortestPathImageIt.Set( FOREGROUND ) ;
+               }
+            }
+         }
+      }
+	}
+
+
+	template <class TInputImageType, class TOutputImageType>
+   typename ShortestPathImageFilter<TInputImageType, TOutputImageType>::OutputImagePointer
+		ShortestPathImageFilter<TInputImageType, TOutputImageType>::
+		GetVectorOrderImage()
+	{
+      // Create Vector Order Image
+      // Return it
+    
+      OutputImagePointer image = OutputImageType::New();		
+      image->SetRegions(  this->GetInput()->GetLargestPossibleRegion() );
+      image->Allocate();
+      OutputImageIteratorType vectorOrderImageIt  (image, image->GetRequestedRegion());   	
+
+
+      MITK_INFO << "GetVectorOrderImage";
+      for (vectorOrderImageIt.GoToBegin(); !vectorOrderImageIt.IsAtEnd(); ++vectorOrderImageIt)
+      {
+         // First intialize with background color
+         vectorOrderImageIt.Value() =  BACKGROUND ;
+      }
+      for (int i=0; i< m_VectorOrder.size(); i++)
+      {
+         vectorOrderImageIt.SetIndex(NodeToCoord(m_VectorOrder[i]) );
+         vectorOrderImageIt.Set( BACKGROUND+i)  ;
+      }
+		return image;
+	}
+
+
+	template <class TInputImageType, class TOutputImageType>
+	typename ShortestPathImageFilter<TInputImageType, TOutputImageType>::OutputImagePointer
+		ShortestPathImageFilter<TInputImageType, TOutputImageType>::
+		GetDistanceImage()
+	{
+      // Create Distance Image
+      // Return it
+
+      OutputImagePointer image = OutputImageType::New();		
+      image->SetRegions(  this->GetInput()->GetLargestPossibleRegion() );
+      image->Allocate();;
+      OutputImageIteratorType distanceImageIt     (image, image->GetRequestedRegion());
+      // Create Distance Image (Output 1)          
+      NodeNumType myNodeNum;    
+      for (distanceImageIt.GoToBegin(); !distanceImageIt.IsAtEnd(); ++distanceImageIt)
+      {
+         IndexType index = distanceImageIt.GetIndex();
+         myNodeNum = CoordToNode(index); 
+         double newVal = m_Nodes[myNodeNum].distance;
+         distanceImageIt.Set(newVal);     
+      }
+   }
+
+
+ 
+	template <class TInputImageType, class TOutputImageType>
+	std::vector< itk::Index<3> >
+		ShortestPathImageFilter<TInputImageType, TOutputImageType>::
+		GetVectorPath()
+	{
+		return m_VectorPath;
+	}
+
+   template <class TInputImageType, class TOutputImageType>
+	std::vector< std::vector< itk::Index<3> > >
+		ShortestPathImageFilter<TInputImageType, TOutputImageType>::
+		GetMultipleVectorPaths()
+	{
+		return m_MultipleVectorPaths;
+	}
+
+	template <class TInputImageType, class TOutputImageType>
+	void
+		ShortestPathImageFilter<TInputImageType, TOutputImageType>::
+		MakeShortestPathVector()
+	{
+      MITK_INFO << "Make ShortestPath Vec";
+      
+      // single end point
+      if ( !multipleEndPoints )
+      {
+         // fill m_VectorPath with the Shortest Path
+         m_VectorPath.clear();
+
+         // Go backwards from endnote to startnode		
+         NodeNumType prevNode = m_Graph_EndNode;   		
+         while (prevNode != -1) 
+         { 
+            m_VectorPath.push_back( NodeToCoord(prevNode) );           
+
+            if (prevNode == m_Graph_StartNode)
+               break;
+
+            prevNode = m_Nodes[prevNode].prevNode;     
+         }
+
+         // reverse it 
+         std::reverse(m_VectorPath.begin(), m_VectorPath.end() ); 
+      }
+      // Multiple end end points and pathes
+      else
+      {
+         for (int i=0; i<m_endPointsClosed.size(); i++)
+         {
+            m_VectorPath.clear();
+            // Go backwards from endnote to startnode		
+            NodeNumType prevNode = CoordToNode(m_endPointsClosed[i]);   		
+            while (prevNode != -1) 
+            { 
+               m_VectorPath.push_back( NodeToCoord(prevNode) );           
+
+               if (prevNode == m_Graph_StartNode)
+                  break;
+
+               prevNode = m_Nodes[prevNode].prevNode;     
+            }
+
+            // reverse it 
+            std::reverse(m_VectorPath.begin(), m_VectorPath.end() ); 
+            // push_back
+            m_MultipleVectorPaths.push_back( m_VectorPath );
+         }         
+      }
+	}
+
+
+	template <class TInputImageType, class TOutputImageType>
+	void
+		ShortestPathImageFilter<TInputImageType, TOutputImageType>::
+		CleanUp()
+	{      
+      
+
+      m_VectorPath.clear();
+      //TODO: if multiple Path, clear all multiple Paths
+		/*
+		for (NodeNumType i=0; i<m_Graph_NumberOfNodes; i++)
+		{      
+		delete m_Nodes[i];
+		}*/
+
+      // This is properly not a proper cleanup
+      if (m_Nodes)
+		   delete m_Nodes;
+		m_VectorOrder.clear();
+
+	}
+
+	template <class TInputImageType, class TOutputImageType>
+	void
+		ShortestPathImageFilter<TInputImageType, TOutputImageType>::
+		GenerateData()
+	{     
+		// Build Graph
+		InitGraph();
+
+		// Calc Shortest Parth
+		StartShortestPathSearch();
+
+		// Fill Shortest Path
+		MakeShortestPathVector();
+
+		// Make Outputs
+		MakeOutputs();
+	} 
+
+	template <class TInputImageType, class TOutputImageType>
+	void
+		ShortestPathImageFilter<TInputImageType, TOutputImageType>::
+		PrintSelf( std::ostream& os, Indent indent ) const
+	{
+		Superclass::PrintSelf(os,indent);
+
+	}
+
+} /* end namespace itk */
diff --git a/Modules/GraphAlgorithms/itkShortestPathNode.cpp b/Modules/GraphAlgorithms/itkShortestPathNode.cpp
new file mode 100644
index 0000000000..975a975885
--- /dev/null
+++ b/Modules/GraphAlgorithms/itkShortestPathNode.cpp
@@ -0,0 +1,15 @@
+#include "itkShortestPathNode.h"
+
+namespace itk 
+{
+
+//	bool ShortestPathNode::operator<(const ShortestPathNode &a) const
+//	{
+//		return (this->distance < a.distance);
+//	}
+//	bool ShortestPathNode::operator==(const ShortestPathNode &a) const
+//	{
+//		return (this->mainListIndex == a.mainListIndex);
+//	}
+
+}
diff --git a/Modules/GraphAlgorithms/itkShortestPathNode.h b/Modules/GraphAlgorithms/itkShortestPathNode.h
new file mode 100644
index 0000000000..787b03f0a1
--- /dev/null
+++ b/Modules/GraphAlgorithms/itkShortestPathNode.h
@@ -0,0 +1,27 @@
+#ifndef __itkShortestPathNode_h_
+#define __itkShortestPathNode_h_
+
+
+namespace itk 
+{
+	typedef double                 DistanceType; // Type to declare the costs
+    typedef unsigned int           NodeNumType;  // Type for Node Numeration: unsignend int for up to 4.2 billion pixel in 32Bit system
+
+	class ShortestPathNode
+	{
+	public: 
+      DistanceType distance;      // minimal costs from StartPoint to this pixel
+	   DistanceType distAndEst;    // Distance+Estimated Distnace to target
+      NodeNumType prevNode;       // previous node. Important to find the Shortest Path
+      NodeNumType mainListIndex;  // Indexnumber of this node in m_Nodes
+      bool closed; // determines if this node is closes, so its optimal path to startNode is known
+	};
+
+	//bool operator<(const ShortestPathNode &a) const;
+	//bool operator==(const ShortestPathNode &a) const;
+
+}
+
+
+
+#endif