Skip to content

Commit

Permalink
Pathfinder: improve GridNode storage
Browse files Browse the repository at this point in the history 
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.
  • Loading branch information
@est31
est31 committed May 1, 2016
1 parent f0de237 commit 9aec701
Showing 1 changed file with 161 additions and 98 deletions.
259 changes: 161 additions & 98 deletions src/pathfinder.cpp
View file
@@ -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
Expand Down Expand Up @@ -121,7 +122,6 @@ class PathGridnode {
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 */

Expand All @@ -130,6 +130,41 @@ class PathGridnode {
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 {

Expand All @@ -139,6 +174,8 @@ class Pathfinder {
*/
Pathfinder();

~Pathfinder();

/**
* path evaluation function
* @param env environment to look for path
Expand Down Expand Up @@ -181,6 +218,12 @@ class Pathfinder {
*/
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
Expand Down Expand Up @@ -280,8 +323,10 @@ class Pathfinder {

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

Expand Down Expand Up @@ -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')
Expand All @@ -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)
Expand All @@ -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;

Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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;
}

/******************************************************************************/
Expand Down Expand Up @@ -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));
}

/******************************************************************************/
Expand Down Expand Up @@ -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) << "-";
}
Expand Down Expand Up @@ -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) << "-";
}
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
}
Expand Down

0 comments on commit 9aec701

Please sign in to comment.