X-Git-Url: http://git.scottworley.com/slidingtile/blobdiff_plain/ab51c07a99c1927c95a123e6912925adba83714f..8e3c9cff3a9dbe44519dcea244d528809cc45821:/sliding_tile_lib.cc

diff --git a/sliding_tile_lib.cc b/sliding_tile_lib.cc
index 9ac0217..5a4114b 100644
--- a/sliding_tile_lib.cc
+++ b/sliding_tile_lib.cc
@@ -2,6 +2,7 @@
 
 #include <cstdlib>
 #include <istream>
+#include <map>
 #include <memory>
 #include <ostream>
 #include <queue>
@@ -188,7 +189,7 @@ std::ostream& operator<<(std::ostream& os, const Step& step) {
   return os;
 }
 
-std::shared_ptr<Step> find_path(const std::string& start, const std::string& goal) {
+std::shared_ptr<Step> find_path(const std::string& start, const std::string& goal, unsigned max_frontier) {
   std::istringstream iss_start{start}, iss_goal{goal};
   Board board_start, board_goal;
   iss_start >> board_start;
@@ -199,39 +200,39 @@ std::shared_ptr<Step> find_path(const std::string& start, const std::string& goa
   if (iss_goal.fail() || !iss_goal.eof()) {
     throw std::runtime_error("Could not parse the goal board: " + goal);
   }
-  return find_path(board_start, board_goal);
+  return find_path(board_start, board_goal, max_frontier);
 }
 
-std::shared_ptr<Step> find_path(const Board& start, const Board& goal) {
+std::shared_ptr<Step> find_path(const Board& start, const Board& goal, unsigned max_frontier) {
   InvertedBoard invgoal = goal.invert();
-  auto heap_greater = [invgoal](const std::shared_ptr<Step>& a, const std::shared_ptr<Step>& b) {
-    return a->cost(invgoal) > b->cost(invgoal);
-  };
-  std::priority_queue<std::shared_ptr<Step>,
-                      std::vector<std::shared_ptr<Step>>,
-                      decltype(heap_greater)> todo(heap_greater);
+  std::multimap<int, std::shared_ptr<Step>> todo;
   std::set<Board> seen;
 
-  seen.emplace(start);
-  todo.push(std::make_shared<Step>(start, nullptr));
+  seen.insert(start);
+  auto start_step = std::make_shared<Step>(start, nullptr);
+  todo.emplace(start_step->cost(invgoal), start_step);
   while (!todo.empty()) {
-    if (todo.top()->board == goal) {
-      return todo.top();
+    auto cur = todo.begin()->second;
+    todo.erase(todo.begin());
+    if (cur->board == goal) {
+      return cur;
     }
-    std::vector<std::shared_ptr<Step>> successors = todo.top()->successors(todo.top());
+    std::vector<std::shared_ptr<Step>> successors = cur->successors(cur);
     for (const std::shared_ptr<Step>& s : successors) {
       if (seen.find(s->board) == seen.end()) {
-        seen.emplace(s->board);
-        todo.push(s);
+        seen.insert(s->board);
+        todo.emplace(s->cost(invgoal), s);
         if (seen.size() % 10000 == 0) {
           std::cerr << "Examined " << seen.size() << " boards.  Tracking "
                     << Step::count << " steps.  " << todo.size()
                     << " waiting.  Considering paths of length "
-                    << todo.top()->cost(invgoal) << std::endl;
+                    << cur->cost(invgoal) << std::endl;
         }
       }
     }
-    todo.pop();
+    while (todo.size() > max_frontier) {
+      todo.erase(--todo.end());
+    }
   }
   throw std::runtime_error("No path from start to goal");
 }