diff --git a/libs/util/include/psemek/util/find_path.hpp b/libs/util/include/psemek/util/find_path.hpp
index 04265f93..7fbbe995 100644
--- a/libs/util/include/psemek/util/find_path.hpp
+++ b/libs/util/include/psemek/util/find_path.hpp
@@ -57,7 +57,7 @@ namespace psemek::util
 		void init(Node const & start)
 		{
 			cost[start] = Cost{};
-			priority[start] = cost[start] + heuristic(start);
+			priority[start] = heuristic(start);
 			queue.insert(start);
 		}
 
@@ -100,14 +100,14 @@ namespace psemek::util
 			return cost.contains(end);
 		}
 
-		// Doesn't return the starting node!
-		template <typename Iterator>
-		Iterator path(Node const & start, Node const & end, Iterator it) const
+		// Doesn't include the starting node
+		template <typename Iterator, typename IsStart>
+		Iterator path(IsStart const & is_start, Node const & end, Iterator it) const
 		{
 			Node node = end;
 			while (true)
 			{
-				if (node == start)
+				if (is_start(node))
 					break;
 				*it++ = node;
 				node = previous.at(node);
@@ -115,6 +115,29 @@ namespace psemek::util
 			return it;
 		}
 
+		// Includes the starting node
+		template <typename Iterator, typename IsStart>
+		Iterator path_full(IsStart const & is_start, Node const & end, Iterator it) const
+		{
+			Node node = end;
+			while (true)
+			{
+				*it++ = node;
+				if (is_start(node))
+					break;
+				node = previous.at(node);
+			}
+			return it;
+		}
+
+		// Doesn't include the starting node
+		template <typename Iterator>
+		Iterator path(Node const & start, Node const & end, Iterator it) const
+		{
+			return path([start](Node const & node){ return node == start; }, end, it);
+		}
+
+		// Doesn't include the starting node
 		std::deque<Node> path(Node const & start, Node const & end) const
 		{
 			std::deque<Node> result;