raycaster/tests/goap.cpp

#include <catch2/catch_test_macros.hpp>
#include "dbc.hpp"
#include <iostream>
#include <vector>
#include "levelmanager.hpp"
#include "matrix.hpp"
#include "components.hpp"
#include <bitset>
#include <limits>

using namespace dbc;
using namespace components;

constexpr const int SCORE_MAX = std::numeric_limits<int>::max();
constexpr const size_t STATE_MAX = 16;

using GOAPState = std::bitset<STATE_MAX>;

bool is_subset(GOAPState& source, GOAPState& target) {
  GOAPState result = source & target;
  return result == target;
}

struct Action {
  std::string name;
  int cost = 0;

  std::unordered_map<int, bool> preconds;
  std::unordered_map<int, bool> effects;

  Action(std::string name, int cost) :
    name(name), cost(cost) {}

  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);
  }
};

const Action FINAL_ACTION("END", SCORE_MAX);

struct ActionState {
  Action action;
  GOAPState state;

  ActionState(Action action, GOAPState state) :
    action(action), state(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<Action, Action>& came_from, Action& current) {
  AStarPath total_path{current};
  int count = 0;

  while(came_from.contains(current) && count++ < 10) {
    current = came_from.at(current);
    if(current != FINAL_ACTION) {
      total_path.push_front(current);
    }
  }

  return total_path;
}

inline int h(GOAPState& start, GOAPState& goal) {
  return distance_to_goal(start, goal);
}

inline int d(GOAPState& start, GOAPState& goal) {
  return distance_to_goal(start, goal);
}

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");
  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;
    }
  }

  return *result;
}


// 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;

  ActionState start_state{FINAL_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(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) {
      // calculate the GOAPState being current/neighbor
      if(!neighbor_action.can_effect(current.state)) {
        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) {
        came_from.insert_or_assign(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);
      }
    }
  }

  return std::nullopt;
}


TEST_CASE("worldstate works", "[goap]") {
  enum StateNames {
    ENEMY_IN_RANGE,
    ENEMY_DEAD
  };

  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", 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", 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(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]") {
  enum StateNames {
    ENEMY_IN_RANGE,
    ENEMY_DEAD
  };

  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", 10);
  move_closer.preconds[ENEMY_IN_RANGE] = false;
  move_closer.effects[ENEMY_IN_RANGE] = true;

  Action kill_it("kill_it", 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);

  auto result = plan_actions(actions, start, goal);
  REQUIRE(result != std::nullopt);

  auto state = start;

  for(auto& action : *result) {
    fmt::println("ACTION: {}", action.name);
    state = action.apply_effect(state);
  }

  REQUIRE(state[ENEMY_DEAD]);
}

TEST_CASE("wargame test from cppGOAP", "[goap]") {
  std::vector<Action> actions;

  // Constants for the various states are helpful to keep us from
  // accidentally mistyping a state name.
  enum WarGameStates {
    target_acquired,
    target_lost,
    target_in_warhead_range,
    target_dead
  };

  // Now establish all the possible actions for the action pool
  // In this example we're providing the AI some different FPS actions
  Action spiral("searchSpiral", 5);
  spiral.preconds[target_acquired] = false;
  spiral.preconds[target_lost] = true;
  spiral.effects[target_acquired] = true;
  actions.push_back(spiral);

  Action serpentine("searchSerpentine", 5);
  serpentine.preconds[target_acquired] = false;
  serpentine.preconds[target_lost] = false;
  serpentine.effects[target_acquired] = true;
  actions.push_back(serpentine);

  Action intercept("interceptTarget", 5);
  intercept.preconds[target_acquired] = true;
  intercept.preconds[target_dead] = false;
  intercept.effects[target_in_warhead_range] = true;
  actions.push_back(intercept);

  Action detonateNearTarget("detonateNearTarget", 5);
  detonateNearTarget.preconds[target_in_warhead_range] = true;
  detonateNearTarget.preconds[target_acquired] = true;
  detonateNearTarget.preconds[target_dead] = false;
  detonateNearTarget.effects[target_dead] = true;
  actions.push_back(detonateNearTarget);

  // Here's the initial state...
  GOAPState initial_state;
  initial_state[target_acquired] = false;
  initial_state[target_lost] = true;
  initial_state[target_in_warhead_range] = false;
  initial_state[target_dead] = false;

  // ...and the goal state
  GOAPState goal_target_dead;
  goal_target_dead[target_dead] = true;

  auto result = plan_actions(actions, initial_state, goal_target_dead);
  REQUIRE(result != std::nullopt);

  auto state = initial_state;

  for(auto& action : *result) {
    fmt::println("ACTION: {}", action.name);
    state = action.apply_effect(state);
  }

  REQUIRE(state[target_dead]);
}