diff --git a/src/pathfind.cpp b/src/pathfind.cpp
index b3db56a8494..7d87d7b5967 100644
--- a/src/pathfind.cpp
+++ b/src/pathfind.cpp
@@ -267,22 +267,34 @@ static void find_routes(const gamemap& map, const unit_map& units,
 	}
 }
 
-paths::dest_vect::const_iterator paths::dest_vect::find(const map_location &loc) const
+static paths::dest_vect::iterator lower_bound(paths::dest_vect &v, const map_location &loc)
 {
-	size_t sz = size(), pos = 0;
+	size_t sz = v.size(), pos = 0;
 	while (sz)
 	{
-		if ((*this)[pos + sz / 2].curr < loc) {
+		if (v[pos + sz / 2].curr < loc) {
 			pos = pos + sz / 2 + 1;
 			sz = sz - sz / 2 - 1;
 		} else sz = sz / 2;
 	}
+	return v.begin() + pos;
+}
 
-	const_iterator i_end = end(), i = begin() + pos;
+paths::dest_vect::const_iterator paths::dest_vect::find(const map_location &loc) const
+{
+	const_iterator i = lower_bound(const_cast<dest_vect &>(*this), loc), i_end = end();
 	if (i != i_end && i->curr != loc) i = i_end;
 	return i;
 }
 
+void paths::dest_vect::insert(const map_location &loc)
+{
+	iterator i = lower_bound(*this, loc), i_end = end();
+	if (i != i_end && i->curr == loc) return;
+	paths::step s = { loc, map_location(), 0 };
+	std::vector<step>::insert(i, s);
+}
+
 /**
  * Returns the path going from the source point (included) to the
  * destination point @a j (excluded).
diff --git a/src/pathfind.hpp b/src/pathfind.hpp
index 846552cb121..49f74d54fc2 100644
--- a/src/pathfind.hpp
+++ b/src/pathfind.hpp
@@ -115,6 +115,7 @@ struct paths
 	{
 		const_iterator find(const map_location &) const;
 		bool contains(const map_location &) const;
+		void insert(const map_location &);
 		std::vector<map_location> get_path(const const_iterator &) const;
 	};
 	dest_vect destinations;