I'm working on a vector field pathfinding algorithm.
So far I have everything working but my distance (potential?) field takes some time to generate (given that I'm generating for a very large grid size).
Testing with 10000*10000, I've gotten the time down to 1.21 seconds (from over a minute).
I need help in improving the algorithm as well as any other feedback.
Below is my algo in calcDistanceGrid
, keep in mind that this is the minimal code for the algorithm and has a lot stripped out.
#include<future>
#include<iostream>
#include<vector>
#include<chrono>
//evil? yes. acceptable for readibility of post? yes.
using namespace std;
#define unvisited -1
#define impassable -2
typedef pair<int, int> iVec2d;
typedef pair<unsigned int, unsigned int> uiVec2d;
typedef pair<float, float> fVec2d;
class distanceFieldGrid
{
private:
vector<vector<int>> distanceGrid;
uiVec2d gridExtents;
fVec2d worldMinExtents;
fVec2d worldMaxExtents;
uiVec2d nodeSize;
void initDistanceGrid()
{
distanceGrid.resize(gridExtents.first, vector<int>(gridExtents.second, -1));
}
void calcDistanceGrid(uiVec2d startGridPos)
{
//starting distance is 0
int dist = 0;
//set starting node's distance
distanceGrid[startGridPos.first][startGridPos.second] = dist;
vector<uiVec2d> currentNeighbours;
currentNeighbours.push_back(startGridPos);
vector<uiVec2d> nextNeighbours;
//as long as there are neighbours
while (!currentNeighbours.empty())
{
dist++;
nextNeighbours.clear();
nextNeighbours.reserve(currentNeighbours.size() * 4);
//for each current neighbour, look for more neighbours and set their distance
for (int i = 0 ; i<currentNeighbours.size(); i++)
{
//add adjacent blocks with an unvisited distance to the nextNeighbours and set their distance
if (gridExtents.second>(currentNeighbours[i].second - 1))
if (distanceGrid[currentNeighbours[i].first][currentNeighbours[i].second - 1]==unvisited)
{
nextNeighbours.emplace_back(std::piecewise_construct, forward_as_tuple(currentNeighbours[i].first), forward_as_tuple(currentNeighbours[i].second - 1));
distanceGrid[currentNeighbours[i].first][currentNeighbours[i].second - 1] = dist;
}
if (gridExtents.second>(currentNeighbours[i].second + 1))
if (distanceGrid[currentNeighbours[i].first][currentNeighbours[i].second + 1] == unvisited)
{
nextNeighbours.emplace_back(std::piecewise_construct, forward_as_tuple(currentNeighbours[i].first), forward_as_tuple(currentNeighbours[i].second + 1));
distanceGrid[currentNeighbours[i].first][currentNeighbours[i].second + 1] = dist;
}
if (gridExtents.first>(currentNeighbours[i].first - 1))
if (distanceGrid[currentNeighbours[i].first - 1][currentNeighbours[i].second] == unvisited)
{
nextNeighbours.emplace_back(std::piecewise_construct, forward_as_tuple(currentNeighbours[i].first - 1), forward_as_tuple(currentNeighbours[i].second));
distanceGrid[currentNeighbours[i].first - 1][currentNeighbours[i].second] = dist;
}
if (gridExtents.first>(currentNeighbours[i].first + 1))
if (distanceGrid[currentNeighbours[i].first + 1][currentNeighbours[i].second] == unvisited)
{
nextNeighbours.emplace_back(std::piecewise_construct, forward_as_tuple(currentNeighbours[i].first + 1), forward_as_tuple(currentNeighbours[i].second));
distanceGrid[currentNeighbours[i].first + 1][currentNeighbours[i].second] = dist;
}
}
currentNeighbours.swap(nextNeighbours);
}
}
public:
void printDistanceGrid()
{
cout << "Distance grid (" << gridExtents.first << 'x' << gridExtents.second << ")\n: ";
for (int i = 0; i<distanceGrid.size(); i++)
{
for (int j = 0; j<distanceGrid[i].size(); j++)
cout << distanceGrid[i][j] << '\t';
cout << endl;
}
}
distanceFieldGrid(uiVec2d nodeSz, fVec2d worldMinExt, fVec2d worldMaxExt)
{
//store the world size/bounds and indicated node size
worldMinExtents = worldMinExt;
worldMaxExtents = worldMaxExt;
nodeSize = nodeSz;
//get the size of the world and divide by block size to get grid extents (ie max x and y of the grid)
gridExtents.first = static_cast<int>(worldMaxExt.first - worldMinExt.first) / nodeSz.first;
gridExtents.second = static_cast<int>(worldMaxExt.second - worldMinExt.second) / nodeSz.second;
cout << "grid Extents:"<<gridExtents.first << 'x' << gridExtents.second << endl;
initDistanceGrid();
}
void doPathfind(uiVec2d startGridPos)
{
//for now just calculate distance field and time it
chrono::time_point<chrono::system_clock> before = chrono::system_clock::now();
calcDistanceGrid(startGridPos);
chrono::time_point<chrono::system_clock> after = chrono::system_clock::now();
chrono::duration<double> dur = after - before;
cout << "Distance grid calculation completed with size (" << gridExtents.first << 'x' << gridExtents.second <<") in:" << dur.count() << endl;
}
//sets a vector of possitions to the indicated passability
void setPassable(vector<uiVec2d> gridPos, bool isPassable)
{
for (auto pos : gridPos)
{
distanceGrid[pos.second][pos.first] = (isPassable ? unvisited : impassable);
}
}
};
int main()
{
{
//a big grid with a node size of 1x1, 0,0 as min extants and 10000x10000 as max extents (
distanceFieldGrid bigGrid(make_pair(1, 1), make_pair(0.0f, 0.0f), make_pair(10000.0f, 10000.0f));
//create a big grid to test performance
bigGrid.setPassable(vector<uiVec2d>({ make_pair(3,4), make_pair(2, 4) ,make_pair(1,4) ,make_pair(1, 3) ,make_pair(1, 2),make_pair(1, 1) ,make_pair(2, 1) ,
make_pair(3, 1),make_pair(8, 10),make_pair(9, 9),make_pair(8, 9) }), false);
bigGrid.doPathfind(make_pair(3, 3));
}
{
//a smaller grid to test correctness
distanceFieldGrid smallGrid(make_pair(1, 1), make_pair(0.0f, 0.0f), make_pair(50.0f, 50.0f));
smallGrid.setPassable(vector<uiVec2d>({ make_pair(3,4), make_pair(2, 4) ,make_pair(1,4) ,make_pair(1, 3) ,make_pair(1, 2),make_pair(1, 1) ,make_pair(2, 1) ,
make_pair(3, 1),make_pair(8, 10),make_pair(9, 9),make_pair(8, 9),make_pair(8, 11),make_pair(8, 12),make_pair(8, 13)
,make_pair(8, 14) ,make_pair(8, 17) ,make_pair(7, 17) ,make_pair(6, 17) ,make_pair(5, 16) ,make_pair(8, 16) ,make_pair(5, 15) }), false);
smallGrid.doPathfind(make_pair(5, 3));
smallGrid.printDistanceGrid();
}
cin.get();
return 0;
}
A rundown of the algorithm:
The intent of the algorithm is to generate a distance field where each node/block is allocated a path distance to the start position. This is to later generate a vector field to the start position.
Nodes with a distance of -2 are impassable/blocked. Nodes are defaulted to a distance of -1 representing 'unvisited'. The algorithm starts at the start position and then loops through each unvisited, unblocked neighbor. It adds these neighbors to a vector to loop through again for the same process and sets each of the neighbor's distances.
it both checks and sets the distance at each of the neighboring nodes to ensure that the same neighbor isn't checked more than once, which would cause the neighbor vector to contain almost every node by the end of the algorithm.
I first thought to parallelize the algorithm, but it's relatively tightly coupled with reads and writes to the distance grid multiple times on each iteration.
previously I set the distance outside of the loop and used std::find
to make sure it wasn't being checked already, but this was too slow.
EDIT2: I appreciate the feedback received so far. Perhaps I'm going about this the wrong way.
The code shown isn't meant to be perfect, it's only meant to show calcDistanceGrid
with an example of running it in such a way that I could be suggested on improvements to the actual algorithm itself and not so much the code outside of the function.
I definitely will use constexpr
instead of the #define
s and not use both pair
and forward_as_tuple
in the actual implementation. and as far as main using setPassable
with a vector and other such comments, the plan for the system is to have a parent grid that indicates if nodes are passable, and the distance grid will initialize from that. That way multiple distance grids can use the same 'is passable' grid.
Thank you so much for the feedback so far!