GOAP is structured but not working yet.

tests/goap.cpp

@@ -6,20 +6,92 @@
 #include "matrix.hpp"
 #include "components.hpp"
 #include <bitset>
+#include <limits>
 
 using namespace dbc;
 using namespace components;
 
-using AStarPath = std::deque<Point>;
+constexpr const int SCORE_MAX = std::numeric_limits<int>::max();
 
-void update_map(Matrix& map, std::deque<Point>& total_path) {
-  for(auto &point : total_path) {
-    map[point.y][point.x] = 10;
+enum StateNames {
+  ENEMY_IN_RANGE,
+  ENEMY_DEAD,
+  STATE_MAX
+};
+
+using GOAPState = std::bitset<STATE_MAX>;
+
+bool is_subset(GOAPState& source, GOAPState& target) {
+
+  GOAPState result = source & target;
+
+  std::cout << "IS_SUBSET: source: " << source << " target: " << target << " result: " << result << " is it? " << (result == target) << std::endl;
+  return result == target;
+}
+
+struct Action {
+  std::string name;
+  int cost = 0;
+
+  std::unordered_map<StateNames, bool> preconds;
+  std::unordered_map<StateNames, bool> effects;
+
+  bool can_effect(GOAPState& state) {
+    for(auto [name, setting] : preconds) {
+      if(state[name] != setting) return false;
+    }
+
+    return true;
+  }
+
+  GOAPState apply_effect(GOAPState& state) {
+    // RCR SUGGEST: state = (state & ~write_mask) | effect
+    auto state_cp = state;
+
+    for(auto [name, setting] : effects) {
+      state_cp[name] = setting;
+    }
+
+    return state_cp;
   }
+
+  bool operator==(const Action& other) const {
+    return other.name == name;
+  }
+};
+
+template<> struct std::hash<Action> {
+  size_t operator()(const Action& p) const {
+    return std::hash<std::string>{}(p.name);
+  }
+};
+
+
+struct ActionState {
+  Action action;
+  GOAPState state;
+
+  bool operator==(const ActionState& other) const {
+    return other.action == action && other.state == state;
+  }
+};
+
+template<> struct std::hash<ActionState> {
+  size_t operator()(const ActionState& p) const {
+      return std::hash<Action>{}(p.action) ^ std::hash<GOAPState>{}(p.state);
+  }
+};
+
+using AStarPath = std::deque<Action>;
+
+int distance_to_goal(GOAPState& from, GOAPState& to) {
+  auto result = from ^ to;
+  return result.count();
 }
 
-AStarPath reconstruct_path(std::unordered_map<Point, Point>& came_from, Point current) {
-  std::deque<Point> total_path{current};
+
+AStarPath reconstruct_path(std::unordered_map<Action, Action>& came_from, Action& current) {
+  AStarPath total_path{current};
 
   while(came_from.contains(current)) {
     current = came_from[current];
@@ -29,61 +101,80 @@ AStarPath reconstruct_path(std::unordered_map<Point, Point>& came_from, Point cu
   return total_path;
 }
 
-inline h(Point from, Point to) {
-  return std::hypot(float(from.x) - float(to.x),
-      float(from.y) - float(to.y));
+inline int h(GOAPState& start, GOAPState& goal) {
+  return distance_to_goal(start, goal);
 }
 
-inline d(Point current, Point neighbor) {
-  return std::hypot(float(current.x) - float(neighbor.x),
-      float(current.y) - float(neighbor.y));
+inline int d(GOAPState& start, GOAPState& goal) {
+  return distance_to_goal(start, goal);
 }
 
-
-inline Point find_lowest(std::unordered_map<Point, float>& open_set) {
+inline ActionState find_lowest(std::unordered_map<ActionState, int>& open_set) {
   dbc::check(!open_set.empty(), "open set can't be empty in find_lowest");
-  Point result;
-  float lowest_score = 10000;
+  ActionState result;
+  int lowest_score = SCORE_MAX;
 
-  for(auto [point, score] : open_set) {
+  for(auto [as, score] : open_set) {
     if(score < lowest_score) {
       lowest_score = score;
-      result = point;
+      result = as;
     }
   }
 
   return result;
 }
 
+std::optional<Action> first_action(std::vector<Action>& actions, GOAPState& start) {
+  Action start_action;
 
-std::optional<AStarPath> path_to_player(Matrix& map, Point start, Point goal) {
-  std::unordered_map<Point, float> open_set;
-  std::unordered_map<Point, Point> came_from;
-  std::unordered_map<Point, float> g_score;
-  g_score[start] = 0;
+  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
+std::optional<AStarPath> plan_actions(std::vector<Action>& actions, GOAPState& start, GOAPState& goal) {
+  std::unordered_map<ActionState, int> open_set;
+  std::unordered_map<Action, Action> came_from;
+  std::unordered_map<GOAPState, int> g_score;
 
-  open_set[start] = g_score[start] + h(start, goal);
+  auto start_action = first_action(actions, 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()) {
     auto current = find_lowest(open_set);
 
-    if(current == goal) {
-      return std::make_optional<AStarPath>(reconstruct_path(came_from, current));
+    if(current.state == goal) {
+      return std::make_optional<AStarPath>(reconstruct_path(came_from, current.action));
     }
 
     open_set.erase(current);
 
-    for(matrix::compass it{map, current.x, current.y}; it.next();) {
-      Point neighbor{it.x, it.y};
+    for(auto& neighbor_action : actions) {
+      // calculate the GOAPState being current/neighbor
+      if(!neighbor_action.can_effect(current.state)) continue;
 
-      float d_score = d(current, neighbor) + map[it.y][it.x] * 1000;
-      float tentative_g_score = g_score[current] + d_score;
-      float neighbor_g_score = g_score.contains(neighbor) ? g_score[neighbor] : 10000.0f;
+      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) {
-        came_from[neighbor] = current;
+        // action attached?
+        came_from[neighbor_action] = current.action;
         g_score[neighbor] = tentative_g_score;
         // open_set gets the fScore
-        open_set[neighbor] = tentative_g_score + h(neighbor, goal);
+        ActionState neighbor_as{neighbor_action, neighbor};
+        open_set[neighbor_as] = tentative_g_score + h(neighbor, goal);
       }
     }
   }
@@ -91,20 +182,85 @@ std::optional<AStarPath> path_to_player(Matrix& map, Point start, Point goal) {
   return std::nullopt;
 }
 
+
+TEST_CASE("worldstate works", "[goap]") {
+  GOAPState goal;
+  GOAPState start;
+  std::vector<Action> actions;
+
+  // start off enemy not dead and not in range
+  start[ENEMY_DEAD] = false;
+  start[ENEMY_IN_RANGE] = false;
+
+  // end goal is enemy is dead
+  goal[ENEMY_DEAD] = true;
+
+  Action move_closer;
+  move_closer.name = "move_closer";
+  move_closer.cost = 10;
+  move_closer.preconds[ENEMY_IN_RANGE] = false;
+  move_closer.effects[ENEMY_IN_RANGE] = true;
+
+  REQUIRE(move_closer.can_effect(start));
+  auto after_move_state = move_closer.apply_effect(start);
+  REQUIRE(start[ENEMY_IN_RANGE] == false);
+  REQUIRE(after_move_state[ENEMY_IN_RANGE] == true);
+  REQUIRE(after_move_state[ENEMY_DEAD] == false);
+  // start is clean but after move is dirty
+  REQUIRE(move_closer.can_effect(start));
+  REQUIRE(!move_closer.can_effect(after_move_state));
+  REQUIRE(distance_to_goal(start, after_move_state) == 1);
+
+  Action kill_it;
+  kill_it.name = "kill_it";
+  kill_it.cost = 10;
+  kill_it.preconds[ENEMY_DEAD] = false;
+  kill_it.effects[ENEMY_DEAD] = true;
+
+  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);
+  REQUIRE(!kill_it.can_effect(after_kill_state));
+  REQUIRE(distance_to_goal(after_move_state, after_kill_state) == 1);
+
+  actions.push_back(kill_it);
+  actions.push_back(move_closer);
+
+  REQUIRE(start != goal);
+}
+
 TEST_CASE("basic feature tests", "[goap]") {
-  for(int i = 0; i < 10; i++) {
-    LevelManager levels;
-    GameLevel level = levels.current();
-    auto &map = *level.map;
-
-    auto& player_at = level.world->get<Position>(level.player);
-    // matrix::dump("A* PLAYER", map.walls(), player_at.location.x, player_at.location.y);
-
-    level.world->query<Position, Combat>([&](const auto ent, auto& enemy_at, auto&) {
-      if(ent != level.player) {
-        auto result = path_to_player(map.walls(), enemy_at.location, player_at.location);
-        REQUIRE(result != std::nullopt);
-      }
-    });
+  GOAPState goal;
+  GOAPState start;
+  std::vector<Action> actions;
+
+  // start off enemy not dead and not in range
+  start[ENEMY_DEAD] = false;
+  start[ENEMY_IN_RANGE] = false;
+
+  // end goal is enemy is dead
+  goal[ENEMY_DEAD] = true;
+
+  Action move_closer;
+  move_closer.name = "move_closer";
+  move_closer.cost = 10;
+  move_closer.preconds[ENEMY_IN_RANGE] = false;
+  move_closer.effects[ENEMY_IN_RANGE] = true;
+
+  Action kill_it;
+  kill_it.name = "kill_it";
+  kill_it.cost = 10;
+  kill_it.preconds[ENEMY_DEAD] = false;
+  kill_it.effects[ENEMY_DEAD] = true;
+
+  actions.push_back(kill_it);
+  actions.push_back(move_closer);
+
+  auto result = plan_actions(actions, start, goal);
+  REQUIRE(result != std::nullopt);
+
+  for(auto& action : *result) {
+    fmt::println("ACTION: {}", action.name);
   }
 }