A barely working GOAP now, but need to confirm it's on par with other libraries.

tests/goap.cpp

@@ -66,6 +66,7 @@ template<> struct std::hash<Action> {
   }
 };
 
+const Action FINAL_ACTION{"END", SCORE_MAX, {}, {}};
 
 struct ActionState {
   Action action;
@@ -91,22 +92,32 @@ int distance_to_goal(GOAPState& from, GOAPState& to) {
 
 
 AStarPath reconstruct_path(std::unordered_map<Action, Action>& came_from, Action& current) {
+  fmt::println(">> reconstruct path: {}", current.name);
   AStarPath total_path{current};
+  int count = 0;
 
-  while(came_from.contains(current)) {
+  while(came_from.contains(current) && count++ < 10) {
     current = came_from[current];
-    total_path.push_front(current);
+    if(current != FINAL_ACTION) {
+      fmt::println("adding next action: {}", current.name);
+      total_path.push_front(current);
+    }
   }
 
+  fmt::println("Exited reconstruct path.");
   return total_path;
 }
 
 inline int h(GOAPState& start, GOAPState& goal) {
-  return distance_to_goal(start, goal);
+  int result = distance_to_goal(start, goal);
+  std::cout << "h on " << start << " and " << goal << " gives distance " << result << "\n";
+  return result;
 }
 
 inline int d(GOAPState& start, GOAPState& goal) {
-  return distance_to_goal(start, goal);
+  int result = distance_to_goal(start, goal);
+  std::cout << "d on " << start << " and " << goal << " gives distance " << result << "\n";
+  return result;
 }
 
 inline ActionState find_lowest(std::unordered_map<ActionState, int>& open_set) {
@@ -115,26 +126,18 @@ inline ActionState find_lowest(std::unordered_map<ActionState, int>& open_set) {
   int lowest_score = SCORE_MAX;
 
   for(auto [as, score] : open_set) {
+    fmt::println("### find_lowest: action={}, score={}", as.action.name, score);
+
     if(score < lowest_score) {
       lowest_score = score;
       result = as;
     }
   }
 
+  fmt::println("<<< found lowest: action={}, score={}", result.action.name, lowest_score);
   return result;
 }
 
-std::optional<Action> first_action(std::vector<Action>& actions, GOAPState& start) {
-  Action start_action;
-
-  for(auto& action : actions) {
-    if(action.can_effect(start)) {
-      return std::make_optional<Action>(action);
-    }
-  }
-
-  return std::nullopt;
-}
 
 // map is the list of possible actions
 // start and goal are two world states
@@ -143,40 +146,50 @@ std::optional<AStarPath> plan_actions(std::vector<Action>& actions, GOAPState& s
   std::unordered_map<Action, Action> came_from;
   std::unordered_map<GOAPState, int> g_score;
 
-  auto start_action = first_action(actions, start);
+  ActionState start_state{FINAL_ACTION, start};
-  dbc::check(start_action != std::nullopt, "no action can start");
-
-  ActionState start_state{*start_action, start};
 
   g_score[start] = 0;
   open_set[start_state] = g_score[start] + h(start, goal);
 
   while(!open_set.empty()) {
+    fmt::println(">>>>>>>>>>>>>>>>>>>>>> TOP OF WHILE");
     auto current = find_lowest(open_set);
 
-    if(current.state == goal) {
+    if(is_subset(current.state, goal)) {
       return std::make_optional<AStarPath>(reconstruct_path(came_from, current.action));
     }
 
     open_set.erase(current);
 
     for(auto& neighbor_action : actions) {
+      fmt::println("^^^ NEXT ACTION {}", neighbor_action.name);
+
       // calculate the GOAPState being current/neighbor
-      if(!neighbor_action.can_effect(current.state)) continue;
+      if(!neighbor_action.can_effect(current.state)) {
+        fmt::println("^^^ SKIP action {}", neighbor_action.name);
+        continue;
+      }
 
       auto neighbor = neighbor_action.apply_effect(current.state);
       int d_score = d(current.state, neighbor);
       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) {
-        // action attached?
+        fmt::println("!!! NEW LOW SCORE::: SETTING {} with PARENT {}, tg_score={}, ng_score={}",
+            neighbor_action.name, current.action.name,
+            tentative_g_score, neighbor_g_score);
+
         came_from[neighbor_action] = current.action;
+
         g_score[neighbor] = tentative_g_score;
         // open_set gets the fScore
         ActionState neighbor_as{neighbor_action, neighbor};
         open_set[neighbor_as] = tentative_g_score + h(neighbor, goal);
       }
+      fmt::println("^^^ END ACTION LOOP");
     }
+
+    fmt::println("<<<<<<<<<<<<<<<<< END OF WHILE");
   }
 
   return std::nullopt;
@@ -214,10 +227,11 @@ TEST_CASE("worldstate works", "[goap]") {
   Action kill_it;
   kill_it.name = "kill_it";
   kill_it.cost = 10;
+  kill_it.preconds[ENEMY_IN_RANGE] = true;
   kill_it.preconds[ENEMY_DEAD] = false;
   kill_it.effects[ENEMY_DEAD] = true;
 
-  REQUIRE(kill_it.can_effect(start));
+  REQUIRE(!kill_it.can_effect(start));
   REQUIRE(kill_it.can_effect(after_move_state));
 
   auto after_kill_state = kill_it.apply_effect(after_move_state);
@@ -251,9 +265,11 @@ TEST_CASE("basic feature tests", "[goap]") {
   Action kill_it;
   kill_it.name = "kill_it";
   kill_it.cost = 10;
+  kill_it.preconds[ENEMY_IN_RANGE] = true;
   kill_it.preconds[ENEMY_DEAD] = false;
   kill_it.effects[ENEMY_DEAD] = true;
 
+  // order seems to matter which is wrong
   actions.push_back(kill_it);
   actions.push_back(move_closer);