Remove origin and target from path and discovered

main.cc

@@ -213,12 +213,18 @@ class World
 
 			} while (!targetIsReached && !open.empty());
 
+			// Remove origin and target
+			discovered.pop_front();
+			discovered.pop_back();
+
 			// Build path
 			if (targetIsReached) {
 				do {
 					path.push_back(current);
 					current = previous[current];
 				} while (current != origin);
+
+				path.pop_front();
 			}
 
 			return targetIsReached;