Compound queries seem to work

src/examples/queries_test.cpp

@@ -45,3 +45,19 @@ BOOST_AUTO_TEST_CASE(simple_query_test_unsat)
   BOOST_REQUIRE(not q2.next_solution());
 }
 
+BOOST_AUTO_TEST_CASE(and_query_test_sat)
+{
+  PlanState state({Predicate("p","a","b"), Predicate("p","a","c"),
+      Predicate("p", "b", "d"), Predicate("p","d","a"), Predicate("q","a"),
+      Predicate("q", "d")});
+  INFO("state: " << state);
+  SimpleQuery q1(state, Predicate("p", "X", "Y"));
+  SimpleQuery q2(state, Predicate("q", "X"));
+  //NotQuery not1(q2);
+  AndQuery and1(q1, q2);
+  while (and1.next_solution())
+  {
+    INFO(and1.get_solution());
+  }
+}
+

src/queries.cpp

@@ -3,11 +3,13 @@
 namespace imagine_planner
 {
 
+// Simple query methods
+
 bool SimpleQuery::unify(const Predicate& goal)
 {
-  assignment_.clear();
   if (goal.arity() != target_.arity()) return false;
   if (goal.get_name() != target_.get_name()) return false;
+  sigma_ = sigma0_;
   for (int idx = 0; idx < goal.arity(); ++idx)
   {
     const TermWrapper& term1 = target_.get_arguments()[idx];
@@ -16,20 +18,21 @@ bool SimpleQuery::unify(const Predicate& goal)
     {
       if (term1 != term2) return false;
     }
-    else if (const Term* assig = assignment_.get(term1.to_str()))
+    else if (const Term* assig = sigma_.get(term1.to_str()))
     {
       if (*assig != *term2.get_term()) return false;
     }
     else
     {
-      assignment_.put(term1.to_str(), term2);
+      sigma_.put(term1.to_str(), term2);
     }
   }
   return true;
 }
 
-SimpleQuery::SimpleQuery(const PlanState& state, const Predicate& target) :
-  state_(state), target_(target)
+SimpleQuery::SimpleQuery(const PlanState& state, const Predicate& target,
+    const Substitution& sigma0) :
+  state_(state), target_(target), sigma0_(sigma0)
 {
   cursor_ = state_.begin();
 }
@@ -54,5 +57,52 @@ bool SimpleQuery::next_solution()
   return false;
 }
 
+void SimpleQuery::reset(const Substitution& sigma0)
+{
+  sigma0_ = sigma0;
+  cursor_ = state_.begin();
+}
+
+// NotQuery methods
+
+NotQuery::NotQuery(Query& q) : query_(q), done_(false) {}
+
+bool NotQuery::next_solution()
+{
+  if (done_) return false;
+  done_ = true;
+  return not query_.next_solution();
+}
+
+void NotQuery::reset(const Substitution& sigma0)
+{
+  query_.reset(sigma0);
+  done_ = false;
+}
+
+// AndQuery methods
+
+AndQuery::AndQuery(Query& q1, Query& q2) :
+  q1_(q1), q2_(q2), first_fixed_(false) {}
+
+bool AndQuery::next_solution()
+{
+  if (first_fixed_ and q2_.next_solution())
+  {
+    return true;
+  }
+  first_fixed_ = false;
+  while (q1_.next_solution())
+  {
+    q2_.reset(q1_.get_solution());
+    if (q2_.next_solution())
+    {
+      first_fixed_ = true;
+      return true;
+    }
+  }
+  return false;
+}
+
 }
 

src/queries.h

@@ -13,7 +13,12 @@ class Query
 
     virtual bool next_solution() =0;
 
-    virtual const Substitution& get_solution() const=0;
+    virtual const Substitution& get_solution() const =0;
+
+    virtual const Substitution& get_sigma0() const =0;
+
+    virtual void reset(const Substitution& sigma0) =0;
+
 };
 
 class SimpleQuery : public Query
@@ -21,24 +26,76 @@ class SimpleQuery : public Query
   private:
     const PlanState& state_;
     Predicate target_;
-    Substitution assignment_;
+    Substitution sigma0_, sigma_;
     PlanState::CIter cursor_;
 
     bool unify(const Predicate& goal);
     
   public:
-    SimpleQuery(const PlanState& state, const Predicate& target);
+    SimpleQuery(const PlanState& state, const Predicate& target,
+        const Substitution& sigma0=Substitution());
 
     virtual bool next_solution() override;
 
-    const Substitution& get_solution() const { return assignment_; }
+    virtual const Substitution& get_solution() const override { return sigma_; }
+
+    virtual const Substitution& get_sigma0() const override { return sigma0_; }
+
+    virtual void reset(const Substitution& sigma0) override;
 
 };
 
 class NotQuery : public Query
 {
   private:
-    Query q_;
+    Query& query_;
+    bool done_;
+
+  public:
+    NotQuery(Query& q);
+
+    virtual bool next_solution() override;
+
+    virtual const Substitution& get_solution() const override
+    {
+      return query_.get_sigma0();
+    }
+
+    virtual const Substitution& get_sigma0() const override
+    {
+      return query_.get_sigma0();
+    }
+
+    virtual void reset(const Substitution& sigma0) override;
+};
+
+class AndQuery : public Query
+{
+  private:
+    Query& q1_;
+    Query& q2_;
+    bool first_fixed_;
+
+  public:
+    AndQuery(Query& q1, Query& q2);
+
+    virtual bool next_solution() override;
+
+    virtual const Substitution& get_solution() const override
+    {
+      return q2_.get_solution();
+    }
+
+    virtual const Substitution& get_sigma0() const override
+    {
+      return q1_.get_sigma0();
+    }
+
+    virtual void reset(const Substitution& sigma0) override
+    {
+      q1_.reset(sigma0);
+    }
+
 };
 
 } /* end namespace imagine-planner */