dirty yet working goap

s2ga.hpp

@@ -1,6 +1,7 @@
 #include <random>
 #include <bitset>
 #include <magic_enum/magic_enum.hpp>
+#include <stdio.h>
 #include <type_traits>
 #include <iostream>
 #include <format>
@@ -99,7 +100,7 @@ namespace s2ga
         bool preconds_met(const std::bitset<N>& state) const
         {
             return ((state & positive_preconds) == positive_preconds) && 
-                (state & ~negative_preconds).none();
+                (state & negative_preconds).none();
         }
 
         std::bitset<N> apply(const std::bitset<N>& state) const
@@ -150,8 +151,333 @@ namespace s2ga
     template<typename E>
     requires std::is_enum_v<E>
     using action_list = std::vector<action<E>>;
-}; // namespace s2ga
 
-inline void foo()
+    template<typename E>
+    requires std::is_enum_v<E>
+    struct goal 
     {
+        static constexpr size_t N = magic_enum::enum_count<E>();
+        std::bitset<N> positive_goals;
+        std::bitset<N> negative_goals;
+
+        int distance(const std::bitset<N>& state)
+        {
+            /*
+             *V000111
+             *P111100
+             *&000100
+             *^111000 <- distance 3
+             */
+            /*V000111
+             *P000000
+             *&000000
+             *^000000 <- distance 0
+             */
+            /*V000111
+             *P111111
+             *&000111
+             *^111000 <- distance 3
+             */
+
+            auto positive_bm = (state & positive_goals) ^ positive_goals;
+            
+            /*
+             *V000111 
+             *N111100
+             *&000100 <- distance 1 
+             */
+            /*
+             *V000111 
+             *N000000
+             *&000000 <- distance 0 
+             */
+            auto negative_bm = state & negative_goals;
+
+            return positive_bm.count() + negative_bm.count(); 
         }
+    };
+
+    template<typename E, int MaxDepth = 20>
+    requires std::is_enum_v<E>
+    class genetic_goap
+    {
+        static constexpr size_t N = magic_enum::enum_count<E>();
+    
+        struct genotype 
+        {
+            int distance(const genotype& g)
+            {
+                int d = 0;
+
+                for(int i = 0; i < MaxDepth; ++i)
+                {
+                    if(action_ids[i] != g.action_ids[i])
+                    {
+                        d++;
+                    }
+                }
+
+                return d;
+            }
+
+            std::array<int, MaxDepth> action_ids;
+            float fitness = 9999.0f;
+        };
+
+        void evaluate(genotype& g)
+        {
+            float cost = 0.0f;
+
+            auto state = initial_state;
+
+            int valid_actions = 0;
+
+            int d = 99999;
+
+            for(int i = 0; i < MaxDepth; ++i)
+            {
+                auto aid = g.action_ids[i];
+
+                if(aid < 0)
+                {
+                    continue;
+                }
+
+                if(!actions[aid].preconds_met(state))
+                {
+                    continue;
+                }
+
+                state = actions[aid].apply(state);
+                d = final_goal.distance(state);
+                cost += actions[aid].cost;
+                valid_actions++;
+
+                if(d == 0)
+                {
+                    break;
+                }
+            }
+
+            if(valid_actions == 0)
+            {
+                g.fitness = 9999.0f;
+                return;
+            }
+
+            g.fitness = 9999.0f * d * float(valid_actions) + cost;
+            //std::cout << std::format("{}({})", g.fitness, cost) << std::endl;
+        }
+
+        void evaluate(std::vector<genotype>& g)
+        {
+            for(auto& i: g)
+            {
+                evaluate(i);
+            }
+        }
+
+        std::vector<genotype> generate_population(int size)
+        {
+            std::vector<genotype> population;
+            population.reserve(size);
+
+            for(int i = 0; i < size; ++i)
+            {
+                population.emplace_back();
+                
+                auto& d = population.back();
+                
+                std::generate(std::begin(d.action_ids), std::end(d.action_ids), 
+                [&]()
+                {
+                    return -1;
+                });
+
+                const int ATTEMPTS = 4;
+               
+                auto last_state = initial_state;
+
+                for(int j = 0; j < MaxDepth; ++j)
+                {
+                    bool found = false;
+                    for(int attempt = 0; attempt < ATTEMPTS; attempt++)
+                    {
+                        int act_id = rng.random(0, int(actions.size()) - 1);
+                        auto& act = actions[act_id];
+                        if(act.preconds_met(last_state))
+                        {
+                            last_state = act.apply(last_state);
+                            d.action_ids[j] = act_id;
+                            found = true;
+                            break;
+                        }
+                    }
+
+                    if(!found)
+                    {
+                        d.action_ids[j] = rng.random(-1, int(actions.size()) - 1);
+                    }
+                }
+
+            }
+
+            return population;
+        }
+
+        action_list<E> to_action_list(const genotype& g)
+        {
+            action_list<E> result;
+            auto state = initial_state;
+
+            int d = 9999;
+
+            for(int i = 0; i < MaxDepth; ++i)
+            {
+                auto aid = g.action_ids[i];
+
+                if(aid < 0)
+                {
+                    continue;
+                }
+
+                if(!actions[aid].preconds_met(state))
+                {
+                    continue;
+                }
+
+                state = actions[aid].apply(state);
+                result.emplace_back(actions[aid]);
+                
+                d = final_goal.distance(state);
+
+                if(d == 0)
+                {
+                    break;
+                }
+            }
+
+            return result;
+        }
+
+        std::vector<genotype> elitism(int N, const std::vector<genotype>& p)
+        {
+            std::vector<genotype> result;
+            result.reserve(N);
+
+            for(int i = 0; i < N; ++i)
+            {
+                result.emplace_back(p[i]);
+            }
+
+            return result;
+        }
+
+        const genotype& tornament_selection(const std::vector<genotype>& p, int k = 8)
+        {
+            int id = rng.random(0, int(p.size()) - 1);
+                
+            for(int i = 1; i < k; ++i)
+            {
+                int nid = rng.random(0, int(p.size()) - 1);
+
+                if(p[nid].fitness < p[id].fitness)
+                {
+                    id = nid;
+                }
+            }
+
+            return p[id];
+        }
+
+        genotype crossover(const genotype& a, const genotype& b)
+        {
+            genotype result;
+
+
+            int split_index = rng.random(0, MaxDepth - 1);
+            for(int i = 0; i < MaxDepth; ++i)
+            {
+                bool from_a = i < split_index;
+                result.action_ids[i] = from_a ? a.action_ids[i] : b.action_ids[i];
+            }
+
+            return result;
+        }
+
+        void mutate(genotype& g)
+        {
+            int index = rng.random(0, MaxDepth - 1);
+            g.action_ids[index] = rng.random(-1, int(actions.size() - 1));
+        }
+    public:
+        action_list<E> plan(int population_size, int max_attempts)
+        {
+            auto population = generate_population(population_size);
+
+            int attempts = 0;
+
+
+            float last_f = 9999999.0f;
+
+            while(attempts < max_attempts)
+            {
+                evaluate(population);
+                std::sort(population.begin(), population.end(), 
+                        [](const genotype& a, const genotype& b)
+                        {
+                            return a.fitness < b.fitness;
+                        });
+
+                if(population[0].fitness < last_f)
+                {
+                    last_f = population[0].fitness;
+                    attempts = 0;
+                }
+                else 
+                {
+                    attempts++;
+                }
+
+                auto new_pop = elitism(3, population);
+
+                while(new_pop.size() < population.size())
+                {
+                    auto solution = crossover(tornament_selection(population), 
+                            tornament_selection(population));
+
+                    if(rng.random(0, 100) <= 5)
+                    {
+                        mutate(solution);
+                    }
+
+                    new_pop.emplace_back(solution);
+                }
+
+                population = new_pop;
+            }
+
+            return to_action_list(population[0]);
+        }
+
+        void set_possible_actions(const action_list<E>& list)
+        {
+            actions = list;
+        }
+
+        void set_initial_state(const std::bitset<N>& state)
+        {
+            initial_state = state;
+        }
+
+        void set_goal(const goal<E>& goal)
+        {
+            final_goal = goal;
+        }
+    private:
+        action_list<E> actions;
+        std::bitset<N> initial_state;
+        goal<E> final_goal;
+        lehmer64 rng;
+    };
+}; // namespace s2ga

tests/test.cpp

@@ -23,6 +23,7 @@ TEST_CASE("(dummy)", "[test]")
         HAS_TOOLS,
         HAS_FABRIC,
         HAS_ANIMAL_SKIN,
+        HAS_RAW_FOOD,
         IS_WET,
         HAS_FIRE,
         IS_RAINING     
@@ -32,24 +33,30 @@ TEST_CASE("(dummy)", "[test]")
     {
         {
             .name = "COLLECT FRUITS",
-            .cost = 2.0f,
+            .cost = 20.0f,
             .positive_effects = bm<State>(HAS_FOOD),
             .negative_preconds = bm<State>(IS_RAINING),
         },
         {
-            .name = "COLLECT FRUITS",
+            .name = "EAT",
-            .cost = 2.0f,
+            .cost = 0.1f,
-            .positive_effects = bm<State>(HAS_FOOD),
+            .negative_effects = bm<State>(HUNGRY, HAS_FOOD),
-            .negative_preconds = bm<State>(IS_RAINING),
+            .positive_preconds = bm<State>(HAS_FOOD)
         },
         {
-            .name = "MAKE FISH TRAP",
+            .name = "TIME TO FISH",
             .cost = 0.5f,
-            .positive_effects = bm<State>(HAS_FOOD),
+            .positive_effects = bm<State>(HAS_RAW_FOOD),
-            .negative_effects = bm<State>(HAS_WOOD),
             .positive_preconds = bm<State>(HAS_TOOLS),
             .negative_preconds = bm<State>(IS_WET), 
         },
+        {
+            .name = "COOK",
+            .cost = 0.25f,
+            .positive_effects = bm<State>(HAS_FOOD),
+            .negative_effects = bm<State>(HAS_RAW_FOOD),
+            .positive_preconds = bm<State>(HAS_RAW_FOOD)
+        },
         {
             .name = "BUILD HOUSE",
             .cost = 8.0f,
@@ -76,8 +83,7 @@ TEST_CASE("(dummy)", "[test]")
         {
             .name = "HUNT ANIMAL",
             .cost = 6.0f,
-            .positive_effects = bm<State>(HAS_ANIMAL_SKIN, HAS_FOOD),
+            .positive_effects = bm<State>(HAS_ANIMAL_SKIN, HAS_RAW_FOOD),
-            .negative_effects = bm<State>(HUNGRY),
             .positive_preconds = bm<State>(HAS_TOOLS),
             .negative_preconds = bm<State>(IS_RAINING),
         },
@@ -99,12 +105,56 @@ TEST_CASE("(dummy)", "[test]")
             .cost = 5.0f,
             .negative_effects = bm<State>(IS_RAINING),
             .positive_preconds = bm<State>(IS_RAINING),
+        },
+        {
+            .name = "PROCESS ANIMAL SKIN",
+            .cost = 6.0f,
+            .positive_effects = bm<State>(HAS_FABRIC),
+            .negative_effects = bm<State>(HAS_ANIMAL_SKIN),
+            .positive_preconds = bm<State>(HAS_ANIMAL_SKIN)
+        },
+    };
+
+    BENCHMARK("GOAPERS")
+    {
+        goal<State> goal;
+        goal.negative_goals = bm<State>(HUNGRY);
+        goal.positive_goals = bm<State>(HAS_HOUSE);
+
+        genetic_goap<State> goap;
+        goap.set_initial_state(bm<State>(HUNGRY, IS_WET, IS_RAINING));
+        goap.set_possible_actions(actions);
+        goap.set_goal(goal);
+
+        for(int i = 0; i < 32; ++i)
+        {
+            auto plan = goap.plan(100, 5);
+            (void)plan;
         }
     };
 
-    for(auto& action: actions)
+    goal<State> goal;
+    goal.negative_goals = bm<State>(HUNGRY);
+    goal.positive_goals = bm<State>(HAS_HOUSE);
+
+    genetic_goap<State> goap;
+    goap.set_initial_state(bm<State>(HUNGRY, IS_WET, IS_RAINING));
+    goap.set_possible_actions(actions);
+    goap.set_goal(goal);
+    auto plan = goap.plan(100, 5);
+
+    float cost = 0.0f;
+    for(auto& i: plan)
     {
-        action.print();
+        i.print();
+        cost += i.cost;
+    }
+
+    std::cout << std::format("[Total Cost: {}]", cost) << std::endl;
+
+    if(plan.size() == 0)
+    {
+        std::cout << "SAD" << std::endl;
     }
 }