Pathfinder: improve GridNode storage

Before, the GridNodes were stored in vector<vector<vector<T>>>,
and initialized in advance. Putting three vectors inside each other
puts lots of unneccessary stress onto the allocator, costs more memory,
and has worse cache locality than a flat vector<T>.

For larger search distances, an the array getting initialized means
essentially O(distance^3) complexity in both time and memory,
which makes the current path search a joke. In order to really
profit from the dijkstra/A* algorithms, other data structures
need to be used for larger distances.

For shorter distances, a map based GridNode storage may be slow as
it requires lots of levels of indirection, which is bad for things like
cache locality, and an array based storage may be faster.

This commit does:

1. remove the vector<vector<vector<T>>> based GridNodes storage that
	is allocated and initialized in advance and for the whole
	possible area.

2. Add a vector<T> based GridNodes storage that is allocated and
	initialized in advance for the whole possible area.

3. Add a map<P,T> based GridNodes storage whose elements are
	allocated and initialized, when the path search code
	demands it.

4. Add code to decide between approach 2 and 3,
	based on the length of the path.

5. Remove the unused "surfaces" member of the PathGridnode class.
	Setting this isn't as easy anymore for the
	map based GridNodes storage.

1 files changed, 161 insertions, 98 deletions

diff --git a/src/pathfinder.cpp b/src/pathfinder.cpp
index 29a39233d..cfbadd1e5 100644
--- a/src/pathfinder.cpp
+++ b/src/pathfinder.cpp
@@ -1,6 +1,7 @@
 /*
 Minetest
 Copyright (C) 2013 sapier, sapier at gmx dot net
+Copyright (C) 2016 est31, <MTest31@outlook.com>
 
 This program is free software; you can redistribute it and/or modify
 it under the terms of the GNU Lesser General Public License as published by
@@ -121,7 +122,6 @@ public:
 	bool      source;              /**< node is stating position              */
 	int       totalcost;           /**< cost to move here from starting point */
 	v3s16     sourcedir;           /**< origin of movement for current cost   */
-	int       surfaces;            /**< number of surfaces with same x,z value*/
 	v3s16     pos;                 /**< real position of node                 */
 	PathCost directions[4];        /**< cost in different directions          */
 
@@ -130,6 +130,41 @@ public:
 	char      type;                /**< type of node                          */
 };
 
+class Pathfinder;
+
+/** Abstract class to manage the map data */
+class GridNodeContainer {
+public:
+	virtual PathGridnode &access(v3s16 p)=0;
+	virtual ~GridNodeContainer() {}
+protected:
+	Pathfinder *m_pathf;
+
+	void initNode(v3s16 ipos, PathGridnode *p_node);
+};
+
+class ArrayGridNodeContainer : public GridNodeContainer {
+public:
+	virtual ~ArrayGridNodeContainer() {}
+	ArrayGridNodeContainer(Pathfinder *pathf, v3s16 dimensions);
+	virtual PathGridnode &access(v3s16 p);
+private:
+	v3s16 m_dimensions;
+
+	int m_x_stride;
+	int m_y_stride;
+	std::vector<PathGridnode> m_nodes_array;
+};
+
+class MapGridNodeContainer : public GridNodeContainer {
+public:
+	virtual ~MapGridNodeContainer() {}
+	MapGridNodeContainer(Pathfinder *pathf);
+	virtual PathGridnode &access(v3s16 p);
+private:
+	std::map<v3s16, PathGridnode> m_nodes;
+};
+
 /** class doing pathfinding */
 class Pathfinder {
 
@@ -139,6 +174,8 @@ public:
 	 */
 	Pathfinder();
 
+	~Pathfinder();
+
 	/**
 	 * path evaluation function
 	 * @param env environment to look for path
@@ -182,6 +219,12 @@ private:
 	PathGridnode &getIndexElement(v3s16 ipos);
 
 	/**
+	 * Get gridnode at a specific index position
+	 * @return gridnode for index
+	 */
+	PathGridnode &getIdxElem(s16 x, s16 y, s16 z);
+
+	/**
 	 * invert a 3d position
 	 * @param pos 3d position
 	 * @return pos *-1
@@ -280,8 +323,10 @@ private:
 
 	core::aabbox3d<s16> m_limits; /**< position limits in real map coordinates  */
 
-	/** 3d grid containing all map data already collected and analyzed */
-	std::vector<std::vector<std::vector<PathGridnode> > > m_data;
+	/** contains all map data already collected and analyzed.
+		Access it via the getIndexElement/getIdxElem methods. */
+	friend class GridNodeContainer;
+	GridNodeContainer *m_nodes_container;
 
 	ServerEnvironment *m_env;     /**< minetest environment pointer             */
 
@@ -390,7 +435,6 @@ PathGridnode::PathGridnode()
 	source(false),
 	totalcost(-1),
 	sourcedir(v3s16(0, 0, 0)),
-	surfaces(0),
 	pos(v3s16(0, 0, 0)),
 	is_element(false),
 	type('u')
@@ -405,7 +449,6 @@ PathGridnode::PathGridnode(const PathGridnode &b)
 	source(b.source),
 	totalcost(b.totalcost),
 	sourcedir(b.sourcedir),
-	surfaces(b.surfaces),
 	pos(b.pos),
 	is_element(b.is_element),
 	type(b.type)
@@ -426,7 +469,6 @@ PathGridnode &PathGridnode::operator= (const PathGridnode &b)
 	is_element = b.is_element;
 	totalcost  = b.totalcost;
 	sourcedir  = b.sourcedir;
-	surfaces   = b.surfaces;
 	pos        = b.pos;
 	type       = b.type;
 
@@ -474,6 +516,96 @@ void PathGridnode::setCost(v3s16 dir, PathCost cost)
 	}
 }
 
+void GridNodeContainer::initNode(v3s16 ipos, PathGridnode *p_node)
+{
+	PathGridnode &elem = *p_node;
+
+	v3s16 realpos = m_pathf->getRealPos(ipos);
+
+	MapNode current = m_pathf->m_env->getMap().getNodeNoEx(realpos);
+	MapNode below   = m_pathf->m_env->getMap().getNodeNoEx(realpos + v3s16(0, -1, 0));
+
+
+	if ((current.param0 == CONTENT_IGNORE) ||
+			(below.param0 == CONTENT_IGNORE)) {
+		DEBUG_OUT("Pathfinder: " << PPOS(realpos) <<
+			" current or below is invalid element" << std::endl);
+		if (current.param0 == CONTENT_IGNORE) {
+			elem.type = 'i';
+			DEBUG_OUT(PPOS(ipos) << ": " << 'i' << std::endl);
+		}
+		return;
+	}
+
+	//don't add anything if it isn't an air node
+	if ((current.param0 != CONTENT_AIR) ||
+				(below.param0 == CONTENT_AIR )) {
+			DEBUG_OUT("Pathfinder: " << PPOS(realpos)
+				<< " not on surface" << std::endl);
+			if (current.param0 != CONTENT_AIR) {
+				elem.type = 's';
+				DEBUG_OUT(PPOS(ipos) << ": " << 's' << std::endl);
+			} else {
+				elem.type = '-';
+				DEBUG_OUT(PPOS(ipos) << ": " << '-' << std::endl);
+			}
+			return;
+	}
+
+	elem.valid = true;
+	elem.pos   = realpos;
+	elem.type  = 'g';
+	DEBUG_OUT(PPOS(ipos) << ": " << 'a' << std::endl);
+
+	if (m_pathf->m_prefetch) {
+		elem.directions[DIR_XP] = m_pathf->calcCost(realpos, v3s16( 1, 0, 0));
+		elem.directions[DIR_XM] = m_pathf->calcCost(realpos, v3s16(-1, 0, 0));
+		elem.directions[DIR_ZP] = m_pathf->calcCost(realpos, v3s16( 0, 0, 1));
+		elem.directions[DIR_ZM] = m_pathf->calcCost(realpos, v3s16( 0, 0,-1));
+	}
+}
+
+ArrayGridNodeContainer::ArrayGridNodeContainer(Pathfinder *pathf, v3s16 dimensions) :
+	m_x_stride(dimensions.Y * dimensions.Z),
+	m_y_stride(dimensions.Z)
+{
+	m_pathf = pathf;
+
+	m_nodes_array.resize(dimensions.X * dimensions.Y * dimensions.Z);
+	INFO_TARGET << "Pathfinder ArrayGridNodeContainer constructor." << std::endl;
+	for (int x = 0; x < dimensions.X; x++) {
+		for (int y = 0; y < dimensions.Y; y++) {
+			for (int z= 0; z < dimensions.Z; z++) {
+				v3s16 ipos(x, y, z);
+				initNode(ipos, &access(ipos));
+			}
+		}
+	}
+}
+
+PathGridnode &ArrayGridNodeContainer::access(v3s16 p)
+{
+	return m_nodes_array[p.X * m_x_stride + p.Y * m_y_stride + p.Z];
+}
+
+MapGridNodeContainer::MapGridNodeContainer(Pathfinder *pathf)
+{
+	m_pathf = pathf;
+}
+
+PathGridnode &MapGridNodeContainer::access(v3s16 p)
+{
+	std::map<v3s16, PathGridnode>::iterator it = m_nodes.find(p);
+	if (it != m_nodes.end()) {
+		return it->second;
+	}
+	PathGridnode &n = m_nodes[p];
+	initNode(p, &n);
+	return n;
+}
+
+
+
 /******************************************************************************/
 std::vector<v3s16> Pathfinder::getPath(ServerEnvironment *env,
 							v3s16 source,
@@ -531,10 +663,11 @@ std::vector<v3s16> Pathfinder::getPath(ServerEnvironment *env,
 	m_max_index_y = diff.Y;
 	m_max_index_z = diff.Z;
 
-	//build data map
-	if (!buildCostmap()) {
-		ERROR_TARGET << "failed to build costmap" << std::endl;
-		return retval;
+	delete m_nodes_container;
+	if (diff.getLength() > 5) {
+		m_nodes_container = new MapGridNodeContainer(this);
+	} else {
+		m_nodes_container = new ArrayGridNodeContainer(this, diff);
 	}
 #ifdef PATHFINDER_DEBUG
 	printType();
@@ -646,96 +779,20 @@ Pathfinder::Pathfinder() :
 	m_prefetch(true),
 	m_start(0, 0, 0),
 	m_destination(0, 0, 0),
-	m_data(),
+	m_nodes_container(NULL),
 	m_env(0)
 {
 	//intentionaly empty
 }
 
-/******************************************************************************/
-v3s16 Pathfinder::getRealPos(v3s16 ipos)
+Pathfinder::~Pathfinder()
 {
-	return m_limits.MinEdge + ipos;
+	delete m_nodes_container;
 }
-
 /******************************************************************************/
-bool Pathfinder::buildCostmap()
+v3s16 Pathfinder::getRealPos(v3s16 ipos)
 {
-	INFO_TARGET << "Pathfinder build costmap: min="
-		<< PPOS(m_limits.MinEdge) << ", max=" << PPOS(m_limits.MaxEdge) << std::endl;
-	m_data.resize(m_max_index_x);
-	for (int x = 0; x < m_max_index_x; x++) {
-		m_data[x].resize(m_max_index_z);
-		for (int z = 0; z < m_max_index_z; z++) {
-			m_data[x][z].resize(m_max_index_y);
-
-			int surfaces = 0;
-			for (int y = 0; y < m_max_index_y; y++) {
-				v3s16 ipos(x, y, z);
-
-				v3s16 realpos = getRealPos(ipos);
-
-				MapNode current = m_env->getMap().getNodeNoEx(realpos);
-				MapNode below   = m_env->getMap().getNodeNoEx(realpos + v3s16(0, -1, 0));
-
-
-				if ((current.param0 == CONTENT_IGNORE) ||
-						(below.param0 == CONTENT_IGNORE)) {
-					DEBUG_OUT("Pathfinder: " << PPOS(realpos) <<
-							" current or below is invalid element" << std::endl);
-					if (current.param0 == CONTENT_IGNORE) {
-						m_data[x][z][y].type = 'i';
-						DEBUG_OUT(x << "," << y << "," << z << ": " << 'i' << std::endl);
-					}
-					continue;
-				}
-
-				//don't add anything if it isn't an air node
-				if ((current.param0 != CONTENT_AIR) ||
-						(below.param0 == CONTENT_AIR )) {
-						DEBUG_OUT("Pathfinder: " << PPOS(realpos)
-								<< " not on surface" << std::endl);
-						if (current.param0 != CONTENT_AIR) {
-							m_data[x][z][y].type = 's';
-							DEBUG_OUT(x << "," << y << "," << z << ": " << 's' << std::endl);
-						}
-						else {
-							m_data[x][z][y].type   = '-';
-							DEBUG_OUT(x << "," << y << "," << z << ": " << '-' << std::endl);
-						}
-						continue;
-				}
-
-				surfaces++;
-
-				m_data[x][z][y].valid  = true;
-				m_data[x][z][y].pos    = realpos;
-				m_data[x][z][y].type   = 'g';
-				DEBUG_OUT(x << "," << y << "," << z << ": " << 'a' << std::endl);
-
-				if (m_prefetch) {
-				m_data[x][z][y].directions[DIR_XP] =
-											calcCost(realpos,v3s16( 1, 0, 0));
-				m_data[x][z][y].directions[DIR_XM] =
-											calcCost(realpos,v3s16(-1, 0, 0));
-				m_data[x][z][y].directions[DIR_ZP] =
-											calcCost(realpos,v3s16( 0, 0, 1));
-				m_data[x][z][y].directions[DIR_ZM] =
-											calcCost(realpos,v3s16( 0, 0,-1));
-				}
-
-			}
-
-			if (surfaces >= 1 ) {
-				for (int y = 0; y < m_max_index_y; y++) {
-					if (m_data[x][z][y].valid) {
-						m_data[x][z][y].surfaces = surfaces;
-					}
-				}
-			}
-		}
-	}
-	return true;
+	return m_limits.MinEdge + ipos;
 }
 
 /******************************************************************************/
@@ -858,7 +915,13 @@ v3s16 Pathfinder::getIndexPos(v3s16 pos)
 /******************************************************************************/
 PathGridnode &Pathfinder::getIndexElement(v3s16 ipos)
 {
-	return m_data[ipos.X][ipos.Z][ipos.Y];
+	return m_nodes_container->access(ipos);
+}
+
+/******************************************************************************/
+inline PathGridnode &Pathfinder::getIdxElem(s16 x, s16 y, s16 z)
+{
+	return m_nodes_container->access(v3s16(x,y,z));
 }
 
 /******************************************************************************/
@@ -1216,9 +1279,9 @@ void Pathfinder::printCost(PathDirections dir)
 		for (int z = 0; z < m_max_index_z; z++) {
 			std::cout << std::setw(4) << z <<": ";
 			for (int x = 0; x < m_max_index_x; x++) {
-				if (m_data[x][z][y].directions[dir].valid)
+				if (getIdxElem(x, y, z).directions[dir].valid)
 					std::cout << std::setw(4)
-						<< m_data[x][z][y].directions[dir].value;
+						<< getIdxElem(x, y, z).directions[dir].value;
 				else
 					std::cout << std::setw(4) << "-";
 				}
@@ -1247,9 +1310,9 @@ void Pathfinder::printYdir(PathDirections dir)
 		for (int z = 0; z < m_max_index_z; z++) {
 			std::cout << std::setw(4) << z <<": ";
 			for (int x = 0; x < m_max_index_x; x++) {
-				if (m_data[x][z][y].directions[dir].valid)
+				if (getIdxElem(x, y, z).directions[dir].valid)
 					std::cout << std::setw(4)
-						<< m_data[x][z][y].directions[dir].direction;
+						<< getIdxElem(x, y, z).directions[dir].direction;
 				else
 					std::cout << std::setw(4) << "-";
 				}
@@ -1278,7 +1341,7 @@ void Pathfinder::printType()
 		for (int z = 0; z < m_max_index_z; z++) {
 			std::cout << std::setw(3) << z <<": ";
 			for (int x = 0; x < m_max_index_x; x++) {
-				char toshow = m_data[x][z][y].type;
+				char toshow = getIdxElem(x, y, z).type;
 				std::cout << std::setw(3) << toshow;
 			}
 			std::cout << std::endl;
@@ -1307,7 +1370,7 @@ void Pathfinder::printPathLen()
 			for (int z = 0; z < m_max_index_z; z++) {
 				std::cout << std::setw(3) << z <<": ";
 				for (int x = 0; x < m_max_index_x; x++) {
-					std::cout << std::setw(3) << m_data[x][z][y].totalcost;
+					std::cout << std::setw(3) << getIdxElem(x, y, z).totalcost;
 				}
 				std::cout << std::endl;
 			}