AI now follows the A* algorithm more closely by using a separate priority queue from the open_set.

goap.cpp

@@ -2,6 +2,7 @@
 #include "goap.hpp"
 #include "ai_debug.hpp"
 #include "stats.hpp"
+#include <queue>
 
 namespace ai {
 
@@ -49,7 +50,8 @@ namespace ai {
 
   int distance_to_goal(State from, State to) {
     auto result = from ^ to;
-    return result.count();
+    int count = result.count();
+    return count;
   }
 
   Script reconstruct_path(std::unordered_map<Action, Action>& came_from, Action& current) {
@@ -75,42 +77,51 @@ namespace ai {
     return total_path;
   }
 
-  inline int h(State start, State goal, Action& action) {
-    (void)action; // not sure if cost goes here or on d()
-    return distance_to_goal(start, goal) + action.cost;
+  inline int h(State start, State goal) {
+    return distance_to_goal(start, goal);
   }
 
-  inline int d(State start, State goal, Action& action) {
-    return distance_to_goal(start, goal) + action.cost;
+  inline int d(State start, State goal) {
+    return distance_to_goal(start, goal);
   }
 
-  ActionState find_lowest(std::unordered_map<ActionState, int>& open_set) {
+  using FScorePair = std::pair<int, ActionState>;
+  auto FScorePair_cmp = [](const FScorePair& l, const FScorePair& r) {
+      return l.first < r.first;
+  };
+  using FScoreQueue = std::vector<FScorePair>;
+
+  ActionState find_lowest(std::unordered_map<ActionState, int>& open_set,
+      FScoreQueue& f_scores)
+  {
     check(!open_set.empty(), "open set can't be empty in find_lowest");
-    const ActionState *result = nullptr;
-    int lowest_score = SCORE_MAX;
 
-    for(auto& kv : open_set) {
-      if(kv.second < lowest_score) {
-        lowest_score = kv.second;
-        result = &kv.first;
+    for(auto& [score, astate] : f_scores) {
+      if(open_set.contains(astate)) {
+        return astate;
       }
     }
 
-    return *result;
+    dbc::sentinel("lowest not found!");
   }
 
   ActionPlan plan_actions(std::vector<Action>& actions, State start, State goal) {
     std::unordered_map<ActionState, int> open_set;
-    std::unordered_map<State, bool> closed_set;
     std::unordered_map<Action, Action> came_from;
     std::unordered_map<State, int> g_score;
+    FScoreQueue f_score;
+    std::unordered_map<State, bool> closed_set;
     ActionState current{FINAL_ACTION, start};
 
-    g_score[start] = 0;
-    open_set.insert_or_assign(current, g_score[start] + h(start, goal, current.action));
+    g_score.insert_or_assign(start, 0);
+    f_score.emplace_back(h(start, goal), current);
+    std::push_heap(f_score.begin(), f_score.end(), FScorePair_cmp);
+
+    open_set.insert_or_assign(current, h(start, goal));
 
     while(!open_set.empty()) {
-      current = find_lowest(open_set);
+      // current := the node in openSet having the lowest fScore[] value
+      current = find_lowest(open_set, f_score);
 
       if(is_subset(current.state, goal)) {
         return {true,
@@ -122,30 +133,34 @@ namespace ai {
 
       for(auto& neighbor_action : actions) {
         // calculate the State being current/neighbor
-        if(!neighbor_action.can_effect(current.state)) {
-          continue;
-        }
+        if(!neighbor_action.can_effect(current.state)) continue;
 
         auto neighbor = neighbor_action.apply_effect(current.state);
-        if(closed_set.contains(neighbor)) continue;
+        // if(closed_set.contains(neighbor)) continue;
+
+        int d_score = d(current.state, neighbor) + neighbor_action.cost;
 
-        int d_score = d(current.state, neighbor, current.action);
         int tentative_g_score = g_score[current.state] + d_score;
         int neighbor_g_score = g_score.contains(neighbor) ? g_score[neighbor] : SCORE_MAX;
+
         if(tentative_g_score < neighbor_g_score) {
           came_from.insert_or_assign(neighbor_action, current.action);
 
-          g_score[neighbor] = tentative_g_score;
+          g_score.insert_or_assign(neighbor, tentative_g_score);
           // open_set gets the fScore
           ActionState neighbor_as{neighbor_action, neighbor};
 
-          int score = tentative_g_score + h(neighbor, goal, neighbor_as.action);
+          int score = tentative_g_score + h(neighbor, goal);
           // could maintain lowest here and avoid searching all things
+          f_score.emplace_back(score, neighbor_as);
+          std::push_heap(f_score.begin(), f_score.end(), FScorePair_cmp);
+
+          // this maybe doesn't need score
           open_set.insert_or_assign(neighbor_as, score);
         }
       }
     }
 
-    return {false, reconstruct_path(came_from, current.action)};
+    return {is_subset(current.state, goal), reconstruct_path(came_from, current.action)};
   }
 }