# Pathfinding algorithm

This topic is 2347 days old which is more than the 365 day threshold we allow for new replies. Please post a new topic.

## Recommended Posts

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.
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?

##### Share on other sites
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
}

##### Share on other sites

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
}

##### Share on other sites
Your algorithm looks like it IS A* to me...

##### Share on other sites

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?

##### Share on other sites

[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.

##### Share on other sites
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);
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;
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]

##### Share on other sites

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);
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;
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.

1. 1
2. 2
Rutin
18
3. 3
4. 4
5. 5

• 26
• 11
• 9
• 9
• 11
• ### Forum Statistics

• Total Topics
633701
• Total Posts
3013441
×