Pathfinding algorithm

Started by
6 comments, last by Matthewj234 11 years, 9 months ago
So, I have been trying to sort out pathfinding for my game, but it isnt working too well. I have tried A* and it didnt work, so I found this algorithm:

1: Origin is now the current square.
2: select an adjacent square who's still set to 0.
if there's none, goto 5.
3: give the selected square a value of the current square +1.
if the selected square is the target, goto 7
4: push selected square onto a list of unfinished squares. goto 2.
5: shift the first value from the unfinished list, make it the current square.
6: goto 2.
7: Target is now the current square.
8: select an adjacent square.
9: if selected square's value is current square -1, set it to the current square.
if it's not, goto 8.
10:store the position of the current square relative to the selected square
(Nort, East, South, West etc.)
if it's the origin, goto 12 after that.
11:goto 8.
12:return list of directions


And this is my implementation:


public void findPath(Vec2D startPos, Vec2D endPos) {
Vec2D start = startPos;
Vec2D end = endPos;

boolean finished = false;
boolean step2 = true, step5 = false, step7 = false;

while (!finished) {
Vec2D c = start;

// Step 2
while (step2) {
for (int i = 0; i < 4; i++) {
Vec2D dta = dirs;
int nx = c.x + dta.x;
int ny = c.y + dta.y;

if (costs[nx][ny] == 0) {
if (costs[nx][ny] != -1) costs[nx][ny] = costs[c.x][c.y] + 1;
}

runs++;

if (runs >= w * h) return;
if (nx == end.x && ny == end.y) {
// Step 7, result found.
step2 = false;
step7 = true;
break;
}

unfinished.push(new Vec2D(nx, ny));
}

if (step2) {
step5 = true;
}

// Step5
if (step5) {
Vec2D us = unfinished.firstElement();
unfinished.remove(0);
c.x = us.x;
c.y = us.y;
}
}

// Step 7 (End case)
c.x = end.x;
c.y = end.y;
if (step7) {

for (int i = 0; i < 4; i++) {
Vec2D dta = dirs;
int nx = c.x + dta.x;
int ny = c.y + dta.y;

if (costs[nx][ny] < costs[c.x][c.y]) {
directions.add(i);
path.add(new Vec2D(nx, ny));
c.x = nx;
c.y = ny;
break;
}
}

if (c.x == start.x && c.y == start.y) finished = true;
}
}
}

Sadly, it keeps running in a circle, can anyone offer some guidance as to what I have done wrong in the implementation of it, and possibly show me a correct implementation?

- Numprt

Advertisement
do you want to create a visual path or just want to find a path? I'm not sure on how you could draw the best path but i do know how to solve a maze with a queue (which is a list, but like a line, where who ever got in line first gets out of the line first, i.e. First-In First-Out). Queues (and stacks) have push (add to the end of queue) and pop (remove the front of the queue) operations

here is psuedocode to seeing a maze is solvable

push the start position onto the queue

while (queue is not empty)
{
pop off the front of the queue, this is the current location
if (current location is the end location) '
done

if (NORTH is not marked)
push to the back of the queue and mark the location in some way
repeat for SOUTH, WEST, EAST
}
Always improve, never quit.

do you want to create a visual path or just want to find a path? I'm not sure on how you could draw the best path but i do know how to solve a maze with a queue (which is a list, but like a line, where who ever got in line first gets out of the line first, i.e. First-In First-Out). Queues (and stacks) have push (add to the end of queue) and pop (remove the front of the queue) operations

here is psuedocode to seeing a maze is solvable

push the start position onto the queue

while (queue is not empty)
{
pop off the front of the queue, this is the current location
if (current location is the end location) '
done

if (NORTH is not marked)
push to the back of the queue and mark the location in some way
repeat for SOUTH, WEST, EAST
}


Thanks, your response just clicked in my head :)

- Numprt

Your algorithm looks like it IS A* to me...

o3o


So, I have been trying to sort out pathfinding for my game, but it isnt working too well. I have tried A* and it didnt work, so I found this algorithm:


Well, it isn't A* that doesn't work, it was your algorithm. What was your problem with A*? I assume you saw this link?

My Gamedev Journal: 2D Game Making, the Easy Way

---(Old Blog, still has good info): 2dGameMaking
-----
"No one ever posts on that message board; it's too crowded." - Yoga Berra (sorta)


[quote name='Matthewj234' timestamp='1341768274' post='4956970']
So, I have been trying to sort out pathfinding for my game, but it isnt working too well. I have tried A* and it didnt work, so I found this algorithm:


Well, it isn't A* that doesn't work, it was your algorithm. What was your problem with A*? I assume you saw this link?
[/quote]

Indeed I have thank you. And yes, it was my implementation that didnt work.

- Numprt

Hello,
My teacher( we're studing programming in the engineering school ) said its really hard to find a way without recursion so I wrote this in C when I was 9th grade; I hope it will help you, its console app:
1. You enter your starting and ending position; X >= 0, X <= 24, Y >=0, Y < 25. Yeah I know its fixed but I was too lazy that day xD
2. FIll in the closed boxes; Places where you can't use to reach the final destination
3. It will print you a 25x25 grid where 5 is your start position, 6 is your destination, 1 is the generated way, 2 are the "locked" boxes, and 0 is the free space where you could walk but u actually didn't

Please don't jungle me for not using const and references ;/

[source lang="cpp"]#include <stdio.h>
#include <math.h>



typedef struct _COORD
{
int x, y;
}COORD;

typedef struct _BOX
{
int info, list;
float g, h;
COORD parent;
}BOX;

COORD bestNode();

void CheckNeighbours(COORD cp);
float magnitute(COORD a, COORD b);
//global vars
BOX grid[25][25]; // main maze
COORD ep; // end postion. ( where u must reach )

void printmaze(int clear)
{
//clear flag -> if 1 then it will clear the grid and set it to 0
//else -> jsut print its current state
int i, z;
for(i = 0; i < 25; i++)
{
for(z = 0; z < 25; z++)
{
if(clear == 1) {
grid[z].info = 0;
}
printf("%i", grid[z].info);
}
printf("\n");
}
printf("\n");
}

int main()
{
COORD cp, sp; int e; COORD ctemp;
e = 0;
printf("Start point. X: / 0-24 / ");
scanf("%i", &sp.y);
printf("Start point. Y: / 0-24 / ");
scanf("%i", &sp.x);
printf("End point. X: / 0-24 / ");
scanf("%i", &ep.y);
printf("End point. Y: / 0-24 / ");
scanf("%i", &ep.x);
printmaze(1);
//add closed
printf("Add a closed block? Yes = 1, No = everything else: ");
scanf("%i", &e);
while(e == 1)
{
printf("Closed Block. X: / 0-24 / ");
scanf("%i", &cp.y);
printf("Closed Block. Y: / 0-24 / ");
scanf("%i", &cp.x);
grid[cp.x][cp.y].list = 2;
grid[cp.x][cp.y].info = 2; // with 2 I've marked the closed postions where u can't go
printf("Add a closed block? Yes = 1, No = everything else: ");
scanf("%i", &e);
}
grid[sp.x][sp.y].info = 5; // start point on the grid
grid[ep.x][ep.y].info = 6; // end point on the grid
printf("\n");
// find best way
cp.x = sp.x;
cp.y = sp.y;
//add to open list;
grid[cp.x][cp.y].list = 1;
grid[cp.x][cp.y].g = 0;
grid[cp.x][cp.y].h = magnitute(cp, ep);
while(!(cp.x == ep.x && cp.y == ep.y)) // while we have not reached the target
{// current postion.x == end postion.x ...

//check best node from open list;
cp = bestNode();
printf("checking %i-%i \n", cp.x, cp.y);
//check out neighbours;
if(cp.x == -25000 || cp.y == -25000) // my grid is fix sized from 0-24 so I use -25000 as a flag
{
printf("There is no way.");
cp.x = ep.x;
cp.y = ep.y;
}else{
CheckNeighbours(cp);
grid[cp.x][cp.y].list = 2; // closed list
}

}

while(!(cp.x == sp.x && cp.y == sp.y))
{
cp = grid[cp.x][cp.y].parent;
grid[cp.x][cp.y].info = 1;
if(cp.x == sp.x && cp.y == sp.y)
grid[cp.x][cp.y].info = 5;
}

printf("\n");
printmaze(2);
printf("End. \n");
scanf("%i", &e);


return 0;
}



void CheckNeighbours(COORD cp)
{
int i, z;
COORD ctemp;
for(i = -1; i < 2; i++)
{

for(z = -1; z < 2; z++)
{
//check if alive;
if(cp.x + i >= 0 && cp.x + i < 25 && cp.y + z >= 0 && cp.y + z < 25)
{
if(!(i == 0 && z == 0))
{
if(grid[cp.x + i][cp.y + z].list != 1 && grid[cp.x + i][cp.y + z].list != 2)
{ //if open list or closed
grid[cp.x + i][cp.y + z].list = 1;
grid[cp.x + i][cp.y + z].parent.x = cp.x;
grid[cp.x + i][cp.y + z].parent.y = cp.y;


//record h value
ctemp.x = cp.x + i;
ctemp.y = cp.y + z;
grid[cp.x + i][cp.y + z].h = magnitute(ctemp, ep);
grid[cp.x + i][cp.y + z].g = magnitute(ctemp, cp) + grid[cp.x][cp.y].g;
printf("new neighbour: %i - %i \n", cp.x + i, cp.y + z);
}else if(grid[cp.x + i][cp.y + z].list == 1)
{
ctemp.x = cp.x + i;
ctemp.y = cp.y + z;
if( grid[cp.x + i][cp.y + z].g > grid[cp.x][cp.y].g + magnitute(cp, ctemp))
{
grid[cp.x + i][cp.y + z].parent = cp;
grid[cp.x + i][cp.y + z].g = magnitute(ctemp, cp) + grid[cp.x][cp.y].g;
}
}
} // end if mid
} // end if alive
} // end for z
}
return;
}

COORD bestNode()
{
int i, z;
int bestf = -25000;
int a = 0;
COORD bestNode;
for(i = 0; i < 25; i++)
{
for(z = 0; z < 25; z++)
{
if(grid[z].list == 1)
{
if(a == 0)
{
a = 1;
bestf = grid[z].g + grid[z].h;
bestNode.x = i;
bestNode.y = z;
}
else
{
if(bestf > grid[z].g + grid[z].h)
{
bestf = grid[z].g + grid[z].h;
bestNode.x = i;
bestNode.y = z;
}
}
}

}
}
if(a == 0)
{
bestNode.x = -25000;
bestNode.y = -25000;
}
return bestNode;
}





float magnitute(COORD a, COORD b)
{
COORD toret; float ab;
if(a.x > b.x)
{
toret.x = a.x - b.x;
}else
{
toret.x = b.x - a.x;
}
if(a.y > b.y)
{
toret.y = a.y - b.y;
}else
{
toret.y = b.y - a.y;
}
ab = toret.x * toret.x + toret.y * toret.y;
ab = sqrt(ab);
return ab;
}[/source]

Hello,
My teacher( we're studing programming in the engineering school ) said its really hard to find a way without recursion so I wrote this in C when I was 9th grade; I hope it will help you, its console app:
1. You enter your starting and ending position; X >= 0, X <= 24, Y >=0, Y < 25. Yeah I know its fixed but I was too lazy that day xD
2. FIll in the closed boxes; Places where you can't use to reach the final destination
3. It will print you a 25x25 grid where 5 is your start position, 6 is your destination, 1 is the generated way, 2 are the "locked" boxes, and 0 is the free space where you could walk but u actually didn't

Please don't jungle me for not using const and references ;/

[source lang="cpp"]#include <stdio.h>
#include <math.h>



typedef struct _COORD
{
int x, y;
}COORD;

typedef struct _BOX
{
int info, list;
float g, h;
COORD parent;
}BOX;

COORD bestNode();

void CheckNeighbours(COORD cp);
float magnitute(COORD a, COORD b);
//global vars
BOX grid[25][25]; // main maze
COORD ep; // end postion. ( where u must reach )

void printmaze(int clear)
{
//clear flag -> if 1 then it will clear the grid and set it to 0
//else -> jsut print its current state
int i, z;
for(i = 0; i < 25; i++)
{
for(z = 0; z < 25; z++)
{
if(clear == 1) {
grid[z].info = 0;
}
printf("%i", grid[z].info);
}
printf("\n");
}
printf("\n");
}

int main()
{
COORD cp, sp; int e; COORD ctemp;
e = 0;
printf("Start point. X: / 0-24 / ");
scanf("%i", &sp.y);
printf("Start point. Y: / 0-24 / ");
scanf("%i", &sp.x);
printf("End point. X: / 0-24 / ");
scanf("%i", &ep.y);
printf("End point. Y: / 0-24 / ");
scanf("%i", &ep.x);
printmaze(1);
//add closed
printf("Add a closed block? Yes = 1, No = everything else: ");
scanf("%i", &e);
while(e == 1)
{
printf("Closed Block. X: / 0-24 / ");
scanf("%i", &cp.y);
printf("Closed Block. Y: / 0-24 / ");
scanf("%i", &cp.x);
grid[cp.x][cp.y].list = 2;
grid[cp.x][cp.y].info = 2; // with 2 I've marked the closed postions where u can't go
printf("Add a closed block? Yes = 1, No = everything else: ");
scanf("%i", &e);
}
grid[sp.x][sp.y].info = 5; // start point on the grid
grid[ep.x][ep.y].info = 6; // end point on the grid
printf("\n");
// find best way
cp.x = sp.x;
cp.y = sp.y;
//add to open list;
grid[cp.x][cp.y].list = 1;
grid[cp.x][cp.y].g = 0;
grid[cp.x][cp.y].h = magnitute(cp, ep);
while(!(cp.x == ep.x && cp.y == ep.y)) // while we have not reached the target
{// current postion.x == end postion.x ...

//check best node from open list;
cp = bestNode();
printf("checking %i-%i \n", cp.x, cp.y);
//check out neighbours;
if(cp.x == -25000 || cp.y == -25000) // my grid is fix sized from 0-24 so I use -25000 as a flag
{
printf("There is no way.");
cp.x = ep.x;
cp.y = ep.y;
}else{
CheckNeighbours(cp);
grid[cp.x][cp.y].list = 2; // closed list
}

}

while(!(cp.x == sp.x && cp.y == sp.y))
{
cp = grid[cp.x][cp.y].parent;
grid[cp.x][cp.y].info = 1;
if(cp.x == sp.x && cp.y == sp.y)
grid[cp.x][cp.y].info = 5;
}

printf("\n");
printmaze(2);
printf("End. \n");
scanf("%i", &e);


return 0;
}



void CheckNeighbours(COORD cp)
{
int i, z;
COORD ctemp;
for(i = -1; i < 2; i++)
{

for(z = -1; z < 2; z++)
{
//check if alive;
if(cp.x + i >= 0 && cp.x + i < 25 && cp.y + z >= 0 && cp.y + z < 25)
{
if(!(i == 0 && z == 0))
{
if(grid[cp.x + i][cp.y + z].list != 1 && grid[cp.x + i][cp.y + z].list != 2)
{ //if open list or closed
grid[cp.x + i][cp.y + z].list = 1;
grid[cp.x + i][cp.y + z].parent.x = cp.x;
grid[cp.x + i][cp.y + z].parent.y = cp.y;


//record h value
ctemp.x = cp.x + i;
ctemp.y = cp.y + z;
grid[cp.x + i][cp.y + z].h = magnitute(ctemp, ep);
grid[cp.x + i][cp.y + z].g = magnitute(ctemp, cp) + grid[cp.x][cp.y].g;
printf("new neighbour: %i - %i \n", cp.x + i, cp.y + z);
}else if(grid[cp.x + i][cp.y + z].list == 1)
{
ctemp.x = cp.x + i;
ctemp.y = cp.y + z;
if( grid[cp.x + i][cp.y + z].g > grid[cp.x][cp.y].g + magnitute(cp, ctemp))
{
grid[cp.x + i][cp.y + z].parent = cp;
grid[cp.x + i][cp.y + z].g = magnitute(ctemp, cp) + grid[cp.x][cp.y].g;
}
}
} // end if mid
} // end if alive
} // end for z
}
return;
}

COORD bestNode()
{
int i, z;
int bestf = -25000;
int a = 0;
COORD bestNode;
for(i = 0; i < 25; i++)
{
for(z = 0; z < 25; z++)
{
if(grid[z].list == 1)
{
if(a == 0)
{
a = 1;
bestf = grid[z].g + grid[z].h;
bestNode.x = i;
bestNode.y = z;
}
else
{
if(bestf > grid[z].g + grid[z].h)
{
bestf = grid[z].g + grid[z].h;
bestNode.x = i;
bestNode.y = z;
}
}
}

}
}
if(a == 0)
{
bestNode.x = -25000;
bestNode.y = -25000;
}
return bestNode;
}





float magnitute(COORD a, COORD b)
{
COORD toret; float ab;
if(a.x > b.x)
{
toret.x = a.x - b.x;
}else
{
toret.x = b.x - a.x;
}
if(a.y > b.y)
{
toret.y = a.y - b.y;
}else
{
toret.y = b.y - a.y;
}
ab = toret.x * toret.x + toret.y * toret.y;
ab = sqrt(ab);
return ab;
}[/source]


Thanks, this was really helpful, showing different ways of achieving the same thing. :)

- Numprt

This topic is closed to new replies.

Advertisement