Fix approximative origin and target removal

main.cc

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