Remove target and origin only

main.cc

@@ -215,12 +215,11 @@ class World
 			} while (!targetIsReached && !open.empty());
 
 			// Remove origin and target
-			if (!discovered.empty()) {
+			if (!discovered.empty() && discovered.front() == origin)
 				discovered.pop_front();
 
-				if (!discovered.empty())
-					discovered.pop_back();
-			}
+			if (!discovered.empty() && discovered.back() == target)
+				discovered.pop_back();
 
 			// Build path
 			if (targetIsReached) {