Feature/abstract gridmap expansion (#21)

* abstract out gridmap based expansion policy expand and node generation

* bugfix identity assert, now checks is_none

include/warthog/constants.h

@@ -37,7 +37,7 @@ struct identity_base
 	        max()) constexpr identity_base(identity_base<Tag, IdType2> alt)
 	    : id(static_cast<IdType>(alt.id))
 	{
-		assert(id == alt.id);
+		assert(id == alt.id || (is_none() && alt.is_none()));
 	}
 	constexpr identity_base(const identity_base&) noexcept = default;
 	constexpr identity_base(identity_base&&) noexcept = default;

include/warthog/search/gridmap_expansion_policy.h

@@ -30,14 +30,10 @@
 namespace warthog::search
 {
 
-class gridmap_expansion_policy : public expansion_policy
+class gridmap_expansion_policy_base : public expansion_policy
 {
 public:
-	gridmap_expansion_policy(domain::gridmap* map, bool manhattan = false);
-	virtual ~gridmap_expansion_policy() { }
-
-	void
-	expand(search_node*, search_problem_instance*) override;
+	gridmap_expansion_policy_base(domain::gridmap* map);
 
 	search_problem_instance
 	get_problem_instance(problem_instance* pi) override;
@@ -55,6 +51,21 @@ class gridmap_expansion_policy : public expansion_policy
 	void
 	print_node(search_node* n, std::ostream& out) override;
 
+	size_t
+	mem() override;
+
+protected:
+	domain::gridmap* map_;
+};
+
+class gridmap_expansion_policy : public gridmap_expansion_policy_base
+{
+public:
+	gridmap_expansion_policy(domain::gridmap* map, bool manhattan = false);
+
+	void
+	expand(search_node*, search_problem_instance*) override;
+
 	search_node*
 	generate_start_node(search_problem_instance* pi) override;
 
@@ -64,8 +75,7 @@ class gridmap_expansion_policy : public expansion_policy
 	size_t
 	mem() override;
 
-private:
-	domain::gridmap* map_;
+protected:
 	bool manhattan_;
 };
 

include/warthog/search/vl_gridmap_expansion_policy.h

@@ -38,21 +38,12 @@
 namespace warthog::search
 {
 
-class vl_gridmap_expansion_policy : public expansion_policy
+class vl_gridmap_expansion_policy_base : public expansion_policy
 {
 public:
-	vl_gridmap_expansion_policy(
+	vl_gridmap_expansion_policy_base(
 	    domain::vl_gridmap* map, util::cost_table& costs);
-	virtual ~vl_gridmap_expansion_policy();
-
-	void
-	expand(search_node*, search_problem_instance*) override;
-
-	size_t
-	mem() override
-	{
-		return expansion_policy::mem() + sizeof(*this) + map_->mem();
-	}
+	virtual ~vl_gridmap_expansion_policy_base();
 
 	search_problem_instance
 	get_problem_instance(problem_instance* pi) override;
@@ -70,15 +61,30 @@ class vl_gridmap_expansion_policy : public expansion_policy
 	void
 	print_node(search_node* n, std::ostream& out) override;
 
+	size_t
+	mem() override;
+
+protected:
+	domain::vl_gridmap* map_;
+	util::cost_table& costs_;
+};
+
+class vl_gridmap_expansion_policy : public vl_gridmap_expansion_policy_base
+{
+public:
+	using vl_gridmap_expansion_policy_base::vl_gridmap_expansion_policy_base;
+
+	void
+	expand(search_node*, search_problem_instance*) override;
+
 	search_node*
 	generate_start_node(search_problem_instance* pi) override;
 
 	search_node*
 	generate_target_node(search_problem_instance* pi) override;
 
-private:
-	domain::vl_gridmap* map_;
-	util::cost_table& costs_;
+	size_t
+	mem() override;
 };
 
 } // namespace warthog::search

src/search/gridmap_expansion_policy.cpp

@@ -5,10 +5,71 @@
 namespace warthog::search
 {
 
+//
+// gridmap_expansion_policy_base
+//
+
+gridmap_expansion_policy_base::gridmap_expansion_policy_base(
+    domain::gridmap* map)
+    : expansion_policy(map->height() * map->width()), map_(map)
+{ }
+
+search_problem_instance
+gridmap_expansion_policy_base::get_problem_instance(problem_instance* pi)
+{
+	assert(pi != nullptr);
+	return convert_problem_instance_to_search(*pi, *map_);
+}
+
+pack_id
+gridmap_expansion_policy_base::get_state(pad_id node_id)
+{
+	return map_->to_unpadded_id(node_id);
+}
+
+pad_id
+gridmap_expansion_policy_base::unget_state(pack_id node_id)
+{
+	return map_->to_padded_id(node_id);
+}
+
+void
+gridmap_expansion_policy_base::get_xy(pack_id node_id, int32_t& x, int32_t& y)
+{
+	uint32_t lx, ly;
+	map_->to_unpadded_xy(node_id, lx, ly);
+	x = lx;
+	y = ly;
+}
+void
+gridmap_expansion_policy_base::get_xy(pad_id node_id, int32_t& x, int32_t& y)
+{
+	uint32_t lx, ly;
+	map_->to_unpadded_xy(node_id, lx, ly);
+	x = lx;
+	y = ly;
+}
+
+void
+gridmap_expansion_policy_base::print_node(search_node* n, std::ostream& out)
+{
+	uint32_t x, y;
+	map_->to_unpadded_xy(n->get_id(), x, y);
+	out << "(" << x << ", " << y << ")...";
+	n->print(out);
+}
+
+size_t
+gridmap_expansion_policy_base::mem()
+{
+	return expansion_policy::mem()
+	    + (sizeof(gridmap_expansion_policy_base) - sizeof(expansion_policy))
+	    + map_->mem();
+}
+
 gridmap_expansion_policy::gridmap_expansion_policy(
     domain::gridmap* map, bool manhattan)
-    : expansion_policy(map->height() * map->width()), map_(map),
-      manhattan_(manhattan)
+    : gridmap_expansion_policy_base(map), manhattan_(manhattan)
 { }
 
 void
@@ -82,51 +143,6 @@ gridmap_expansion_policy::expand(
 	}
 }
 
-search_problem_instance
-gridmap_expansion_policy::get_problem_instance(problem_instance* pi)
-{
-	assert(pi != nullptr);
-	return convert_problem_instance_to_search(*pi, *map_);
-}
-
-pack_id
-gridmap_expansion_policy::get_state(pad_id node_id)
-{
-	return map_->to_unpadded_id(node_id);
-}
-
-pad_id
-gridmap_expansion_policy::unget_state(pack_id node_id)
-{
-	return map_->to_padded_id(node_id);
-}
-
-void
-gridmap_expansion_policy::get_xy(pack_id node_id, int32_t& x, int32_t& y)
-{
-	uint32_t lx, ly;
-	map_->to_unpadded_xy(node_id, lx, ly);
-	x = lx;
-	y = ly;
-}
-void
-gridmap_expansion_policy::get_xy(pad_id node_id, int32_t& x, int32_t& y)
-{
-	uint32_t lx, ly;
-	map_->to_unpadded_xy(node_id, lx, ly);
-	x = lx;
-	y = ly;
-}
-
-void
-gridmap_expansion_policy::print_node(search_node* n, std::ostream& out)
-{
-	uint32_t x, y;
-	map_->to_unpadded_xy(n->get_id(), x, y);
-	out << "(" << x << ", " << y << ")...";
-	n->print(out);
-}
-
 search_node*
 gridmap_expansion_policy::generate_start_node(search_problem_instance* pi)
 {
@@ -148,7 +164,9 @@ gridmap_expansion_policy::generate_target_node(search_problem_instance* pi)
 size_t
 gridmap_expansion_policy::mem()
 {
-	return expansion_policy::mem() + sizeof(*this) + map_->mem();
+	return gridmap_expansion_policy_base::mem()
+	    + (sizeof(gridmap_expansion_policy)
+	       - sizeof(gridmap_expansion_policy_base));
 }
 
 } // warthog::expansion_policy

src/search/vl_gridmap_expansion_policy.cpp

@@ -5,12 +5,67 @@
 namespace warthog::search
 {
 
-vl_gridmap_expansion_policy::vl_gridmap_expansion_policy(
+vl_gridmap_expansion_policy_base::vl_gridmap_expansion_policy_base(
     domain::vl_gridmap* map, util::cost_table& costs)
     : expansion_policy(map->height() * map->width()), map_(map), costs_(costs)
 { }
 
-vl_gridmap_expansion_policy::~vl_gridmap_expansion_policy() { }
+vl_gridmap_expansion_policy_base::~vl_gridmap_expansion_policy_base() { }
+
+size_t
+vl_gridmap_expansion_policy_base::mem()
+{
+	return expansion_policy::mem()
+	    + (sizeof(vl_gridmap_expansion_policy_base) - sizeof(expansion_policy))
+	    + map_->mem();
+}
+
+search_problem_instance
+vl_gridmap_expansion_policy_base::get_problem_instance(problem_instance* pi)
+{
+	assert(pi != nullptr);
+	return convert_problem_instance_to_search(*pi, *map_);
+}
+
+pack_id
+vl_gridmap_expansion_policy_base::get_state(pad_id node_id)
+{
+	return map_->to_unpadded_id(node_id);
+}
+
+pad_id
+vl_gridmap_expansion_policy_base::unget_state(pack_id node_id)
+{
+	return map_->to_padded_id(node_id);
+}
+
+void
+vl_gridmap_expansion_policy_base::get_xy(
+    pack_id node_id, int32_t& x, int32_t& y)
+{
+	uint32_t lx, ly;
+	map_->to_unpadded_xy(node_id, lx, ly);
+	x = lx;
+	y = ly;
+}
+void
+vl_gridmap_expansion_policy_base::get_xy(
+    pad_id node_id, int32_t& x, int32_t& y)
+{
+	uint32_t lx, ly;
+	map_->to_unpadded_xy(node_id, lx, ly);
+	x = lx;
+	y = ly;
+}
+
+void
+vl_gridmap_expansion_policy_base::print_node(search_node* n, std::ostream& out)
+{
+	uint32_t x, y;
+	map_->to_unpadded_xy(n->get_id(), x, y);
+	out << "(" << x << ", " << y << ")...";
+	n->print(out);
+}
 
 void
 vl_gridmap_expansion_policy::expand(
@@ -106,51 +161,6 @@ vl_gridmap_expansion_policy::expand(
 	}
 }
 
-search_problem_instance
-vl_gridmap_expansion_policy::get_problem_instance(problem_instance* pi)
-{
-	assert(pi != nullptr);
-	return convert_problem_instance_to_search(*pi, *map_);
-}
-
-pack_id
-vl_gridmap_expansion_policy::get_state(pad_id node_id)
-{
-	return map_->to_unpadded_id(node_id);
-}
-
-pad_id
-vl_gridmap_expansion_policy::unget_state(pack_id node_id)
-{
-	return map_->to_padded_id(node_id);
-}
-
-void
-vl_gridmap_expansion_policy::get_xy(pack_id node_id, int32_t& x, int32_t& y)
-{
-	uint32_t lx, ly;
-	map_->to_unpadded_xy(node_id, lx, ly);
-	x = lx;
-	y = ly;
-}
-void
-vl_gridmap_expansion_policy::get_xy(pad_id node_id, int32_t& x, int32_t& y)
-{
-	uint32_t lx, ly;
-	map_->to_unpadded_xy(node_id, lx, ly);
-	x = lx;
-	y = ly;
-}
-
-void
-vl_gridmap_expansion_policy::print_node(search_node* n, std::ostream& out)
-{
-	uint32_t x, y;
-	map_->to_unpadded_xy(n->get_id(), x, y);
-	out << "(" << x << ", " << y << ")...";
-	n->print(out);
-}
-
 search_node*
 vl_gridmap_expansion_policy::generate_start_node(search_problem_instance* pi)
 {
@@ -167,4 +177,12 @@ vl_gridmap_expansion_policy::generate_target_node(search_problem_instance* pi)
 	return generate(pi->target_);
 }
 
+size_t
+vl_gridmap_expansion_policy::mem()
+{
+	return vl_gridmap_expansion_policy_base::mem()
+	    + (sizeof(vl_gridmap_expansion_policy)
+	       - sizeof(vl_gridmap_expansion_policy_base));
+}
+
 } // namespace warthog::search