0
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[i];
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[i];
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?
