Distance Field Generation Algorithm

Question

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 #defines 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!

Answer 1

There are some things here to be improved:

1. Order your #includes alphabetically. This will facilitate checking whether all required #includes are there. Also, consider leaving a space between #include and <...> as this tends to be a coding style issue most programmers agree on. Also, you don't seem to use anything from future, so you should remove it.

2. //evil? yes. acceptable for readibility of post? yes. No, I disagree. You're putting your code up for review here, so you should be posting your real code and not some changed variant that will never actually see a compiler. Using using namespace std; or not can mean the differences between subtle bugs that may not manifest until some edge case occurs, and you'd probably want to be told about those in a code review. Also, in all honesty, writing std:: every few lines doesn't change the length of your code much at all.

3. It is discouraged nowadays to use #define for simple numeric constants. You could either use enum class here (or, if you don't have anything >=C++11, enum) or define constants.

4. Again, if you have C++11 or beyond available, you should prefer using to typedef, because it is much more readable and versatile.

5. Being explicit is good, but being too explicit is bad, too. unsigned int can be written as unsigned, which is most commonly used today. Ultimately, though, this is a personal style issue, so don't take this point as a required change.

6. int is not the right type for everything. For example, for (int i = 0; i<distanceGrid.size(); i++) might overflow because distanceGrid.size() might be bigger than int and, in any case, unsigned. The standard offers std::size_t for sizes, so you should make use of it where appropriate.

7. Don't use std::endl because it's horrible. Although it does insert an endline character into the output stream, it also flushes the underlying buffer which is very rarely required (and for that cases there is std::flush) and can seriously harm performance if you're doing a lot of IO. Just write '\n' instead, the underlying system takes care of converting it to the correct end-of-line sequence for the current OS.

8. Use member initialization lists where possible. Lines such as worldMinExtents = worldMinExt; should just be part of the member initialization list of your constructor.

9. Your code violates the Single Responsibility Principle. To be exact, distanceFieldGrid is doing not only its main purpose but also IO, which violates its responsibility. Extract the IO to a helper class/function so that other people can use distanceFieldGrid without getting extremely annoyed at standard output they don't want. The same is true for benchmarking: You shouldn't have a method doPathfind that measures time, but write a separate benchmark class/function, possibly utilizing a benchmark library that allows you to get more reliable results.

10. setPassable should take its first argument by const&. Currently, you're making a copy of gridPos every single time, but don't do anything with it that would change its contents. Taking the parameter by reference to const will remove that copy and thus speed up the code (probably).

11. nextNeighbours.emplace_back(std::piecewise_construct, forward_as_tuple(currentNeighbours[i].first - 1), forward_as_tuple(currentNeighbours[i].second)); is a lot of code that does remarkably little. std::piecewise_construct is very handy in some cases, but this is definitely not one of them. Just write nextNeighbours.emplace_back(currentNeighbours[i].first - 1, currentNeighbours[i].second), or just use push_back (std::pair<unsigned, unsigned> is not exactly what you would call expensive to copy).

12. Don't make assumptions about the container type everybody uses. Instead of taking a std::vector or a std::list or any other container as a function parameter, take a begin and an end iterator instead. Iterators enable the caller to use whatever container he wants while not putting a huge burden on the library implementer to ensure correctness for every available container type.

13. //a smaller grid to test correctness The keyword here is test. You should restructure your project and write actual tests and benchmarks for correctness and performance. The main function is not the right place to do any of those.

14. Since we are talking about restructuring your project: Nearly all of your code should be extracted into a header and an implementation file.

15. vector<uiVec2d>({ make_pair(3,4), make_pair(2, 4), /* many, many more elements */}) is terrible to read and verify. You should have a separate object definition, maybe even a dedicated generator function (in the test class this actually belongs to, of course).

---

You said that your code has a lot stripped out. However, if you really want us to help you, you should restructure your code into multiple smaller units and ask for review of each of these units alone. As is, your code may be big and bulky, but refactoring can help to limit the scope of functionality for certain classes and make effective code review possible.

Answer 2

acceptable for readibility of post? yes.

I'd argue that your post is now less readable. Shorter doesn't mean more readable. At any point in your code we should be able to tell what names are in scope. Without using namespace XXX we usually only have a handful of names. As soon as we use using namespace XXX we flooded our namespace with another one and have to keep those names also in mind.

For example, your dist in calcDistanceGrid could be called distance without problems if you hadn't used using namespace std. But now std::distance is grabbed from its namespace and (possibly) also in scope, which can yield very strange compiler errors if you ever decide to change dist to distance.

---

While we're looking at names: your class, method, member and local variable names don't follow any rules, except that they are all camel case. As long as you have an IDE at hand you won't have any problems, but if you ever come into a situation where you only have a plain old text editor without any additional information, you will have to keep at least two views open to know whether you're dealing with a member, a variable or a method.

---

#define unvisited -1
#define impassable -2

Those two should be in an enum or a (static) constexpr or (static) const.

---

Your currentNeighbours look would be somewhat more readable if you used a C++11 range based for loop:

        for (auto & currentNeighbour : currentNeighbours)
        {
            //add adjacent blocks with an unvisited distance to the nextNeighbours and set their distance
            if (gridExtents.second > (currentNeighbour.second - 1))
                if (distanceGrid[currentNeighbour.first][currentNeighbour.second - 1]==unvisited)
                {
                    nextNeighbours.emplace_back(std::piecewise_construct, forward_as_tuple(currentNeighbour.first), forward_as_tuple(currentNeighbour.second - 1));
                    distanceGrid[currentNeighbour.first][currentNeighbour.second - 1] = dist;
                }

Furthermore, you don't compare int with size_t in that case, which can lead to an infinite loop.

But we only get a little bit more readable because .first and .second are completely meaningless in that context.

You have a clear intent for your std::pair's, but that intent gets lost due to its bland names. It's a nice interface for several functions in the standard library, but you can easily write

template <typename T>
struct Vec2d {
    T x;
    T y;
};

using IntVec2d = Vec2d<int>;
using FloatVec2d = Vec2d<float>;
using UintVec2d = Vec2d<unsigned int>;

yourself.

I have no idea why you use forward_as_tuple together with emplace_back at that point by the way. emplace calls the constructor with the given arguments. However, our constructor will copy currentNeighbour[i].first since it is not an rvalue. I'd just call push_back at that point. We're dealing with primitive types here after all.