2D pathfinding with obstacles

mdooligan · 06-03-2013, 05:16 PM

I have this fun project I hack at in my spare time. It's something I've always wanted to figure out, and I'm getting close. It's game oriented, and that's why I've never got around to it, I'm always working on something functional/useful/needed.

Basically we have a grid:

Code:

#define MAPSZ 128 /* arbitrary */
static unsigned char map[MAPSZ][MAPSZ];

If the value of a map item is 0, it is walkable. Anything else is not (or increases the cost to move, but let's stay simple here).

I made a catchall struct to lug the frequently used items around in:

Code:

typedef struct _pg_data {
    GRand *rand; // I like glib

    int wh, ww; // window size
    int bh, bw; // board size
    int cx, cy; // cursor location
    int sx, sy; // source location
    int dx, dy; // dest location

    GList *l_list, *p_list;
    /* etc etc */
} pg_data;

I also made these funny pointers that superceded the map[][] array:

Code:

typedef struct _path_pt ppt;
struct _path_pt {
    int x, y; // it needs to know where it is
    ppt *parent; // and who to go back to
    ppt *child; // and who to go next
    guchar flags; // closed, open, etc
    guchar mapval; // how much crap on the site
    int cost; // etc etc
};
#define PT_OPEN     (1<<2)
#define PT_CLOSED   (1<<3)
#define PT_TESTFILL (1<<4)
#define PT_F5       (1<<5)
#define PT_F6       (1<<6)

and, finally:

Code:

pg_data pg; // our grab bag
static ppt xpt[MAPSZ][MAPSZ]; // and our actual map array

That's the guts of the data structs. Now to explain my approach:

I did a lot of reading on the topic before I ever started coding. I mean I've made grid based programs before, but they never had any kind of proper pathfinding to them. Almost all of the stuff I read points to Dijkanskis algorithm, Astar, etc etc. I'm certainly not any smarter than any of these people, but it seems they overlook the 2 most important questions:

#1. Does a straight line work? If it does, go with it. The code is simple and lightning fast.

#2. Can DEST be reached from SRC at all? We have to eliminate this one next, because why chew on a big grid with no solution. I found the answer to this question in the bucketfill:

Code:

int map_fill_spot ( int x, int y ) {
    int i=0, j=0;
    int target = 0;
    for ( i = -1; i < 2; i++ ) {
        for ( j = -1; j < 2; j++ ) {
            if ( (x+i) >= 0 && (x+i) < pg.bw && (y+j) >=0 && (y+j) < pg.bh && (i||j) ) {
                if ( ! xpt[x+i][y+j].mapval && !(xpt[x+i][y+j].flags & PT_TESTFILL) ) { // walkable
                    if ( pg.dx == (x+i) && pg.dy == (y+j) ) {
                        target++; // BANG! Found it.
                        return target;
                    }
                    xpt[x+i][y+j].flags |= PT_TESTFILL;
                    target = map_fill_spot(x+i,y+j); /* recurse */
                    if ( target )
                        return target;
                }
            }
        }
    }
    return target;
}
int target_is_reachable ( void ) {
    int target = 0;
    int x, y;
    for ( x = 0; x < pg.bw; x++ ) { // clear the grid of testflags
        for ( y = 0; y < pg.bh; y++ ) {
            xpt[x][y].flags &= ~(PT_TESTFILL);
        }
    }
    target = map_fill_spot ( pg.sx, pg.sy );
    if ( target )
        topmsg("target is reachable");
    else
        topmsg("target CANNOT be reached");
    return target;
}

It's simple, and also lightning fast.

So, if straight line does not work, but target_is_reachable(), we need to get a little fancier.

I modified the bucketfill to be a bit smarter. Instead of rambling stupidly all over the map until it finally bumbles upon the target, I made it pick better directions, all the time setting the xpt.parent so that when we find target, we just follow the parent pointers back to source.

It works great. The initial path, what I call type0, is still kinda rambling, and tends to muddle around in areas before finding it's way past a clump of obstacles, but I devised a few simple routines that eliminate unnccessary ppts and straighten simple zigzags (type1), find better/longer straight lines (type 2 and type3), and finally I'm working on type4, to find less obvious shortcuts through a jumble of obstacles.

Now, creating a type0 path can be done in about 2000 usec on a really complicated map where the path has to wind and spiral in a crazy labyrinthine way. Creating a type1 only takes about 10% longer. This is essentially instantanious and I'm left with a good working quick and dirty path. It points in the right direction, and can easily be used to get going. But it's not very smart. It kinda reminds me of a drunken Irishman looking for more beer at 3 in the morning.

Hey, I'm part Irish.

To improve the path to something I would be proud of (type4), the small increases gained cause logarithmically increasing times. Here's an example from a moderately complicated map:

type0: moves:212 pcost:2630 1016usec(4/move)
(it found target in 212 moves, in 1016usec)

type1: moves:153 pcost:1876 1163usec(7/move)
(it eliminated 59 unnecessary moves, and only cost 147usec)

type 2&3: abandoned. type 4 works better and just as slow.

type4: moves:134 pcost:1610 313569usec(2340/move)
(it eliminated another 19 moves, made the path a lot prettier,
but took 334 times as long, per move, to do it, because it
went step by step and iterated through the path every step
of the way. On heavy duty paths, this can be up to 3 seconds.
Obviously this could use some refining, but still, you can see
the diminishing returns happening.)

So far, the conclusion that I've come up with is that I should just keep the quick&dirty type1 path for reference. I can calculate it fast and easy. As we travel the path towards target, we can recalculate and clean up parts of it as needed (ie create short type4 paths for the immediate future, instead of creating 1 really long one). Why plan out a perfect path if things can happen to change it. Obstacles move, target moves, random things pop up.

Any thoughts or ideas? path-0.1 is ready for complete re-write.

ntubski · 06-03-2013, 09:32 PM

Quote:

#1. Does a straight line work? If it does, go with it. The code is simple and lightning fast.

A* will find the straight line path right away if you do it right.

Quote:

#2. Can DEST be reached from SRC at all? We have to eliminate this one next, because why chew on a big grid with no solution. I found the answer to this question in the bucketfill:

In case of unreachable destinations map_fill_spot() will take longer than Dijkstra's algorithm to discover that there is no solution. If there is a solution it may find it faster than Dijkstra's algorithm (depending on which direction the destination is) but it has a high chance of finding a bad solution whereas Dijkstra's always finds the best one.

mdooligan · 06-04-2013, 10:17 AM

Quote:

If there is a solution it may find it faster than Dijkstra's algorithm (depending on which direction the destination is) but it has a high chance of finding a bad solution whereas Dijkstra's always finds the best one.

Yes. That is what I'm running into here. There is so much reduction and extra calculation with the bucketfill trick that I get into diminishing returns very quickly. If the path is fairly short, or very long with not too many alternate routes, it works very well, but on a fairly open grid with well placed obstacles, it picks some pretty dumb routes.

And you're right. Dijkstras is where I'm going to go next: trying to implementing someone else's algorithms and theory. As it stands right now, I understand exactly what is going on in the code. Looking at the samples of pseudo code for Astar and Dijkstras, I'm not completely clear how exactly they work, therefore pushing it all to the side and doing it my own way, more to get my head around the problem than anything. As usual, my way works, but not as well as I would have liked. It's more brute force than finesse. Therefore the multitudinous heavy discussions on this very topic.

And before I implement anything, I like to understand it completely, so I can tweak it to my particular needs perfectly. I have some stolen code in a project or 2 that I don't fully understand, and it makes it kinda blackbox, where I have to adapt my code to it, instead of the other way around, and I don't like that at all.

ntubski · 06-04-2013, 04:49 PM

Quote:

Originally Posted by mdooligan

Looking at the samples of pseudo code for Astar and Dijkstras, I'm not completely clear how exactly they work, therefore pushing it all to the side and doing it my own way, more to get my head around the problem than anything.

As far as I can tell, your code searches all the way southwest first and then backtracks through all the other directions (this is depth-first search). The main difference with Dijkstra's algorithm is that the search order radiates outward from the source (this is breadth-first search). Also, it keeps track of the distance of each node so that the best path will be known when the destination is reached.

A* searches along the straight path first (assuming you use distance to destination as the heuristic, which is the simplest and most obvious thing to do).

mdooligan · 06-05-2013, 11:47 AM

Quote:

As far as I can tell, your code searches all the way southwest first and then backtracks through all the other directions (this is depth-first search). The main difference with Dijkstra's algorithm is that the search order radiates outward from the source (this is breadth-first search). Also, it keeps track of the distance of each node so that the best path will be known when the destination is reached.

The modified version *always* works towards the target, instead of using mindless for() loops, it goes 0-7, 0 being whatever direction the target is, 1&2 are left and right, 3&4 hard left and hard right, 5&6 left behind right behind, and 7 is opposite direction, last resort, all relative to target. I can set a boolean that flips this and makes it either right handed or left handed, and that can make a huge difference in the outcome.

One of the issues I have run into is when the source moves towards a static target. In certain situations, if the path is re-calculated every move, it's like one of those old video games where you can stand in a certain spot, and your pursuer runs backs and forth and cannot make a choice which way to go. One move to the left and the path gets calculated to go right. One move to the right and the path tells him to go left, so the unit gets stuck moving left, then right, left, right, left, right... Infinite loop until target moves or time runs out. It's kind of amusing that I'm dealing with the same issues Atari and Valadon et al had in the 1980s. It can be avoided for the most by only re-calculating every once in a while.

Anyway, I found some sample code and I'm trying figure out how to put it to my purpose. I thought it might be fun to analyze it. I added some printfs to see what's going on.

Code:

/* Example #1: http://www.algolist.com/code/c/Dijkstra's_algorithm */
#define NSZ 7
#define NFN 998
int allselected(int *selected) {
    int i;
    for (i = 0; i < NSZ; i++)
        if (selected[i] == 0)
            return 0;
    return 1;
}
void shortpath(int cost[][NSZ], int *preced, int *distance) {
    int selected[NSZ] = { 0 };
    int current = 0, i, k, dc, smalldist, newdist;
    for (i = 0; i < NSZ; i++)
        distance[i] = NFN;
    selected[current] = 1;
    distance[0] = 0;
    current = 0;
    while (!allselected(selected)) {
        smalldist = NFN;
        dc = distance[current];
        for (i = 0; i < NSZ; i++) {
            if (selected[i] == 0) {
                newdist = dc + cost[current][i];
                if (newdist < distance[i]) {
                    distance[i] = newdist;
                    preced[i] = current;
                }
                if (distance[i] < smalldist) {
                    smalldist = distance[i];
                    k = i;
                }
            }
        }
        current = k;
        printf("[k:%i(%i)]\n", k, smalldist);
        selected[current] = 1;
    }
}
int d_calc( void ) {
    int cost[NSZ][NSZ] = {
        {NFN,   2,   4,   7, NFN,   5, NFN},
        {  2, NFN, NFN,   6,   3, NFN,   8},
        {  4, NFN, NFN, NFN, NFN,   6, NFN},
        {  7,   6, NFN, NFN, NFN,   1,   6},
        {NFN,   3, NFN, NFN, NFN, NFN,   7},
        {  5, NFN,   6,   1, NFN, NFN,   6},
        {NFN,   8, NFN,   6,   7,   6, NFN}
    };

    int i, preced[NSZ] = { 0 }, distance[NSZ];
    shortpath(cost, preced, distance);
    for (i = 0; i < NSZ; i++) {
        printf("d[%i]:%i\tp[%i]:%i\n", i, distance[i], i, preced[i]);
    }
    return 0;
}

And this is the output:

Code:

[k:1(2)]
[k:2(4)]
[k:4(5)]
[k:5(5)]
[k:3(6)]
[k:6(10)]
d[0]:0  p[0]:0
d[1]:2  p[1]:0
d[2]:4  p[2]:0
d[3]:6  p[3]:5
d[4]:5  p[4]:1
d[5]:5  p[5]:0
d[6]:10 p[6]:1

Perhaps I'm being obtuse, but I find myself a bit puzzled.
What's with preced[]? It gets set but never used. That's why I included it in the output.
Where's source and target?
What does the output mean?

ntubski · 06-05-2013, 09:54 PM

Quote:

Originally Posted by mdooligan

Where's source and target?

The source is the 0th node. There is no specific target, this is a version of the algorithm that finds the shortest path to all other nodes. This isn't a very good implementation: such things should be explained in comments. Also, it doesn't use a priority queue which is a significant efficiency loss.

Quote:

What does the output mean?

Code:

[k:Node(Distance)]
d[Node]:Distance  p[Node]:Preceding-Node

Distance means distance from the source node.
Preceding-Node means the previous node on the shortest path from this node back to the source node. Therefore, you can follow the preced array from a target back to the source to find the shortest path.

mdooligan · 06-06-2013, 11:36 AM

Quote:

The source is the 0th node. There is no specific target, this is a version of the algorithm that finds the shortest path to all other nodes. This isn't a very good implementation: such things should be explained in comments. Also, it doesn't use a priority queue which is a significant efficiency loss.

Yes. I thought so. But it is simple, so perhaps a good place to start.

I think I'm getting the idea here. We have arbitrary 7 nodes, that may or may not be connected to each other. Maybe we can show the cost array like this:

Code:

    int cost[NSZ][NSZ] = {
       /* A    B    C    D    E    F    G */
/* A */ {NFN,   2,   3,   7, NFN,   5, NFN},
/* B */ {  2, NFN, NFN,   6,   3, NFN,   8},
/* C */ {  3, NFN, NFN, NFN, NFN,   6, NFN},
/* D */ {  7,   6, NFN, NFN, NFN,   1,   6},
/* E */ {NFN,   3, NFN, NFN, NFN, NFN,   7},
/* F */ {  5, NFN,   6,   1, NFN, NFN,   6},
/* G */ {NFN,   8, NFN,   6,   7,   6, NFN}
    };

A is connected to B, C, D, and F.
B is connected to A, D, E, and G.
C is connected to A and F.
D is connected to A, B, F, and G.
E is connected to B and G.
F is connected to A, C, D, and G.
G is connected to B, D, E, and F.

At this point we could draw a funny looking diagram that might clarify things a bit. Kinda like this:
http://i.ytimg.com/vi/8Ls1RqHCOPw/0.jpg

And there's no reason the cost array needs to be mirrored diagonally like that. You might be able to get to node F from A, but not the other way round (like a one-way door), or it could be more expensive one way than the other, like high wind speed, or fast running water, or a conveyor belt, etc. As matter of fact, the A<->A, B<->B etc fields can have any value, as they are essentially ignored.

And I can see that as the number of nodes increases, it would be good to keep track of things so you don't have to iterate through the whole works every time. That cost array is going to get big and ugly and unmanageable very fast.

Plain Awesome. Thank you for pointing me in the right direction. I have a much fancier version of the algorithm that has node_t, edge_t, and a heap. I'm looking at it right now and it's beginning to make sense.